Hello Dear community.
I am using setpoint_raw/local because I wan to control not only position but yaw_rate. Then, everytime I receive a string message “Searching” the drone needs to rotate over the Z axis slow. When I use the “while” loop the drone rotate prefectly but it keeps rotating even there is no message but using the “If” statement the drone does not rotate but I can see that it’s publishing correctly. I hope you guys can help me to solve this issue.
Main goal: When the drone detect an object it will stop rotate and it will follow it but if there is no object then it must rotate until find the object. The python script below is to rotate the drone but as I said it is not working using the IF statement.
My code:
import rospy
import ast
import mavros
import math
from std_msgs.msg import String
from mavros_msgs.msg import PositionTarget
from tf.transformations import quaternion_from_euler
import numpy as np
from time import sleep
mavros.set_namespace()
class data_processing():
def __init__(self):
self.sub = rospy.Subscriber("/obj_recognition/Searching", String, self.rotate_callback)
def rotate_callback(self, msg):
rospy.loginfo("Searching new faces")
two_pi = 2 * math.pi
att_msg = PositionTarget()
if msg.data == "Searching":
att_msg.header.stamp = rospy.Time.now()
att_msg.header.seq = 0
att_msg.header.frame_id = ""
att_msg.type_mask = 1475
att_msg.coordinate_frame = 8
att_msg.position.x = 0.0
att_msg.position.y = 0.0
att_msg.position.z = 1.6
att_msg.velocity.x = 0.0
att_msg.velocity.y = 0.0
att_msg.velocity.z = 0.0
att_msg.acceleration_or_force.x = 0.0
att_msg.acceleration_or_force.y = 0.0
att_msg.acceleration_or_force.z = 0.0
att_msg.yaw = two_pi
att_msg.yaw_rate = 0.4
pub_att.publish(att_msg)
if __name__ == "__main__":
print("Searching node ready")
rospy.init_node('Searching_node', anonymous=True)
pub_att = rospy.Publisher(mavros.get_topic('setpoint_raw', 'local'), PositionTarget, queue_size=100)
rate = rospy.Rate(10) # 10hz
drone_data = data_processing()
rospy.spin()