Chapter 9: Computer Vision for Robotics

Activity 9: A Robotic Security Guard

Solution

1. 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

2. 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

3. 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

   Note

   The above mentioned path may change as per the directories placed in your computer.

   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)

   Note

   The above mentioned path may change as per the directories placed in your computer.

   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:

   pass

4. This 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")

   break

   Program entry:

   if __name__ == '__main__':

   node = ActivitySub()

   try:

   node.run()

   except rospy.ROSInterruptException:

   pass

5. Now, you need to set the destination to the scripts folder:

   cd ../../

   cd ..

   cd src/activity1/scripts/

6. 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

7. 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

8. 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