Ask Your Question
0

mavros rotation over z axis

asked 2021-09-29 11:10:13 -0600

subarashi gravatar image

updated 2021-09-30 03:45:13 -0600

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!

edit retag flag offensive close merge delete

Comments

It seems you are stuck rospy.spin() as this is a loop that waits for interrupt. Can you try spin once and have an outer while loop

osilva gravatar image osilva  ( 2021-09-29 16:32:59 -0600 )edit
osilva gravatar image osilva  ( 2021-09-29 16:34:04 -0600 )edit

@osilva Thank you for your comment. I read that spin once is for c++ and doest not work for python.

subarashi gravatar image subarashi  ( 2021-09-30 02:48:25 -0600 )edit

I understand but there are ways to do this in Python. Check #q110336. -spin-once-equivalent

osilva gravatar image osilva  ( 2021-09-30 04:12:19 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-09-30 08:05:12 -0600

Mike Scheutzow gravatar image

updated 2021-09-30 08:09:14 -0600

This code needs to be reorganized if you want it to work well.

ros has restrictions on what you can do inside a subscriber callback. A well written app does not do anything inside the callback that takes a long time (looping or heavy cpu processing) and it must never sleep. Creating a subscriber object inside a callback is not a good idea because it may be a little slow and may not see a message sent too soon afterward, but it is technically permitted to do so.

All of your code in callback() should be in a find_face() method that is called from the main loop. You are allowed to loop and sleep in the main loop, and in functions that it calls. Your callback() should simply set a flag that controls whether the main loop calls find_face() or not. It will work better if you create the stop_callback() subscriber just once before you enter the main loop.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-09-29 11:10:13 -0600

Seen: 41 times

Last updated: Sep 30