setpoint_raw/local not working with "If" condition
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 hop you can help me to solve this issue.
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()