Dear community. Since I did not get an answer to my last post due to the lack of info I have posted.
The goal I want to achieve is to rotate the drone over the z-axis slow.
Searching_node is subscribing to the topic
/Face_recognition/Searching. When the drone is not detecting a face the
Searching_node must rotate the drone until a face is detected.
The problem with my code is when the drone detects a face it does not stop publishing and it seems that is not running the subscriber I have inside the while loop. It keeps printing the same value of the flag (True and False), it seems it is not going into the subscriber and the drone continues rotating.
- Ubuntu 18.04
- ROS melodic
- PX4 firmware
- Python 3.6
I am sending smaller position targets to the pose.orientation.z to go for 0.0 to 2pi with an increment of 0.1 to rotate the drone. I am using rospy.duration cause I need the drone to rotate slowly.
#!/usr/bin/env python3 ''' @DiegoHerrera ''' import rospy import ast import mavros import math from std_msgs.msg import String from geometry_msgs.msg import Pose 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.yawVal = 0.0 self.face_found = False self.sub = rospy.Subscriber("/Face_recognition/Searching", String, self.callback) def stop_callback(self, msg): self.face_found = True return self.face_found def callback(self, data): two_pi = 2 * math.pi pose = Pose() d = rospy.Duration(0.2) while self.yawVal < two_pi: X, Y, Z = 0, 0, 1.6 rVal, pVal = 0, 0 pose.position.x = X pose.position.y = Y pose.position.z = Z quat = quaternion_from_euler(rVal, pVal, self.yawVal) pose.orientation.x = quat pose.orientation.y = quat pose.orientation.z = quat pose.orientation.w = quat rospy.Subscriber("/Face_recognition_s/face_found_data", String, self.stop_callback) print(self.face_found) if self.face_found: print("Flag is True") self.face_found = False break else: pub.publish(pose) print('yawVal: ', self.yawVal) self.yawVal += 0.1 rospy.sleep(d) else: self.yawVal = 0 if __name__ == "__main__": print("Searching node ready") rospy.init_node('Searching_node', anonymous=True) pub = rospy.Publisher('/Face_recognition/coordinates', Pose, queue_size=10) drone_data = data_processing() rospy.spin()
Searching node ready False ('yawVal: ', 0.0) False ('yawVal: ', 0.1) False ('yawVal: ', 0.2) False ('yawVal: ', 0.30000000000000004) False ('yawVal: ', 0.4) False ('yawVal: ', 0.5) False ('yawVal: ', 0.6) False ('yawVal: ', 0.7) False ('yawVal: ', 0.7999999999999999) True Flag is True False ('yawVal: ', 0.8999999999999999) True Flag is True False ('yawVal: ', 0.9999999999999999) True Flag is True False ('yawVal: ', 1.0999999999999999)
Any help I would appreciate!