setpoint_raw/local not working with "If" condition

asked 2021-09-18 12:42:50 -0600

subarashi gravatar image

updated 2021-09-21 04:34:27 -0600

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()
edit retag flag offensive close merge delete