Solution
Create a new package in your catkin workspace to contain the integration node. Do it with this command to include the correct dependencies:
cd ~/catkin_ws/ source devel/setup.bash roscore cd src catkin_create_pkg activity1 rospy cv_bridge geometry_msgs image_transport sensor_msgs std_msgs
Switch to the package folder and create a new scripts directory. Then, create the Python file and make it executable:
cd activity1 mkdir scripts cd scripts touch activity.py touch activity_sub.py chmod +x activity.py chmod +x activity_sub.py
This is the implementation of the first node:
Libraries importation:
#!/usr/bin/env python import rospy import cv2 import sys import os from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image from std_msgs.msg import String sys.path.append(os.path.join(os.getcwd(), '/home/alvaro/Escritorio/tfg/darknet/python/')) import darknet as dn
Class definition:
class Activity(): def __init__(self):
Node, subscriber, and network initialization:
rospy.init_node('Activity', anonymous=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("camera/rgb/image_raw", Image, self.imageCallback) self.pub = rospy.Publisher('yolo_topic', String, queue_size=10) self.imageToProcess = None cfgPath = "/home/alvaro/Escritorio/tfg/darknet/cfg/yolov3.cfg" weightsPath = "/home/alvaro/Escritorio/tfg/darknet/yolov3.weights" dataPath = "/home/alvaro/Escritorio/tfg/darknet/cfg/coco2.data" self.net = dn.load_net(cfgPath, weightsPath, 0) self.meta = dn.load_meta(dataPath) self.fileName = 'predict.jpg' self.rate = rospy.Rate(10)Function image callback. It obtains images from the robot camera:
def imageCallback(self, data): self.imageToProcess = self.bridge.imgmsg_to_cv2(data, "bgr8")
Main function of the node:
def run(self): print("The robot is recognizing objects") while not rospy.core.is_shutdown(): if(self.imageToProcess is not None): cv2.imwrite(self.fileName, self.imageToProcess)Method for making predictions on images:
r = dn.detect(self.net, self.meta, self.fileName) objects = "" for obj in r: objects += obj[0] + " "
Publish the predictions:
self.pub.publish(objects) self.rate.sleep()
Program entry:
if __name__ == '__main__': dn.set_gpu(0) node = Activity() try: node.run() except rospy.ROSInterruptException: passThis is the implementation of the second node:
Libraries importation:
#!/usr/bin/env python import rospy from std_msgs.msg import String
Class definition:
class ActivitySub(): yolo_data = "" def __init__(self):
Node initialization and subscriber definition:
rospy.init_node('ThiefDetector', anonymous=True) rospy.Subscriber("yolo_topic", String, self.callback)The callback function for obtaining published data:
def callback(self, data): self.yolo_data = data def run(self): while True:
Start the alarm if a person is detected in the data:
if "person" in str(self.yolo_data): print("ALERT: THIEF DETECTED") breakProgram entry:
if __name__ == '__main__': node = ActivitySub() try: node.run() except rospy.ROSInterruptException: pass
Now, you need to set the destination to the scripts folder:
cd ../../ cd .. cd src/activity1/scripts/
Execute the movement.py file:
touch movement.py chmod +x movement.py cd ~/catkin_ws source devel/setup.bash roslaunch turtlebot_gazebo turtlebot_world.launch
Open a new terminal and execute the command to get the output:
cd ~/catkin_ws source devel/setup.bash rosrun activity1 activity.py cd ~/catkin_ws source devel/setup.bash rosrun activity1 activity_sub.py cd ~/catkin_ws source devel/setup.bash rosrun activity1 movement.py
Run both nodes at the same time. This is an execution example:
Gazebo situation:
Figure 9.16: Example situation for the activity
First node output:
Figure 9.17: First activity node output
Second node output:
Figure 9.18: Second activity node output