ROS2 (Jazzy) package for the ICCLab Summit XL robot platform with UR5 arm, Robotiq gripper, and perception capabilities. Supports simulation (Gazebo Ignition/Harmonic), real robot operation, autonomous navigation (Nav2), manipulation (MoveIt2/Servo), SLAM (rtabmap), and vision-based segmentation (SAM/LangSAM).
Maintainer: Giovanni Toffetti (toff@zhaw.ch)
- Core Dependencies
- Optional Feature Dependencies
- Launch Files
- Scripts
- LangSAM Segmentation Server Setup
- Configuration Files
These are required for the standard simulation and manipulation stack and are declared in package.xml:
sudo apt install ros-jazzy-navigation2 ros-jazzy-nav2-bringup \
ros-jazzy-moveit ros-jazzy-moveit-servo \
ros-jazzy-ur ros-jazzy-ur-simulation-gz \
ros-jazzy-robotiq-description \
ros-jazzy-depth-image-proc \
ros-jazzy-ros-gz-bridge ros-jazzy-ros-gz-image ros-jazzy-ros-gz-sim \
ros-jazzy-rviz2 ros-jazzy-joint-state-publisher \
ros-jazzy-realsense2-descriptionThese dependencies are not declared in package.xml (to keep the default install lean) but are required for specific features. Install only what you need.
Required by rtabmap.launch.py.
sudo apt install ros-jazzy-rtabmap-rosRequired by summit_xl_nav2.launch.py for dual-LIDAR fusion.
sudo apt install ros-jazzy-ros2-laser-scan-mergerRequired by real/oak.camera.launch.py.
sudo apt install ros-jazzy-depthai-rosOr follow the official installation guide at: https://docs.luxonis.com/software/ros/depthai-ros/
Required by real/astra_mini.launch.py.
sudo apt install ros-jazzy-astra-cameraOr build from source: https://github.com/orbbec/ros_astra_camera
Required by segmentation.launch.py and segmentation_node.py.
Python packages (install in the ROS Python environment or a venv):
pip install open3d
pip install git+https://github.com/facebookresearch/segment-anything.git
# For LangSAM (includes SAM2 + grounding DINO):
pip install lang-segment-anythingNote: For GPU-accelerated local segmentation, see the LangSAM Segmentation Server Setup section, which describes the recommended installation using a dedicated venv.
Full Gazebo Ignition/Harmonic simulation stack. Spawns the robot, bridges ROS2↔Gazebo topics, and starts all controllers.
ros2 launch icclab_summit_xl summit_xl_simulation_ign.launch.pyArguments:
| Argument | Default | Description |
|---|---|---|
robot_id |
summit |
Robot namespace identifier |
robot_xacro |
(icclab xacro) | Path to URDF xacro file |
world |
tugbot_depot |
Gazebo world URL |
headless |
false |
Run without Gazebo GUI |
Notes:
- Controller startup is sequenced with delays (joint_state_broadcaster at 2s, arm_controller at 8s, gripper at ~10s) to avoid race conditions with
gz_ros2_control. - Generates corrected pointclouds via
depth_image_procas a workaround for the Gazebo Harmonic rgbd_camera optical frame bug. - Publishes compressed image topics (15s delay) for MCP server compatibility.
Nav2 autonomous navigation with dual LIDAR scan merging.
Requires: ros2-laser-scan-merger (see Optional Dependencies)
ros2 launch icclab_summit_xl summit_xl_nav2.launch.pyArguments:
| Argument | Default | Description |
|---|---|---|
map |
tugbot_depot.yaml |
Nav2 map YAML file (from maps/) |
params_file |
nav2_params_sim.yaml |
Nav2 configuration file |
rviz |
true |
Launch RViz |
Available maps: empty, icclab, tb3_house, tugbot_depot, willow_garage.
Save a Nav2-generated map to disk.
ros2 launch icclab_summit_xl map_save.launch.pySaves to my_map.yaml / my_map.pgm by subscribing to /summit/map.
MoveIt2 move_group with optional real-time Servo control.
ros2 launch icclab_summit_xl summit_xl_move_it.launch.py
# With servo enabled:
ros2 launch icclab_summit_xl summit_xl_move_it.launch.py use_servo:=trueArguments:
| Argument | Default | Description |
|---|---|---|
use_sim_time |
true |
Use simulation clock |
use_servo |
false |
Enable MoveIt Servo node |
Note: Namespace and TF remappings are intentionally omitted for MoveItPy (Jazzy) compatibility.
Complete MoveIt Servo demo with optional keyboard or autonomous circle control.
ros2 launch icclab_summit_xl servo_demo_full.launch.py start_keyboard_control:=true
ros2 launch icclab_summit_xl servo_demo_full.launch.py start_circle_demo:=true circle_demo_plane:=xzArguments:
| Argument | Default | Description |
|---|---|---|
use_sim_time |
true |
Use simulation clock |
start_rviz |
true |
Launch RViz |
start_keyboard_control |
false |
Enable keyboard teleoperation |
start_circle_demo |
false |
Enable autonomous circle motion |
circle_demo_plane |
xy |
Plane for circle: xy, xz, or yz |
circle_demo_radius |
0.1 |
Circle radius in meters |
Move the arm to a specific Cartesian pose (C++ node).
ros2 launch icclab_summit_xl move_arm_to_pose.launch.py x:=0.7 y:=0.02 z:=1.3Arguments: robot_id, x, y, z, rx, ry, rz, rw (quaternion).
RGB-D SLAM using rtabmap for real-time mapping and localization.
Requires: ros-jazzy-rtabmap-ros (see Optional Dependencies)
ros2 launch icclab_summit_xl rtabmap.launch.py
# Localization-only mode (no map building):
ros2 launch icclab_summit_xl rtabmap.launch.py localization:=trueConfigured for 2D SLAM (Force3DoF). Uses the arm RGBD camera. Deletes the previous database on startup by default.
Run SAM/LangSAM image segmentation locally (requires GPU for reasonable performance).
Requires: open3d, segment-anything or lang-segment-anything Python packages.
ros2 launch icclab_summit_xl segmentation.launch.pyArguments:
| Argument | Default | Description |
|---|---|---|
use_sim_time |
true |
Use simulation clock |
rgb_topic |
/arm_camera/color/image_raw |
Input RGB image |
depth_topic |
/arm_camera/depth/image_raw |
Input depth image |
camera_info_topic |
/arm_camera/color/camera_info |
Camera intrinsics |
Model configuration (device, model_type, etc.) is read from config/seg_params.yaml.
Connect to a remote LangSAM HTTP server for distributed GPU processing. Use this when the GPU is on a separate machine.
Requires: Running LangSAM server (see LangSAM Segmentation Server Setup)
ros2 launch icclab_summit_xl segmentation_remote.launch.py server_url:=http://192.168.1.100:8000Arguments:
| Argument | Default | Description |
|---|---|---|
server_url |
http://localhost:8001 |
LangSAM server address |
server_timeout |
30.0 |
Request timeout in seconds |
sam_type |
sam2.1_hiera_small |
SAM2 model variant |
box_threshold |
0.3 |
Grounding DINO box threshold |
text_threshold |
0.25 |
Grounding DINO text threshold |
voxel_size |
0.002 |
Pointcloud voxel downsampling (meters) |
remove_outliers |
true |
Statistical outlier removal |
Standalone pointcloud generation for the arm camera (workaround for Gazebo Harmonic rgbd_camera optical frame bug). Normally included automatically by summit_xl_simulation_ign.launch.py.
Full bringup for the physical Summit XL robot.
Connect to a physical UR5 via ros2_control.
Key Arguments:
| Argument | Default | Description |
|---|---|---|
ur_type |
ur5 |
UR model: ur3, ur5, ur10, ur5e, etc. |
robot_ip |
192.168.0.210 |
UR controller IP address |
use_fake_hardware |
false |
Use mock hardware (no physical robot) |
headless_mode |
true |
Connect without UR teach pendant |
Wrapper for ur_control.launch.py with the standard ICCLab UR5 configuration (IP: 192.168.0.210, TF prefix: arm_).
Orbbec Astra Mini RGB-D camera with pointcloud generation. Image compression is disabled by default (floods the network).
Requires: ros-jazzy-astra-camera
Luxonis OAK-D PRO camera with configurable RGB-D streams, rectification, and pointcloud generation. Supports RealSense compatibility mode.
Requires: ros-jazzy-depthai-ros
Launch RViz2 with a saved configuration.
ros2 launch icclab_summit_xl rviz.launch.py rviz_config:=robot.rvizOrchestrates a full system regression test: starts simulation → Nav2 → MoveIt → Servo → runs test suite.
ros2 launch icclab_summit_xl regression_test.launch.py headless:=trueArguments: test (all/simulation/navigation/moveit), headless, timeout.
Run individual tests assuming simulation, Nav2, and MoveIt are already running.
All scripts are in the scripts/ directory.
Local SAM/LangSAM segmentation node. Subscribes to synchronized RGB-D images, runs segmentation, and publishes the segmentation mask and a filtered pointcloud.
Topics subscribed: rgb_topic, depth_topic, camera_info_topic (configured via params)
Topics published:
/segmentation_mask— binary mask (sensor_msgs/Image, uint8)/segmented_pointcloud— filtered pointcloud (sensor_msgs/PointCloud2)/segmentation_status— status string
Segmentation triggers:
/segment_text— text prompt (LangSAM only)/segment_point— pixel coordinate prompt (SAM only)/segment_bbox— bounding box prompt (SAM only)
Lightweight ROS2 node that sends RGB images to a remote LangSAM HTTP server and processes the returned masks. Identical output interface to segmentation_node.py. Use when the GPU is on a separate machine.
Real-time Cartesian teleoperation via keyboard. Publishes TwistStamped at 50 Hz to /servo_node/delta_twist_cmds.
Controls: w/x (X±), a/d (Y±), q/e (Z±), i/k (roll±), j/l (pitch±), u/o (yaw±), SPACE (stop).
High-level Python wrapper around MoveItPy for arm and gripper control. Used by moveit_py_example.py.
Demo script showing arm/gripper manipulation using named SRDF configurations (home, up, docked, look_forward, open, closed).
Action client for sending navigation goals to Nav2.
Autonomous circular end-effector motion for Servo testing. Parameters: plane (xy/xz/yz), radius.
Joint-level Servo control (alternative to Cartesian).
Display Servo node status and diagnostics.
Configure and initialize the Servo node.
ROS2 node that bridges perception, simulation physics, and MoveIt2 planning to implement automatic object grasping in Gazebo. It ties together three subsystems:
- Segmentation — consumes the segmented pointcloud produced by
segmentation_node.py/segmentation_node_remote.py. - Gazebo physics — attaches/detaches the target object to the gripper using the Gazebo
DetachableJointplugin (via bridged ROS2 topics). - MoveIt2 planning scene — registers the object's convex hull as a collision object so the motion planner can account for its geometry while carrying it.
Start gripper_attach_node after all of the following are running:
| Prerequisite | Why |
|---|---|
summit_xl_simulation_ign.launch.py |
Provides the Gazebo world, TF tree, and the /gripper/attach–/gripper/detach bridge |
summit_xl_nav2.launch.py (with a map) |
Publishes the map frame into TF — required for calibration. Without it, lookup_transform(map → base_footprint) always fails and the gz→ROS offset is never computed, so Gazebo model matching is permanently disabled |
summit_xl_move_it.launch.py |
Provides move_group and the /planning_scene topic |
Segmentation node (segmentation.launch.py or segmentation_remote.launch.py) |
Provides /segmented_pointcloud |
ros2 run icclab_summit_xl gripper_attach_nodeThe node can also be launched with custom parameters:
ros2 run icclab_summit_xl gripper_attach_node \
--ros-args \
-p attach_distance:=0.08 \
-p gz_world:=tugbot_depot-
Calibration (automatic, one-time): On startup, the node reads Gazebo world poses (via
gz topic) and compares the robot's Gazebo position against its TF position in themapframe. This produces a coordinate offset that lets Gazebo positions be matched to ROS TF positions. Calibration completes as soon as TF becomes available — you will see a log line like:Calibrated gz→ROS offset: [x.xxx, y.yyy, z.zzz] -
Object registration (on each
/segmented_pointcloudmessage): The node transforms the pointcloud into themapframe, computes its convex hull, and publishes it to/planning_sceneas aCollisionObject. It also searches Gazebo world poses to auto-resolve the nearest model name (withinmax_match_distance). -
Proximity monitoring (10 Hz timer): Computes the midpoint between the two inner finger pads (via TF) and measures its distance to the object centroid. Log output (throttled to 1 Hz):
finger_midpoint→object dist: 0.073m (target=cardboard_box, attached=False) -
Attach (triggered when
dist < attach_distanceand a model name is resolved):- Publishes the model name to
/gripper/attach→ GazeboDetachableJointwelds the object to the gripper. - Publishes an
AttachedCollisionObjectto/planning_scene→ MoveIt2 knows to carry the object's shape with the arm.
- Publishes the model name to
-
Detach (triggered by
/gripper/force_detach):- Publishes an
Emptyto/gripper/detach→ Gazebo releases the weld. - Removes the collision object from the MoveIt2 planning scene.
- Publishes an
Note: The node does not auto-detach based on distance once grasped. The stored centroid is the pre-grasp position, so distance naturally grows as the arm lifts. Always use the
/gripper/force_detachtopic to release the object.
| Topic | Type | Direction | Purpose |
|---|---|---|---|
/segmented_pointcloud |
sensor_msgs/PointCloud2 |
Subscribed | Segmented object points (camera frame) |
/gripper/attach |
std_msgs/String |
Published | Gz model name to attach (DetachableJoint) |
/gripper/detach |
std_msgs/Empty |
Published | Release Gz DetachableJoint |
/gripper/force_detach |
std_msgs/Empty |
Subscribed | External trigger to release grasp |
/planning_scene |
moveit_msgs/PlanningScene |
Published | Add/attach/remove collision objects |
| Parameter | Type | Default | Description |
|---|---|---|---|
finger_link_left |
str |
left_inner_finger_pad |
Left finger TF frame for proximity |
finger_link_right |
str |
right_inner_finger_pad |
Right finger TF frame for proximity |
attach_link |
str |
robotiq_140_base_link |
MoveIt link the object attaches to |
planning_frame |
str |
map |
TF root frame for MoveIt planning |
robot_gz_model |
str |
summit |
Robot's Gazebo model name (for calibration) |
robot_tf_frame |
str |
base_footprint |
Robot's ROS TF frame (for calibration) |
attach_distance |
float |
0.10 |
Grasp trigger distance in metres |
max_match_distance |
float |
0.30 |
Max radius to search for Gz model match |
exclude_prefixes |
str |
summit,ground_plane,sun,aws_robomaker |
Comma-separated Gz model name prefixes to ignore during model matching |
gz_world |
str |
world_demo |
Gazebo world name (used to subscribe to pose topic) |
A typical pick-and-place sequence using gripper_attach_node:
# 1. Segment the target object
ros2 topic pub --once /segment_text std_msgs/msg/String "data: 'red box'"
# → segmentation_node publishes /segmented_pointcloud
# → gripper_attach_node registers it in the planning scene
# 2. Move the arm toward the object with MoveIt (plan + execute)
# gripper_attach_node auto-attaches when the finger midpoint is within attach_distance
# 3. Lift the arm / execute place motion
# 4. Release the object
ros2 topic pub --once /gripper/force_detach std_msgs/msg/Empty "{}"From Python (e.g., inside a MoveItPy script):
from std_msgs.msg import Empty
detach_pub = node.create_publisher(Empty, '/gripper/force_detach', 1)
detach_pub.publish(Empty())| Symptom | Likely cause | Fix |
|---|---|---|
Calibrated gz→ROS offset never appears |
TF not publishing or gz binary not on PATH |
Ensure simulation is running and gz CLI is available |
| Object not attached after gripper closes | attach_distance too small, or model not resolved |
Check log for distance readout; increase attach_distance or reduce max_match_distance |
| Wrong object attached | Another Gazebo model is closer to the centroid | Set exclude_prefixes to filter it out, or reduce max_match_distance |
| MoveIt plans through the grasped object | Attach did not complete before planning | Wait for the Attached "..." log line before sending the next MoveIt goal |
The segmentation nodes use lang-segment-anything, which combines SAM2 and Grounding DINO for language-guided segmentation. The server runs in a dedicated Python venv at ~/rap/lang-segment-anything/ for GPU isolation from ROS.
This package ships patches and a custom server in langsam/:
langsam/
├── install_langsam.sh # automated install + patch script
├── server_ros.py # ROS-specific LitServe server (JSON endpoint, port 8001)
└── patches/
├── lang_sam.py # adds gdino_model_id parameter to LangSAM.__init__
└── gdino.py # propagates model_id through build_model; default: grounding-dino-tiny
Two changes are needed beyond the upstream release:
-
gdino.pydefault model (grounding-dino-tiny): the upstream default wasgrounding-dino-base, which fails to load on Ubuntu 24.04 / ROS Jazzy due to atransformersversion conflict. The fix is to use-tinyas the default. -
lang_sam.py+gdino.py—gdino_model_idparameter: the ROS server (server_ros.py) usesgrounding-dino-basefor better accuracy but needs to pass the model ID through the constructor. This patch adds agdino_model_idargument toLangSAM.__init__andGDINO.build_model. -
server_ros.py— a ROS-specific server that:- Runs on port 8001 (avoids conflict with the original
server.pyon 8000) - Adds an
output_format=jsonresponse mode that returns base64-encoded masks + bounding boxes as JSON (required bysegmentation_node_remote.pyfor pointcloud generation) - Uses
grounding-dino-basefor better detection accuracy
- Runs on port 8001 (avoids conflict with the original
Prerequisites: CUDA-capable GPU, CUDA toolkit, Python 3.12.
cd ~/colcon_ws/src/icclab_summit_xl/langsam
bash install_langsam.shThe script will:
- Clone the repo to
~/rap/lang-segment-anything(skips if already present) - Install
uvif not found - Create a Python 3.12 venv with
--system-site-packages - Install PyTorch with auto-detected CUDA version
- Install lang-segment-anything with
uv pip install -e . - Apply the two source patches
- Copy
server_ros.pyinto the repo
If you prefer to install step by step:
# 1. Install uv
curl -LsSf https://astral.sh/uv/install.sh | sh
# 2. Clone
mkdir -p ~/rap && cd ~/rap
git clone https://github.com/luca-medeiros/lang-segment-anything.git
cd lang-segment-anything
# 3. Create venv (system-site-packages lets the venv see ROS Python packages)
uv venv --python 3.12 --system-site-packages .venv
source .venv/bin/activate
# 4. Install PyTorch with CUDA (replace cu128 with your CUDA version, e.g. cu118, cu121)
uv pip install torch torchvision --index-url https://download.pytorch.org/whl/cu128
# 5. Install lang-segment-anything
uv pip install -e .
# 6. Apply patches
ICCLAB=~/colcon_ws/src/icclab_summit_xl/langsam
cp $ICCLAB/patches/lang_sam.py lang_sam/lang_sam.py
cp $ICCLAB/patches/gdino.py lang_sam/models/gdino.py
cp $ICCLAB/server_ros.py lang_sam/server_ros.py
# 7. Verify GPU
python -c "import torch; print('CUDA:', torch.cuda.is_available(), torch.cuda.get_device_name(0))"cd ~/rap/lang-segment-anything
source .venv/bin/activate
# Start the ROS-compatible server on port 8001
python3 -m lang_sam.server_rosOn first run the server downloads SAM2 weights (~160 MB for sam2.1_hiera_small) and Grounding DINO Base (~680 MB). Both are cached in ~/.cache/huggingface/.
The original
server.py/app.py(port 8000, image-only responses) still works but is not compatible withsegmentation_node_remote.py, which requires the JSON endpoint provided byserver_ros.py.
# Server on the same machine (default)
ros2 launch icclab_summit_xl segmentation_remote.launch.py
# Server on a different machine
ros2 launch icclab_summit_xl segmentation_remote.launch.py server_url:=http://192.168.1.100:8001ros2 topic pub --once /segment_text std_msgs/msg/String "data: 'red box'"The node captures the next synchronized RGB-D frame, sends it to the server, and publishes results on /segmented_pointcloud and /segmentation_mask.
| File | Purpose |
|---|---|
seg_params.yaml |
Segmentation node parameters (model_type, device, topics) |
nav2_params_sim.yaml |
Nav2 parameters for simulation |
nav2_params_real.yaml |
Nav2 parameters for real robot |
ur_controllers.yaml |
UR5 joint controllers (simulation) |
ur_controllers_real.yaml |
UR5 joint controllers (real robot) |
joint_limits.yaml |
UR5 joint velocity/acceleration limits |
joint_limits_simulation.yaml |
Relaxed limits for simulation |
lidar_front.yaml / lidar_rear.yaml |
LIDAR topic and frame configuration |
ign_gazebo_bridge.yaml |
ROS2↔Gazebo topic bridge config |
ign_gazebo_bridge_depth_image.yaml |
Bridge config without pointcloud (Gazebo bug workaround) |
astra_mini_params.yaml |
Astra Mini camera parameters |
ur5_calibrated_kinematics.yaml |
Calibrated UR5 kinematics for real robot |
gripper_controller.yaml |
Robotiq gripper controller config |