ROS Follow the line

Gazebo image topics

• Terminal1 (gazebo turtlebot)

roslaunch turtlebot_gazebo turtlebot_world.launch

• Terminal2

rostopic list

• depth camera
• rgb camera
  • raw: raw image
  • compress: compress image
  • theora: compress video

ROS sent image useing sensor_msgs/Image

Follower node

• subscribe to /camera/rgb/image_raw topic receive Image msg

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image

def image_callback(msg):
    pass

def main():
    rospy.init_node("follower")
    rospy.loginfo("start node")
    sub = rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback)
    while not rospy.is_shutdown():
        rospy.spin()

if __name__ == "__main__":
    main()

• Set node permission (chmod +x)
• Run in separated Terminal

rosrun <package_name> follower.py

• Check that the node is running
• Get node info with rosnode info /follower
• Check message receive interval

rostopic hz /camera/rgb/image_raw

subscribed to [/camera/rgb/image_raw]
WARNING: may be using simulated time
average rate: 30.208
 min: 0.010s max: 0.060s std dev: 0.01642s window: 30
average rate: 28.061
 min: 0.010s max: 0.060s std dev: 0.01638s window: 56

Setup follow the line

launch file and projects structure

• launch file
  • worlds
• gazebo resource
  • materials

├── launch
│   └── follow.launch
├── materials
│   ├── course.material
│   └── course.png
├── models
├── scripts
│   └── follower.py
├── src
└── worlds
    └── course.world

<?xml version="1.0"?>
<launch>
    <env name="GAZEBO_RESOURCE_PATH" value="$(find follow_bot)/materials:$(optenv GAZEBO_RESOURCE_PATH)" />
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find follow_bot)/worlds/course.world"/>
    </include>

    <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml">
        <arg name="base" value="kobuki"/>
        <arg name="stacks" value="hexagons"/>
        <arg name="3d_sensor" value="kinect"/>
    </include>
</launch>

Note: launch file set GAZEBO_RESOURCE_PATH environment variable using env tag

View image

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
import cv2
import cv_bridge

class Follower(object):
    def __init__(self):
        self._bridge = cv_bridge.CvBridge()
        
        sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)

    def image_callback(self, msg):
        image = self._bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
        cv2.imshow("window", image)
        cv2.waitKey(3)


def on_shutdown():
    rospy.loginfo("shutdown")
    cv2.destroyAllWindows()

def main():
    rospy.init_node("follower")
    rospy.loginfo("start node")
    rospy.on_shutdown(on_shutdown)
    while not rospy.is_shutdown():
        follower = Follower()
        rospy.spin()

if __name__ == "__main__":
    main()

Note: When we use the cv2.namedWindow Method the Window was freeze check the line solution

Detecting the line

• Find the yellow line

lower_yellow = numpy.array([ 50, 50, 170])
upper_yellow = numpy.array([255, 255, 190])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

• left: orignal
• center: mask imaged (binary)
• right: masked image (extrat only the line from orignal image)

Find line center

• Mask image and take only relevant part (closer path front to the robot)

• Find line center with cv2 moments method
• For debug draw circle on line centroid

M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(image, (cx, cy), 20, (0,0,255), -1)

- Public `Twist` message , calc z angular value
```python
#delta/ error between line center to image center
delta = cx - w/2
twist = Twist()
twist.linear.x = 0.2
twist.angular.z = -float(delta) / 100
self._cmd_vel_pub.publish(twist)

Final demo

Reference

• Programing robots with ROS book
• OpenCV moments
• My source code