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.
The node 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[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
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()
Terminal output:
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!