Lab3: Mini Pupper Bringup and Motion Control
Introduction
In this lab, you will learn how to bring up the Mini Pupper robot using ROS2 and control its movement using velocity commands. The bringup launch file initializes all the necessary nodes for robot operation, and you’ll use the /cmd_vel topic to send motion commands.
Prerequisites
- Mini Pupper with ROS2 installed
- Completed Lab1 and Lab2
- Mini Pupper hardware connected and powered on
Part 1: Robot Bringup
Launch the Bringup
The bringup launch file starts all necessary nodes for the Mini Pupper to operate, including motor controllers, sensors, and the robot state publisher.
Open a terminal and run:
source ~/ros2_ws/install/setup.bash
# Use setup.zsh if you use zsh instead of bash
ros2 launch mini_pupper_bringup bringup.launch.py hardware_connected:=True
This command:
- Sources the ROS2 workspace
- Launches the bringup with hardware connected
- Initializes motor controllers, IMU, and other sensors
Verify the Bringup
Check that all topics are available:
ros2 topic list
You should see topics like:
/cmd_vel- Velocity commands/odom- Odometry data/joint_states- Joint positions/imu/data- IMU sensor data
Part 2: Moving the Robot
Understanding cmd_vel
The /cmd_vel topic accepts geometry_msgs/msg/Twist messages with:
linear.x: Forward/backward velocity (m/s)linear.y: Left/right velocity (m/s)linear.z: Up/down velocity (not used for ground robots)angular.x: Roll rate (not typically used)angular.y: Pitch rate (not typically used)angular.z: Yaw rate - turning (rad/s)
Move Forward
In a new terminal, send a command to move the robot forward:
source ~/ros2_ws/install/setup.bash
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 1.0, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}"
Stop the Robot
To stop the robot, send zero velocities:
source ~/ros2_ws/install/setup.bash
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}"
Part 3: Motion Commands Reference
Basic Movement Commands
| Motion | Command |
|---|---|
| Move Forward | linear.x: 1.0 |
| Move Backward | linear.x: -1.0 |
| Move Left | linear.y: 1.0 |
| Move Right | linear.y: -1.0 |
| Turn Left | angular.z: 1.0 |
| Turn Right | angular.z: -1.0 |
| Stop | All values: 0.0 |
Example Commands
Move Backward
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: -0.5, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}"
Turn Left
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.5}}"
Turn Right
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: -0.5}}"
Move Forward While Turning
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.5, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.3}}"
Strafe Left (Sideways Movement)
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.0, y: 0.5, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}"
Part 4: Continuous Publishing
Continuous Movement
To keep the robot moving continuously, remove the --once flag and use --rate:
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.3, y: 0.0, z: 0.0}, \
angular: {x: 0.0, y: 0.0, z: 0.0}}" \
--rate 10
Press Ctrl+C to stop publishing.
Using Teleop Keyboard
For interactive control, you can use the teleop keyboard package:
sudo apt install ros-humble-teleop-twist-keyboard
ros2 run teleop_twist_keyboard teleop_twist_keyboard
Use the keyboard to control the robot:
i: Forwardk: Stopj: Turn leftl: Turn right,: Backward
Part 5: Python Script for Motion Control
Create a Python script to control the robot programmatically:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import time
class MotionController(Node):
def __init__(self):
super().__init__('motion_controller')
self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info('Motion Controller Started')
def move(self, linear_x=0.0, linear_y=0.0, angular_z=0.0):
msg = Twist()
msg.linear.x = linear_x
msg.linear.y = linear_y
msg.angular.z = angular_z
self.publisher.publish(msg)
self.get_logger().info(f'Moving: linear=({linear_x}, {linear_y}), angular={angular_z}')
def stop(self):
self.move(0.0, 0.0, 0.0)
self.get_logger().info('Stopped')
def main():
rclpy.init()
controller = MotionController()
try:
# Move forward for 2 seconds
controller.move(linear_x=0.5)
time.sleep(2)
# Turn left for 1 second
controller.move(angular_z=0.5)
time.sleep(1)
# Move forward for 2 seconds
controller.move(linear_x=0.5)
time.sleep(2)
# Stop
controller.stop()
except KeyboardInterrupt:
controller.stop()
finally:
controller.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Save this as motion_control.py and run:
python3 motion_control.py
Architecture Diagram
┌─────────────────────────────────────────────────────────────────┐
│ Mini Pupper │
│ │
│ ┌──────────────────────────────────────────────────────────┐ │
│ │ Bringup Launch │ │
│ │ ┌────────────┐ ┌────────────┐ ┌────────────────────┐ │ │
│ │ │ Motor │ │ IMU │ │ Robot State │ │ │
│ │ │ Controller │ │ Driver │ │ Publisher │ │ │
│ │ └─────▲──────┘ └────────────┘ └────────────────────┘ │ │
│ └────────┼─────────────────────────────────────────────────┘ │
│ │ │
│ ┌────────┴────────┐ │
│ │ /cmd_vel │◄──────────────────────────────────────────┤
│ │ Topic │ Twist Messages │
│ └─────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
▲
│
┌───────────────┴───────────────┐
│ ros2 topic pub │
│ or Python script │
│ or teleop_keyboard │
└───────────────────────────────┘
Exercises
Exercise 1: Square Path
Write a script that makes the robot move in a square pattern:
- Move forward
- Turn 90 degrees
- Repeat 4 times
Exercise 2: Touch-Controlled Movement
Combine Lab1 (touch sensors) with motion control:
- Front touch: Move forward
- Back touch: Move backward
- Left touch: Turn left
- Right touch: Turn right
Exercise 3: Camera-Based Obstacle Avoidance
Use the camera to detect obstacles and avoid them:
- Subscribe to
/image_rawtopic - Use OpenCV to detect objects or measure distance (e.g., color detection, object size)
- If obstacle detected in front, turn away
- Otherwise, move forward
# Hint: Simple color-based obstacle detection
import cv2
import numpy as np
def detect_obstacle(image):
# Convert to HSV for color detection
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# Define range for obstacle color (e.g., red)
lower = np.array([0, 100, 100])
upper = np.array([10, 255, 255])
# Create mask
mask = cv2.inRange(hsv, lower, upper)
# Check if obstacle is in center of image
height, width = mask.shape
center_region = mask[height//3:2*height//3, width//3:2*width//3]
return np.sum(center_region) > 10000 # Threshold
Troubleshooting
| Issue | Solution |
|---|---|
| Robot not moving | Check bringup is running and hardware_connected:=True |
| Motors not responding | Verify motor connections and power |
| cmd_vel not received | Check topic name with ros2 topic list |
| Robot moves erratically | Reduce velocity values |
Debugging Commands
# Check if bringup is running
ros2 node list
# Monitor cmd_vel messages
ros2 topic echo /cmd_vel
# Check message frequency
ros2 topic hz /cmd_vel
# View robot state
ros2 topic echo /joint_states
Summary
In this lab, you learned:
- How to bring up the Mini Pupper with ROS2
- How to send velocity commands using
/cmd_vel - Different motion commands (forward, backward, turn, strafe)
- How to use teleop keyboard for interactive control
- How to create a Python script for programmatic control