3

I'm writing a python script to run with darknet_ros and bebop_autonomy package in order to take the bounding boxes of the detected objects and depending on the position of the detected objects send different flight commands (takeoff, landing, ecc) by using if elif statement. I'm struggling as I don't know how to take the published data from darknet_ros topic (list with detected objects coordinates) and use it in the python script.
Do you have any suggestion on how to do this?

import rospy
from sensor_msgs.msg import Image
from darknet_ros_msgs.msg import BoundingBoxes
from std_msgs.msg import Header
from std_msgs.msg import String


def callback(data):
    rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data)


def main():
    while not rospy.is_shutdown():
        rospy.init_node('listener', anonymous=True)
        rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes , callback)
        rospy.spin()


if __name__ == '__main__':
    try :
        main()
    except rospy.ROSInterruptException:
        pass
Ilyas
  • 53
  • 6

1 Answers1

0

BoundingBoxes message is a custom created msg by darknet_ros package, so due to this information we will have the like following log to get the X and Y:

def callback(data):
    for box in data.bounding_boxes:
        rospy.loginfo(
            "Xmin: {}, Xmax: {} Ymin: {}, Ymax: {}".format(
                box.xmin, box.xmax, box.ymin, box.ymax
            )
        )
Benyamin Jafari
  • 242
  • 2
  • 12
  • Thank you Benyamin, you're answer has been very useful. In case I have more than 1 detected object at the same time how should I modify the callback function to see the boundingboxes of each object? – Ilyas Apr 14 '19 at 08:32
  • With this code snippet, you can do it. BoundingBoxes msg is a BoundingBoxe array, so with a loop (above for) you can access them (each BoundingBoxe and its X,Y) – Benyamin Jafari Apr 14 '19 at 09:26
  • so I should do another loop before for box in data.bounding_boxes? like for n in data.bounding_boxes with n=number of detected objects – Ilyas Apr 14 '19 at 14:57
  • no, my answer is enough. my answer loop turns to number of detections. – Benyamin Jafari Apr 14 '19 at 16:48
  • solved, you're right. I was able to implement what I wanted to do: thanks for your precious help! – Ilyas Apr 16 '19 at 08:34
  • Your welcome, so if your problem solved you could accept this answer. – Benyamin Jafari Apr 16 '19 at 09:37
  • Hi @Benyamin Jafari, I'm developing a person follower and I'm struggling to send commands to the drone when there is no detection (I want to make the drone turn on its axis until it detects a person). I tried to use states but it doesn't work. Is there another way to implement this without states? Here is the code: https://robotics.stackexchange.com/questions/18792/python-pid-tuning-parrot-bebop-2-target-follower-ros I'm glad if you can help me, thanks – Ilyas May 17 '19 at 01:31