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:
- AI driver and Follow the Leader Demo scenario
- "Look Me In The Eyes" Scenarios for Jetbot
- "The Floor Is NOT Lava" Scenarios for Jetbot
- Follow The Line
- 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.
- 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
- 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
#######################################################
-
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.
- Create a new launch file inside the
launch
directory. For example, create a launch file namedFTL_demo.launch
:
cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
touch FTL_demo.launch
- 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>
-
Save the launch file and close the editor.
-
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 thetarget_id
parameter to1
for theAI_driver
node within the launch file. This parameter corresponds to the ID for aperson
in thessd_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 cameradetections
-
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:
- 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 cameradetections
-
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.
Class ID | Class Name |
---|---|
1 | person |
2 | bicycle |
3 | car |
4 | motorcycle |
5 | airplane |
6 | bus |
7 | train |
8 | truck |
9 | boat |
10 | traffic light |
11 | fire hydrant |
12 | street sign |
13 | stop sign |
14 | parking meter |
15 | bench |
16 | bird |
17 | cat |
18 | dog |
19 | horse |
20 | sheep |
21 | cow |
22 | elephant |
23 | bear |
24 | zebra |
25 | giraffe |
26 | hat |
27 | backpack |
28 | umbrella |
29 | shoe |
30 | eye glasses |
31 | handbag |
32 | tie |
33 | suitcase |
34 | frisbee |
35 | skis |
36 | snowboard |
37 | sports ball |
38 | kite |
39 | baseball bat |
40 | baseball glove |
41 | skateboard |
42 | surfboard |
43 | tennis racket |
44 | bottle |
45 | plate |
46 | wine glass |
47 | cup |
48 | fork |
49 | knife |
50 | spoon |
51 | bowl |
52 | banana |
53 | apple |
54 | sandwich |
55 | orange |
56 | broccoli |
57 | carrot |
58 | hot dog |
59 | pizza |
60 | donut |
61 | cake |
62 | chair |
63 | couch |
64 | potted plant |
65 | bed |
66 | mirror |
67 | dining table |
68 | window |
69 | desk |
70 | toilet |
71 | door |
72 | tv |
73 | laptop |
74 | mouse |
75 | remote |
76 | keyboard |
77 | cell phone |
78 | microwave |
79 | oven |
80 | toaster |
81 | sink |
82 | refrigerator |
83 | blender |
84 | book |
85 | clock |
86 | vase |
87 | scissors |
88 | teddy bear |
89 | hair drier |
90 | toothbrush |
"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.
- 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
- 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
-
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.
- Create a new launch file inside the
launch
directory. For example, create a launch file namedLMITE_demo.launch
:
cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
touch LMITE_demo.launch
- 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>
-
Save the launch file and close the editor.
-
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 camerapose_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:
- 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 camerapose_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:
-
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.
-
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.
-
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.
- 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
- 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()
-
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.
- Create a new launch file inside the
launch
directory. For example, create a launch file namedTFINL_demo.launch
:
cd ~/MorBot/morbot_ros/morbot_ws/src/scenarios/workshop/launch
touch TFINL_demo.launch
- 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>
-
Save the launch file and close the editor.
-
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 camerasegnet_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:
- 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 camerasegnet_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.
- 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.
- 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.
- 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.
-
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.
-
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.
-
Safety Protocols:
- If the line disappears from view or is obstructed significantly, the robot stops to avoid potential pitfalls.
- Jetbot Camera publishing package
- 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
- 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()
-
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.
- 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
- 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>
-
Save the launch file and close the editor.
-
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 withline 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:
- 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 withline 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
anddetectnet_ros_node.py
scripts are executable.
- 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
- 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()
- 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
-
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>
-
Build the package
cd ~/MorBot/morbot_ros/morbot_ws catkin_make source devel/setup.bash
-
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.