6. MorBot Cool Demos - arieldo/MorBot GitHub Wiki

AI-inspired demos designed to showcase the integration of deep learning models and robotic control to create interactive and intelligent behaviors in small-scale robots. Below are some of the exciting projects we'll develop:

1. AI driver and Follow the Leader Demo scenario
2. "Look Me In The Eyes" Scenarios for Jetbot
3. "The Floor Is NOT Lava" Scenarios for Jetbot
4. Follow The Line
5. Looking For Someting On The Line

Please make sure you completed the ROS Workshop and created the scenario folder.

If Not Follow the instructions in Ros-Workshop .

Let's create an AI driver scenario for Jetbot using ROS, detectNet, and several additional scripts.

• Integration of DetectNet for real-time object detection.
• Autonomous navigation and obstacle avoidance.
• Custom scripts for motion control and path planning.

The basic idea is to use the camera feed from the Jetbot, perform object detection on the images using detectNet, and then move the Jetbot based on the detected objects positions.

AI_driver: This is a new Python script that subscribes to the detection results and calculates the required movement commands for the Jetbot. It then publishes these commands on the cmd_vel topic. Here's a brief explanation of the code:

detections_callback(data): This function is the callback for the object detection results topic. It processes each detection and calculates the required movement commands to make the Jetbot follow the detected object. It publishes these commands on the cmd_vel topic.

size_callback(data): This function is the callback for the camera feed from camera_publisher image size topic. It updates the global image_width and image_height variables each time a new message arrives.

listener(): This is the main function of the script. It initializes a ROS node, defines the publishers and subscribers, and starts the ROS event loop.

• DetectNet ROS Node
• Jetbot Camera publishing package

1. create a New workshop Python file named ai_driver.py:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    touch ai_driver.py

Make the script executable:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    chmod +x ai_driver.py

2. The ai_driver.py

#!/usr/bin/env python

import rospy
from vision_msgs.msg import Detection2DArray
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist

def detections_callback(data):
    global cmd_vel_pub, target_id, image_width, image_height

    # Assume no detection at first
    detection_found = False

    for detection in data.detections:

        # Ensure we only follow the specified target_id
        if detection.results[0].id != target_id:
            continue

        # rospy.loginfo('       Cente_x: %s', detection.bbox.center.x)
        # rospy.loginfo('       size_x: %s', detection.bbox.size_x)        
        # rospy.loginfo('       size_y: %s', detection.bbox.size_y)
        

        twist = Twist()

        # Calculate the error as the distance of the object's center from the image's center
        error = detection.bbox.center.x - image_width / 2

        # Control gain
        Kp = 0.0005  # Lowered for slower movements

        # Calculate the angular speed with a proportional control
        twist.angular.z = -Kp * error

        # Calculate the distance error as the difference of the size of the bounding box from a desired size
        # Use the mean of the width and height of the bounding box
        bbox_size = (detection.bbox.size_x + detection.bbox.size_y) / 2
        desired_size = image_width * 0.4  # let's say we want the object to take up SOME% of the image width 
        size_error = bbox_size - desired_size

        # Proportional control for forward speed
        Kp_distance = 0.0009  # Lowered for slower movements
        twist.linear.x = -Kp_distance * size_error

        # print(twist.linear.x , twist.angular.z)

        cmd_vel_pub.publish(twist)

        # Detection was found
        detection_found = True

    # If no detection was found, stop the robot
    if not detection_found:
        twist = Twist()
        cmd_vel_pub.publish(twist)


def size_callback(data):
    global image_width, image_height

    image_width, image_height = data.data

    # rospy.loginfo('Image size: %s', data.data)

def listener():
    global cmd_vel_pub, image_width, image_height, target_id

    rospy.init_node('detection_listener', anonymous=True)

    image_width = 0
    image_height = 0

    # Fetch the target_id parameter, defaulting to 0 if it's not set
    target_id = rospy.get_param("~target_id", None) # from list ssd_coco_labels.txt

    cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

    rospy.Subscriber('/detections/info', Detection2DArray, detections_callback)
    rospy.Subscriber('/image_size', Int32MultiArray, size_callback)
    rospy.spin()

if __name__ == '__main__':
    listener()


##################### more controls ##################


#  if detection.bbox.center.x < image_width / 2:
#             # Move left
#             twist.angular.z = 0.1
#         elif detection.bbox.center.x > image_width / 2:
#             # Move right
#             twist.angular.z = -0.1

#######################################################

3. Build the package

       cd ~/MorBot/morbot_ros/morbot_ws
       catkin_make
       source devel/setup.bash

• This AI driver scenario involves the coordination of multiple packages, so it is highly recommended to utilize a comprehensive launch file for a streamlined experience. Launch files in ROS allow you to start up multiple nodes at once and configure parameters, making it a valuable tool for complex systems.

The "Follow the Leader" (FTL) demo enables the Jetbot to track and follow a specific object (in this case, a person). To facilitate this demo, we'll create a launch file that incorporates all necessary ROS nodes.

Start by creating a new launch file in the workshop scenario directory. This launch file will include the camera_publisher, web_video_server, detectnet, AI_driver, and jetbot_controller nodes.

1. Create a new launch file inside the launch directory. For example, create a launch file named FTL_demo.launch:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
    touch FTL_demo.launch

3. Open the launch file and write the XML code to configure the nodes and parameters you want to launch:

    <launch>
    <!-- Argument for whether to include the jetbot_controller -->
    <arg name="with_controller" default="false"/>

    <!-- Start the camera_publisher node -->
    <node name="camera_publisher" pkg="camera_publish" type="camera_publisher.py" output="screen">
        <param name="camera_type" value="jetbot" /> <!-- "webcam" or "jetbot" -->
    </node>

    <!-- Start the web_video_server node -->
    <node name="web_video_server" pkg="web_video_server" type="web_video_server" output="screen" />

    <!-- Start the detectnet node -->
    <node name="detectnet" pkg="detectnet" type="detectnet_ros_node.py" output="screen" />
    
    <!-- Start the AI_driver node -->
    <node name="AI_driver" pkg="workshop" type="ai_driver.py" output="screen">
        <!-- Set the desired ID (1 for a person) -->
        <param name="target_id" value="1" />
    </node>

    <!-- Start the jetbot_controller node if with_controller argument is true -->
    <group if="$(arg with_controller)">
        <node name="jetbot_controller" pkg="jetbot_control_node" type="jetbot_controller.py" output="screen" />
    </group>
</launch>

4. Save the launch file and close the editor.

5. Build your package again using the catkin_make command:

    cd ~/MorBot/morbot_ros/morbot_ws
    catkin_make
    source devel/setup.bash
    

• For FTL on person class , You'll need to set the target_id parameter to 1 for the AI_driver node within the launch file. This parameter corresponds to the ID for a person in the ssd_coco_labels list list (see blow).

• It's highly recommended to spend time fine-tuning the PID controller values. Manipulating these values will impact the responsiveness and stability of the robot's movements. By adjusting the proportional (P) gains, you can achieve the optimal balance for your specific scenario.

By default, this launch file does not activate the JetBot controller node. This means the robot won't physically move, but the cmd_vel topic will be published. This feature is particularly useful for fine-tuning and testing the system before physically running the robot.

• For fine-tuning and testing run :

    roslaunch workshop FTL_demo.launch with_controller:=false

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/detections/overlay to view the camera detections

• Check the published images:

    rostopic echo /detections/info

• Check the cmd_vel

    rostopic echo /cmd_vel

After thoroughly testing and ensuring the system is ready for operation, you can enable the JetBot controller node to initiate physical movement. To do this, launch the system with the with_controller argument set to true:

6. Run the launch file using the roslaunch command:

roslaunch workshop FTL_demo.launch with_controller:=true

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/detections/overlay to view the camera detections

• To stop the launch file, press Ctrl+C in the terminal where the launch file is running.

  Caution!!!! JetBot will physically move , make sure it has enough space to move around.

"Look Me In The Eyes" is an interesting scenario where you get your Jetbot to maintain its orientation to face you directly, regardless of where you move. This is achieved by using PoseNet to identify facial keypoints, and adjusting the bot's direction based on these keypoints.

• Use of PoseNet for facial keypoint detection.
• Dynamic orientation adjustment to keep the JetBot facing towards you.
• Real-time interaction and engagement with the JetBot.

• PoseNet ROS Node
• Jetbot Camera publishing package

1. create a New workshop Python file named face_control.py:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    touch face_control.py

Make the script executable:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    chmod +x face_control.py

2. The face_control.py

#!/usr/bin/env python
import rospy
import time
from posenet.msg import Pose2DArray
from geometry_msgs.msg import Twist

class FaceControlJetbot:
    def __init__(self):
        rospy.init_node('face_control_jetbot', anonymous=True)
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.subscriber = rospy.Subscriber('/pose_estimations/info', Pose2DArray, self.callback)

        self.reference_center = {"x": None, "y": None}
        self.center_points_sum = {"x": 0, "y": 0}
        self.sample_count = 0
        self.record_time = rospy.Duration(5)  # Record for 5 seconds
        self.start_time = rospy.Time.now()

        # Initializing the PID controller with arbitrary gains
        # PID Controller parameters
        self.kp = 0.005
        self.ki = 0.0
        self.kd = 0.0
        self.previous_error = 0.0
        self.integral = 0.0
        self.max_output = 0.5
        self.min_output = -0.5

        # Deadzone for movement. If movement is within this range, do not move
        self.deadzone_threshold = 5.0  # You can adjust this value


        print("Node initialized. Starting to record reference points for 5 seconds...")

    def compute_pid(self, error):
        self.integral += error
        derivative = error - self.previous_error
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.previous_error = error

        return min(max(output, self.min_output), self.max_output)
    
    def scale_twist_value(self, movement):
        """ Scale the control value based on the magnitude of the movement """
        scaled_value = (abs(movement) / 100.0) * 0.5  # Scales the value such that maximum is 0.5
        return min(scaled_value, 0.5)  # Cap the scaled value to a maximum of 0.5



    def callback(self, data):
        keypoints = data.keypoints
        keypoint_names = data.keypoint_names
        
        # Extract positions of required keypoints
        current_positions = {}
        for i, name in enumerate(keypoint_names):
            if name in ["nose", "left_eye", "right_eye"]:
                current_positions[name] = keypoints[i]

        # If none of the keypoints are detected, stop the robot.
        if not current_positions:
            print("Required keypoints not detected. Stopping the robot.")
            twist = Twist()  # Default is zero velocities.
            self.cmd_pub.publish(twist)
            return

        if rospy.Time.now() - self.start_time < self.record_time:
            # Still in recording phase
            self.sample_count += 1
            for name, pos in current_positions.items():
                self.center_points_sum["x"] += pos.x
                self.center_points_sum["y"] += pos.y

            print("Recording... Sample count: {}".format(self.sample_count))
        else:
            # Control phase
            if self.reference_center["x"] is None: 
                self.reference_center = {
                    "x": self.center_points_sum["x"] / (self.sample_count * 3),  # 3 keypoints
                    "y": self.center_points_sum["y"] / (self.sample_count * 3)
                }
                print("Reference center set to: {}".format(self.reference_center))
                
            current_center = {
                "x": sum(pos.x for pos in current_positions.values()) / 3,
                "y": sum(pos.y for pos in current_positions.values()) / 3
            }

           
            # movement = current_center["x"] - self.reference_center["x"]
            movement = self.reference_center["x"] - current_center["x"]

            print("Current X: {}, Reference X: {}, Movement: {}".format(current_center["x"], self.reference_center["x"], movement))  # Diagnostic print

            # Check if movement is within the deadzone
            if abs(movement) < self.deadzone_threshold:
                print("Robot is centered. No movement applied.")
                self.cmd_pub.publish(Twist())  # Send zero velocities
                return

            control_value = self.compute_pid(movement)
            print("Control Value after PID: ", control_value)

            twist = Twist()
            # Multiply the PID control value by the scaled value based on movement magnitude
            twist.angular.z = control_value * self.scale_twist_value(movement)

            if control_value < 0:
                print("Detected movement to the left. Sending command to move left.")
            elif control_value > 0:
                print("Detected movement to the right. Sending command to move right.")
                
            self.cmd_pub.publish(twist)

if __name__ == '__main__':
    try:
        time.sleep(11)
        controller = FaceControlJetbot()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

3. Build the package

       cd ~/MorBot/morbot_ros/morbot_ws
       catkin_make
       source devel/setup.bash

• Initialization:

  • Define PID controller parameters to calculate how much the robot should adjust its orientation.
  • Define a deadzone threshold to prevent the robot from moving when it is already centered.
  • Initialize variables to store reference face positions and collect samples for 5 seconds at the start.

• Callback:

  • Extract the keypoints of interest (nose, left eye, right eye) from the PoseNet output.
  • If none of the keypoints are detected, stop the robot.
  • If in the recording phase, gather samples to calculate the reference center point.
  • If in the control phase, calculate the movement required to center the robot, and apply PID control to calculate the angular velocity to send to the robot.
  • Calculate the distance between each sample and the current position of the nose.
  • Command the Jetbot to move using the /cmd_vel topic.

"Look Me In The Eyes" is a fun and interactive way to showcase the power of PoseNet and ROS in robot-human interaction. With this foundation, there are numerous directions for enhancements, including gesture-based controls, distance tracking, and multi-person interactions.

To facilitate this demo, we'll create a launch file that incorporates all necessary ROS nodes.

Start by creating a new launch file in the workshop scenario directory. This launch file will include the camera_publisher, web_video_server, posenet, face_control, and jetbot_controller nodes.

1. Create a new launch file inside the launch directory. For example, create a launch file named LMITE_demo.launch:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
    touch LMITE_demo.launch

3. Open the launch file and write the XML code to configure the nodes and parameters you want to launch:

<launch>
    <!-- Argument for whether to include the jetbot_controller -->
    <arg name="with_controller" default="false"/>

    <!-- Start the camera_publisher node -->
    <node name="camera_publisher" pkg="camera_publish" type="camera_publisher.py" output="screen">
        <param name="camera_type" value="jetbot" /> <!-- "webcam" or "jetbot" -->
    </node>

    <!-- Start the web_video_server node -->
    <node name="web_video_server" pkg="web_video_server" type="web_video_server" output="screen" />

    <!-- Start the posenet node -->
    <node name="posenet_node" pkg="posenet" type="posenet_ros_node.py" output="screen" />
    
    <!-- Start the AI_driver node -->
    <node name="face_control" pkg="workshop" type="face_control.py" output="screen" />
     
    <!-- Start the jetbot_controller node if with_controller argument is true -->
    <group if="$(arg with_controller)">
        <node name="jetbot_controller" pkg="jetbot_control_node" type="jetbot_controller.py" output="screen" />
    </group>
</launch>

4. Save the launch file and close the editor.

5. Build your package again using the catkin_make command:

    cd ~/MorBot/morbot_ros/morbot_ws
    catkin_make
    source devel/setup.bash
    

• It's highly recommended to spend time fine-tuning the PID controller values. Manipulating these values will impact the responsiveness and stability of the robot's movements. By adjusting the proportional (P) gains, you can achieve the optimal balance for your specific scenario.

By default, this launch file does not activate the JetBot controller node. This means the robot won't physically move, but the cmd_vel topic will be published. This feature is particularly useful for fine-tuning and testing the system before physically running the robot.

• For fine-tuning and testing run :

    roslaunch workshop LMITE_demo.launch with_controller:=false

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/pose_estimations/overlay to view the camera pose_estimations

• Check the published images info:

    rostopic echo /pose_estimations/info

• Check the cmd_vel

    rostopic echo /cmd_vel

After thoroughly testing and ensuring the system is ready for operation, you can enable the JetBot controller node to initiate physical movement. To do this, launch the system with the with_controller argument set to true:

6. Run the launch file using the roslaunch command:

roslaunch workshop LMITE_demo.launch with_controller:=true

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/pose_estimations/overlay to view the camera pose_estimations

• To stop the launch file, press Ctrl+C in the terminal where the launch file is running.

  Caution!!!! JetBot will physically move , make sure it has enough space to move around.

The Floor Is NOT Lava" is an intriguing scenario in which a robot is required to navigate an environment by solely detecting and driving on the floor using semantic segmentation. In a twist from the popular children's game "The Floor Is Lava," instead of avoiding the floor, our robot must stay on it!

• Utilization of SegNet for semantic segmentation to distinguish the floor from other elements.
• Navigation and path planning based on safe ground detection.
• Enhancement of robot's environmental understanding and safe navigation capabilities.

Using the power of the SegNet deep learning architecture, the robot distinguishes the floor from other elements in its environment, ensuring it remains on safe ground.

• Objective:

Navigate an environment while ensuring the robot stays solely on the floor.

• Operation:

1. Floor Detection:

   • The robot captures real-time visual data using its onboard camera.
   • SegNet processes this data, classifying pixels into different categories.
   • Pixels identified as "floor" are highlighted or masked, and a centroid of this floor area is calculated.

2. Navigation:

   • The robot aims to keep the floor's centroid at its center of vision, ensuring it remains on the floor.
   • If the centroid deviates from the center (indicating a potential turn in the floor path), the robot adjusts its direction to keep aligned.
   • Speed and steering decisions are made based on the floor's continuity and the distance to potential obstacles.

3. Safety Protocols:

   • If the floor disappears from view or is obstructed significantly, the robot stops to avoid potential pitfalls or "lava zones".
   • Additional sensors can be used to ensure collision avoidance and safety.

• SegNet ROS Node
• Jetbot Camera publishing package

1. create a New workshop Python file named seg_floor_drive.py:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    touch seg_floor_drive.py

Make the script executable:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    chmod +x seg_floor_drive.py

2. The seg_floor_drive.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import PointCloud
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist

# Initialize node and publisher
rospy.init_node('drive_based_on_floor')
cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

def size_callback(data):
    global image_width, image_height

    image_width, image_height = data.data

def point_cloud_callback(msg):
    # Image dimensions
    width = image_width
    height = image_height

    # Middle point (bottom center) of the image
    center_x = width / 2.0
    bottom_y = height

    # Extract the centroid's x and y coords
    centroid_x = msg.points[0].x
    centroid_y = msg.points[0].y
    safety_param = 80 #(in pixseles)

    # Calculate distance from the centroid to the bottom center of the image
    distance = ((centroid_x - center_x) ** 2 + (centroid_y - bottom_y) ** 2) ** 0.5
    print(distance)

    # Safety check: If the distance is below the safety parameter, stop the robot
    if distance < safety_param:
        twist_msg = Twist()
        cmd_vel_pub.publish(twist_msg)
        return  # Exit the callback

    # Determine speed proportional to the distance (you may want to add a scaling factor)
    speed_factor = distance * 0.002  # This gives a value between 0 and 1, assuming diagonal is max distance

    # Limit speed_factor between 0 and 1 (in case of any outliers)
    speed_factor = min(max(speed_factor, 0), 1)

    # Steering: Adjust the angular.z based on the difference between the centroid's x and the center_x
    steering_error = center_x - centroid_x  # Positive if centroid is to the left, negative if to the right
    steering_correction = steering_error / width  # Normalize by width
    steering_angle = 0.9 * steering_correction  # This doubles the effect. Adjust the multiplier as necessary.

    # Publish cmd_vel
    twist_msg = Twist()
    twist_msg.linear.x = speed_factor  # Modify this value with a multiplier if you want higher speeds
    twist_msg.angular.z = steering_angle  # Set the turning direction and magnitude
    cmd_vel_pub.publish(twist_msg)

rospy.Subscriber('/image_size', Int32MultiArray, size_callback)
rospy.Subscriber("/segnet_estimations/positions", PointCloud, point_cloud_callback)
rospy.spin()

3. Build the package

       cd ~/MorBot/morbot_ros/morbot_ws
       catkin_make
       source devel/setup.bash

"The Floor Is NOT Lava" scenario demonstrates the capability of modern robotics combined with deep learning to adapt to game-like scenarios, showing potential for more complex navigational and decision-making tasks in dynamic environments. Whether in a fun setting or real-world applications, the fusion of semantic segmentation with robotics promises exciting possibilities for the future.

Start by creating a new launch file in the workshop scenario directory. This launch file will include the camera_publisher, web_video_server, segnet, seg_floor_drive, and jetbot_controller nodes.

1. Create a new launch file inside the launch directory. For example, create a launch file named TFINL_demo.launch:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
    touch TFINL_demo.launch

3. Open the launch file and write the XML code to configure the nodes and parameters you want to launch:

<launch>
    <!-- Argument for whether to include the jetbot_controller -->
    <arg name="with_controller" default="false"/>

    <!-- Start the camera_publisher node -->
    <node name="camera_publisher" pkg="camera_publish" type="camera_publisher.py" output="screen">
        <param name="camera_type" value="jetbot" /> <!-- "webcam" or "jetbot" -->
    </node>

    <!-- Start the web_video_server node -->
    <node name="web_video_server" pkg="web_video_server" type="web_video_server" output="screen" />

    <!-- Start the segnet node -->
    <node name="segnet_node" pkg="segnet" type="segnet_ros_node.py" output="screen" />
    
    <!-- Start the TFINL node -->
    <node name="seg_drive" pkg="workshop" type="seg_floor_drive.py" output="screen" />
     
    <!-- Start the jetbot_controller node if with_controller argument is true -->
    <group if="$(arg with_controller)">
        <node name="jetbot_controller" pkg="jetbot_control_node" type="jetbot_controller.py" output="screen" />
    </group>
</launch>

4. Save the launch file and close the editor.

5. Build your package again using the catkin_make command:

    cd ~/MorBot/morbot_ros/morbot_ws
    catkin_make
    source devel/setup.bash
    

• It's highly recommended to spend time fine-tuning the PID controller values. Manipulating these values will impact the responsiveness and stability of the robot's movements. By adjusting the proportional (P) gains, you can achieve the optimal balance for your specific scenario.

By default, this launch file does not activate the JetBot controller node. This means the robot won't physically move, but the cmd_vel topic will be published. This feature is particularly useful for fine-tuning and testing the system before physically running the robot.

• For fine-tuning and testing run :

    roslaunch workshop TFINL_demo.launch with_controller:=false

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/segnet_estimations/overlay to view the camera segnet_estimations

• Check the published info:

    rostopic echo /segnet_estimations/positions

• Check the cmd_vel

    rostopic echo /cmd_vel

After thoroughly testing and ensuring the system is ready for operation, you can enable the JetBot controller node to initiate physical movement. To do this, launch the system with the with_controller argument set to true:

6. Run the launch file using the roslaunch command:

roslaunch workshop TFINL_demo.launch with_controller:=true

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/segnet_estimations/overlay to view the camera segnet_estimations

• To stop the launch file, press Ctrl+C in the terminal where the launch file is running.

  Caution!!!! JetBot will physically move , make sure it has enough space to move around.

The SUN RGB-D dataset provides segmentation ground-truth for many indoor objects and scenes commonly found in office spaces and homes.

The dataset contains 10,335 RGB-D images, where each image is annotated with:

Line following is a fundamental scenario in robotics where a robot navigates by following a line on the ground. This is commonly achieved using computer vision techniques to detect and track the line's position. In this demo, we utilize a camera and OpenCV to build a line-following robot.

• Utilization of OpenCV for image processing and line detection.
• Proportional control for navigation based on line position.
• Adjustable parameters for different line colors.
• Real-time overlay of detected lines and contours on the camera feed.

Using the power of OpenCV, the robot detects a line of a specified color in its environment and navigates to follow it.

To improve the detection of the yellow line and reduce the effect of noise and unwanted features in the environment, we can use a few simple image processing techniques: Gaussian blur, erosion, and dilation. These techniques help clean up the image, making it easier to detect the line accurately, Here's how they work:

Gaussian Blur: This smooths the image, reducing noise and small details that might interfere with line detection By averaging pixel values with their neighbors, weighted by a Gaussian function.

How It Works:

Convolution: The image is convolved with a Gaussian kernel, which is a matrix where values are determined by the Gaussian function.

Weighted Average: Each pixel's new value is computed as a weighted average of its neighboring pixels, giving more weight to closer pixels.

• Erosion: This removes small white noise by shrinking the white regions in the image.

• Dilation: This enlarges the white regions, closing small gaps in the detected line.

1. Gaussian Blur:

    # Apply Gaussian blur to the mask to reduce noise
    mask = cv2.GaussianBlur(mask, (5, 5), 0)

• How It Works: This function applies a Gaussian filter to the image, which averages the pixel values within a specified kernel size (in this case, 5x5). The 0 indicates that the standard deviation of the Gaussian kernel is calculated based on the kernel size.

• Effect: The result is a blurred image that has less high-frequency noise, making it easier to detect larger structures like the line while ignoring smaller, irrelevant details.

2. Erosion:

    # Apply erosion to remove small noise
    mask = cv2.erode(mask, None, iterations=2)

• How It Works: Erosion applies a kernel to the image, which shrinks the white regions (foreground) and enlarges the black regions (background). The None parameter indicates the use of the default kernel, and iterations=2 means the erosion operation is applied twice.

• Effect: Small white noises are removed, making the remaining white regions (like the detected line) more distinct and less cluttered.

3. Dilation:

    # Apply dilation to close gaps in the line
    mask = cv2.dilate(mask, None, iterations=2)

• How It Works: Dilation applies a kernel to the image, which enlarges the white regions (foreground) and reduces the black regions (background). The None parameter indicates the use of the default kernel, and iterations=2 means the dilation operation is applied twice.

• Effect: Small gaps within the white regions (like the detected line) are closed, making the line more continuous and easier to detect.

The lower_yellow and upper_yellow arrays define the lower and upper bounds of the yellow color in the HSV color space. These bounds are used to create a mask that isolates the yellow regions in the image, which in this case is the yellow line the robot is supposed to follow.

In the HSV color space:

- H (Hue): Represents the color type. The range is [0, 179] in OpenCV.
- S (Saturation): Represents the vibrancy of the color. The range is [0, 255].
- V (Value): Represents the brightness of the color. The range is [0, 255].

The np.array([20, 100, 100]) means:

- Hue: 20
- Saturation: 100
- Value: 100

These values specify the minimum threshold for the yellow color in the HSV space.

1. Line Detection:

   • The robot captures real-time visual data using its onboard camera.
   • The image is converted to the HSV color space for better color segmentation.
   • A binary mask is created for the selected line color.
   • Noise is reduced using Gaussian blur, erosion, and dilation.
   • Contours are found and the largest contour (assumed to be the line) is identified.

2. Navigation:

   • The centroid of the largest contour is calculated.
   • The error is determined as the distance of the centroid from the center of the image.
   • A proportional control approach adjusts the robot's direction based on the error.
   • Speed and steering decisions are made to follow the line smoothly.

3. Safety Protocols:

   • If the line disappears from view or is obstructed significantly, the robot stops to avoid potential pitfalls.

• Jetbot Camera publishing package

1. create a New workshop Python file named line_follow.py:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    touch line_follow.py

Make the script executable:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    chmod +x line_follow.py

2. The line_follow.py

#!/usr/bin/env python

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from geometry_msgs.msg import Twist

# Global variables
cmd_vel_pub = None
overlay_img_pub = None
bridge = CvBridge()
image_width = 640  # Set a default width
image_height = 480  # Set a default height

# Control gains
Kp = 0.001   # Proportional gain
damping_factor = 1  # Damping factor for low-pass filter

previous_angular_z = 0

# Dictionary to store HSV ranges for different colors
color_ranges = {
    "yellow": {"lower": np.array([10, 100, 100]), "upper": np.array([40, 255, 255])},
    "black": {"lower": np.array([0, 0, 0]), "upper": np.array([180, 255, 50])},
    "red": {"lower": np.array([0, 120, 70]), "upper": np.array([10, 255, 255])},
    "blue": {"lower": np.array([94, 80, 2]), "upper": np.array([126, 255, 255])}
}

# Function to get the selected color range
def get_color_range():
    color = rospy.get_param('~line_color', 'yellow')
    if color in color_ranges:
        return color_ranges[color]
    else:
        rospy.logwarn("Invalid color choice. Defaulting to yellow.")
        return color_ranges["yellow"]

def image_callback(data):
    global cmd_vel_pub, overlay_img_pub, image_width, image_height, previous_angular_z

    # Convert ROS Image message to OpenCV image
    cv_image = bridge.imgmsg_to_cv2(data, "bgr8")

    # Convert the image to HSV color space
    hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)

    # Get the selected color range
    color_range = get_color_range()
    lower_color = color_range["lower"]
    upper_color = color_range["upper"]

    # Create a binary mask where the selected color is white and the rest are black
    mask = cv2.inRange(hsv, lower_color, upper_color)

    # Apply Gaussian blur to the mask to reduce noise
    mask = cv2.GaussianBlur(mask, (5, 5), 0)

    # Apply erosion and dilation to remove small noise and close gaps in the line
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)

    # Find the contours of the binary mask
    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    # Create a copy of the image to draw the overlay
    overlay_img = cv_image.copy()

    # Draw all contours on the overlay image
    cv2.drawContours(overlay_img, contours, -1, (0, 255, 0), 3)

    # If no contours found, stop the robot
    if len(contours) == 0:
        stop_robot()
        publish_overlay_image(overlay_img)
        return

    # Find the largest contour (assuming it's the line)
    largest_contour = max(contours, key=cv2.contourArea)

    # Calculate the moments of the largest contour
    M = cv2.moments(largest_contour)

    # If the contour area is zero, stop the robot
    if M['m00'] == 0:
        stop_robot()
        publish_overlay_image(overlay_img)
        return

    # Calculate the center of the contour
    cx = int(M['m10'] / M['m00'])
    cy = int(M['m01'] / M['m00'])

    # Draw a circle at the center of the contour
    cv2.circle(overlay_img, (cx, cy), 5, (255, 0, 0), -1)

    # Calculate the error as the distance of the line's center from the image's center
    error = cx - image_width / 2

    # Calculate the angular speed with a proportional control
    angular_z = -Kp * error

    # Apply a low-pass filter for damping
    angular_z = damping_factor * angular_z + (1 - damping_factor) * previous_angular_z
    previous_angular_z = angular_z

    # Create the Twist message
    twist = Twist()
    twist.angular.z = angular_z
    twist.linear.x = 0.15  # Set a constant forward speed

    # Publish the command
    cmd_vel_pub.publish(twist)

    # Publish the overlay image
    publish_overlay_image(overlay_img)

def publish_overlay_image(overlay_img):
    overlay_img_msg = bridge.cv2_to_imgmsg(overlay_img, encoding="bgr8")
    overlay_img_pub.publish(overlay_img_msg)

def stop_robot():
    twist = Twist()
    cmd_vel_pub.publish(twist)

def listener():
    global cmd_vel_pub, overlay_img_pub

    rospy.init_node('line_follower', anonymous=True)

    cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    overlay_img_pub = rospy.Publisher('line_follower/overlay_image', Image, queue_size=1)

    rospy.Subscriber("/camera/image_raw", Image, image_callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

3. Build the package

       cd ~/MorBot/morbot_ros/morbot_ws
       catkin_make
       source devel/setup.bash

This node subscribes to the raw image feed from the camera, processes the image to detect a specific color line, and publishes the robot's velocity commands to follow the line.

Line following demonstrates the capability of using computer vision for simple navigation tasks. This can be expanded to more complex scenarios where multiple lines or different patterns are followed.

Start by creating a new launch file in the workshop scenario directory. This launch file will include the camera_publisher, web_video_server, line_follower, and jetbot_controller nodes.

1. Create a new launch file inside the launch directory. For example, create a launch file named line_follow.launch:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
    touch line_follow.launch

3. Open the launch file and write the XML code to configure the nodes and parameters you want to launch:

<launch>
    <!-- Argument for whether to include the jetbot_controller -->
    <arg name="with_controller" default="false"/>

    <!-- Start the camera_publisher node -->
    <node name="camera_publisher" pkg="camera_publish" type="camera_publisher.py" output="screen">
        <param name="camera_type" value="jetbot" /> <!-- "webcam" or "jetbot" -->
    </node>

    <!-- Start the web_video_server node -->
    <node name="web_video_server" pkg="web_video_server" type="web_video_server" output="screen" />

    <!-- Start the line_follower node -->
    <node name="line_follower" pkg="workshop" type="line_follow.py" output="screen" />

    <!-- Start the jetbot_controller node if with_controller argument is true -->
    <group if="$(arg with_controller)">
        <node name="jetbot_controller" pkg="jetbot_control_node" type="jetbot_controller.py" output="screen" />
    </group>
</launch>

4. Save the launch file and close the editor.

5. Build your package again using the catkin_make command:

    cd ~/MorBot/morbot_ros/morbot_ws
    catkin_make
    source devel/setup.bash
    

• It's highly recommended to spend time fine-tuning the PID controller values # Control gains in the python code . Manipulating these values will impact the responsiveness and stability of the robot's movements. By adjusting the proportional (P) gains, you can achieve the optimal balance for your specific scenario.

  Proportional Control (Kp): The robot's angular speed is calculated proportionally to the error.

  Damping Factor (damping_factor): A low-pass filter is applied to the angular speed to smooth out rapid changes, reducing oscillations.

  Low-Pass Filter: The low-pass filter smooths the angular velocity by blending the current value with the previous value. The damping_factor controls the level of smoothing.

By adjusting the Kp and damping_factor, you can further fine-tune the robot's response for smoother line following. If the robot's corrections are still too aggressive, try reducing Kp or increasing the damping_factor slightly.

By default, this launch file does not activate the JetBot controller node. This means the robot won't physically move, but the cmd_vel topic will be published. This feature is particularly useful for fine-tuning and testing the system before physically running the robot.

• For fine-tuning and testing run :

    roslaunch workshop line_follow.launch with_controller:=false

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/line_follower/overlay_image to view the camera feed with line detection overlay.

• Check the cmd_vel

    rostopic echo /cmd_vel

After thoroughly testing and ensuring the system is ready for operation, you can enable the JetBot controller node to initiate physical movement. To do this, launch the system with the with_controller argument set to true:

6. Run the launch file using the roslaunch command:

roslaunch workshop line_follow.launch with_controller:=true

• Open a web browser and navigate to http://<jetbot_ip_address>:8080/stream?topic=/line_follower/overlay_image to view the camera feed with line detection overlay.

• To stop the launch file, press Ctrl+C in the terminal where the launch file is running.

  Caution!!!! JetBot will physically move , make sure it has enough space to move around.

This demo combines line following and object detection. The robot will follow a line and stop to look at a specific object when detected.

• Line following using the line_follower node.
• Object detection using the detectnet node.
• Integration of both functionalities to stop and look at a specific object when detected.

• Complete the line following and object detection setups.
• Ensure the line_follow.py and detectnet_ros_node.py scripts are executable.

1. Create a new Python file named line_follow_with_detection.py:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/scripts
    touch line_follow_with_detection.py
    chmod +x line_follow_with_detection.py

2. Implement the combined logic in line_follow_with_detection.py:

#!/usr/bin/env python

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from vision_msgs.msg import Detection2DArray
from cv_bridge import CvBridge
from geometry_msgs.msg import Twist

# Global variables
cmd_vel_pub = None
overlay_img_pub = None
bridge = CvBridge()
image_width = 640  # Set a default width
image_height = 480  # Set a default height
object_detected = False

# Control gains
Kp = 0.001   # Proportional gain
damping_factor = 1  # Damping factor for low-pass filter
previous_angular_z = 0

def image_callback(data):
    global object_detected

    if object_detected:
        return

    cv2_img = bridge.imgmsg_to_cv2(data, "bgr8")
    hsv_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv_img, np.array([20, 100, 100]), np.array([30, 255, 255]))
    mask = cv2.GaussianBlur(mask, (5, 5), 0)
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)

    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    if contours:
        largest_contour = max(contours, key=cv2.contourArea)
        M = cv2.moments(largest_contour)
        if M["m00"] > 0:
            cx = int(M["m10"] / M["m00"])
            error = cx - image_width / 2
            angular_z = -Kp * error
            angular_z = damping_factor * angular_z + (1 - damping_factor) * previous_angular_z
            previous_angular_z = angular_z

            twist = Twist()
            twist.angular.z = angular_z
            twist.linear.x = 0.15
            cmd_vel_pub.publish(twist)

            overlay_img = cv2.drawContours(cv2_img, [largest_contour], -1, (0, 255, 0), 3)
            publish_overlay_image(overlay_img)

def detections_callback(data):
    global object_detected , target_id

    for detection in data.detections:
        if detection.results[0].id == target_id:
            object_detected = True
            stop_robot()
            rospy.loginfo("Object detected, stopping robot.")
            return

def stop_robot():
    twist = Twist()
    cmd_vel_pub.publish(twist)

def publish_overlay_image(overlay_img):
    overlay_img_msg = bridge.cv2_to_imgmsg(overlay_img, encoding="bgr8")
    overlay_img_pub.publish(overlay_img_msg)

def listener():
    global cmd_vel_pub, overlay_img_pub ,target_id

    # Fetch the target_id parameter, defaulting to 0 if it's not set
    target_id = rospy.get_param("~target_id", None) # from list ssd_coco_labels.txt


    rospy.init_node('line_follow_with_detection', anonymous=True)
    cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    overlay_img_pub = rospy.Publisher('line_follower/overlay_image', Image, queue_size=1)
    rospy.Subscriber("/camera/image_raw", Image, image_callback)
    rospy.Subscriber('/detections/info', Detection2DArray, detections_callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

3. Create a new launch file named line_follow_with_detection.launch:

    cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
    touch line_follow_with_detection.launch

4. Add the following content to the launch file

   <launch>
   <!-- Argument for whether to include the jetbot_controller -->
   <arg name="with_controller" default="false"/>
   
   <!-- Start the camera_publisher node -->
   <node name="camera_publisher" pkg="camera_publish" type="camera_publisher.py" output="screen">
       <param name="camera_type" value="jetbot" /> <!-- "webcam" or "jetbot" -->
   </node>
   
   <!-- Start the web_video_server node -->
   <node name="web_video_server" pkg="web_video_server" type="web_video_server" output="screen" />
   
   <!-- Start the line_follower node -->
   <node name="line_follower" pkg="workshop" type="line_follow.py" output="screen" />
   
   <!-- Start the detectnet node -->
   <node name="detectnet" pkg="detectnet" type="detectnet_ros_node.py" output="screen" />
   
   <!-- Start the line_follow_with_detection node -->
   <node name="line_follow_with_detection" pkg="workshop" type="line_follow_with_detection.py" output="screen">
       <!-- Set the desired ID (1 for a person) -->
           <param name="target_id" value="1" />
   </node>
   
   <!-- Start the jetbot_controller node if with_controller argument is true -->
   <group if="$(arg with_controller)">
       <node name="jetbot_controller" pkg="jetbot_control_node" type="jetbot_controller.py" output="screen" />
   </group>
   
   </launch>
   

5. Build the package

       cd ~/MorBot/morbot_ros/morbot_ws
       catkin_make
       source devel/setup.bash

6. Run the launch file using the roslaunch command:

roslaunch workshop line_follow_with_detection.launch with_controller:=true

• To stop the launch file, press Ctrl+C in the terminal where the launch file is running.

  Caution!!!! JetBot will physically move , make sure it has enough space to move around.