ROS2 navigation stack for ergoCub humanoid robots, bridging Nav2 with the YARP-based walking controllers.
Run the following commands on the robot laptop unless explicitly stated otherwise.
Ensure ROS2 is properly set up and all PC clocks are synchronized (ntpdate -b <IP_TO_NTP_SERVER>; on ergoCub robots this is 10.0.2.1).
The ROS2 setup must use cyclone_dds as the DDS middleware. Configure each PC with the cyclonedds.xml file in this repo, with the proper IPs set (guide).
- Install ROS2 following this guide
- Install Nav2 following this guide
- Create your workspace under
$ROBOT_CODE/hsp/ros2_workspace/src - Clone this repo and its dependencies into
/src:git clone https://github.com/hsp-iit/ergocub_navigationhttps://github.com/hsp-iit/bt_nav2_ergocubhttps://github.com/ros-perception/pointcloud_to_laserscan(branchhumble)
- From the
ros2_workspacefolder:colcon build --symlink-install
Normally you do not launch nodes one at a time — bringup.launch.py
starts the whole stack for a chosen profile. The per-node launch files below are
for bringing up or debugging a single component.
Two rules apply to every command in this README:
-
ros2 launch <pkg> <file>matches on the file name only, never on a path inside the package.setup_robot/odom.launch.pydoes not resolve;odom.launch.pydoes. Launch file names are unique across the package, and a test enforces that. -
Every launch file takes
use_sim_time(defaultfalse), and every one that loads a node takesparams_file. So you can point any node at your own YAML without editing the repo:ros2 launch ergocub_navigation odom.launch.py \ params_file:=/abs/path/to/my_odom.yaml use_sim_time:=trueAdd
--show-argsto any launch file to list what it accepts.
Bridges the YARP virtual-unicycle state (from the walking controller) to a ROS2 nav_msgs/Odometry topic. This is the robot's odometry source for Nav2.
Launch: ros2 launch ergocub_navigation odom.launch.py
Params: config/param/odom.yaml
Compensates and filters the 2D lidar scan, removing points that hit the robot's own body based on TF transforms. Also starts the pointcloud_to_laserscan projectors that turn the compensated cloud into the scans the costmaps consume.
Launch: ros2 launch ergocub_navigation scan_filtering.launch.py
Params: config/param/scan.yaml (sim: config/param/simulation/scan.yaml)
Extra arg: projectors:=robot|sim — robot starts four projectors (wide,
front, and two rear wedges); sim starts a single full-circle one. The
configurations live in ergocub_navigation/launch_utils.py as SCAN_PROJECTORS.
Filters the depth camera pointcloud, cropping out the robot's own body to prevent self-collisions in the costmap.
Launch: ros2 launch ergocub_navigation pointcloud_filter.launch.py
Params: config/param/depth_filter.yaml
Converts the Nav2 global plan into walking-controller commands and sends them via YARP to the walking-coordinator. This is the main bridge between Nav2 and the bipedal walking controller.
Launch: ros2 launch ergocub_navigation path_converter.launch.py
Params: config/param/path_converter.yaml
Known issue: that YAML is keyed
path_converter_v2_nodewhile the node is namedpath_converter_node, so none of its values currently reach the node — it runs on the defaults insrc/navigation/walking_planning/path_converter.cpp. Fixing the key would change runtime behaviour, so it has been left alone deliberately.
Exposes a ROS2 action server that the Behavior Tree (bt_nav2_ergocub) calls. Forwards navigation triggers to the walking-controller via YARP.
Run: ros2 run ergocub_navigation planner_trigger_server
Started automatically by bringup.launch.py unless use_planner_trigger:=false.
Controls the robot's head/neck gaze direction along the Nav2 planned path. Subscribes to /plan and sends joint commands to the ergocub-head-controller via YARP RPC, keeping the robot looking ahead toward the next waypoint. Returns the head to a neutral pose when navigation ends.
Launch: ros2 launch ergocub_navigation head_orientation_controller.launch.py
Params: config/param/head_orientation_controller.yaml
Not part of bringup.launch.py; run it alongside.
Generates Nav2 navigation goals based on detected human poses received via YARP. Useful for human-following scenarios.
Launch: ros2 launch ergocub_navigation human_pose_goal_generator.launch.py
Params: config/param/human_pose_goal_generator.yaml
Not part of bringup.launch.py; run it alongside.
Detects ground-plane and performs a camera orientation calibration based on floor RANSAC. Republishes a TF frame called compensated_realsense_frame and re-publishes the pointcloud oriented properly to /adjusted_depth_pc
Launch: ros2 launch ergocub_navigation plane_detector.launch.py
Params: config/param/plane_detector.yaml
Or via bringup.launch.py use_plane_detector:=true.
Subscribes to planned footstep sequences via YARP and publishes them as RViz visualization_msgs/MarkerArray for debugging the walking planning.
Launch: ros2 launch ergocub_navigation footprints_viewer.launch.py
Orientation filter for the head IMU, remapping imu/data_raw → head_imu and imu/data → head_imu/filtered.
Launch: ros2 launch ergocub_navigation imu_filter.launch.py filter:=madgwick
Params: config/param/imu_filter.yaml
Extra arg: filter:=madgwick|complementary.
Known issue: the YAML is keyed
imu_filter, which matches the madgwick node only. The complementary node is namedcomplementary_filter_gain_nodeand runs on its own defaults.
| File | Purpose | Notable args |
|---|---|---|
robot_state_publisher.launch.py |
Publishes the URDF from robotology-superbuild | model (default $YARP_ROBOT_NAME), superbuild_src (default $ROBOTOLOGY_SUPERBUILD_SOURCE_DIR) |
setup_robot.launch.py |
URDF + scan filtering + odometry, optionally the pointcloud filter | projectors, scan_params, odom_params, use_pointcloud_filter |
setup_localization.launch.py |
Map server, optionally AMCL, optionally an identity map→odom TF |
map, amcl_params, use_amcl, static_map_odom_tf |
map_server.launch.py |
Serves one map | map (name under maps/, or an absolute path) |
amcl.launch.py |
AMCL + its lifecycle manager | params_file |
nav2_stack.launch.py |
The nav2 servers | params_file, bt_xml, use_keepout, keepout_mask |
slam_online_async.launch.py |
slam_toolbox in async mapping mode | slam_params_file, autostart |
ergoCub_rviz.launch.py |
RViz | rviz_config |
setup_simulation.launch.py |
gz-sim world + robot spawn + YARP sensor modules | model, superbuild_src, world_sdf, spawn_height |
point_cloud_xyz.launch.py |
depth_image_proc container (not started by bringup) | — |
Turn on the robot and use yarpmanager to start the yarp server and its nodes while the robot is suspended on the crane.
SSH to the robot torso: ssh -X ergocub-torso. In the robot_configuration folder (alias: gotoRobotConfigurationFolder):
yarprobotinterface --config ergocub.xml --enable_tags "(enable_ros2)"yarp rpc /wholeBodyDynamics/rpc
>> calib all 300SSH to the torso, switch to the navigation branch and rebuild:
cd /usr/local/src/robot/robotology-superbuild/src/walking-controllers
git switch nav_integration
goToBuildSuperbuild && make install -j4Launch the walking module:
WalkingModule --from /usr/local/src/robot/robotology-superbuild/src/walking-controllers/src/WalkingModule/app/robots/ergoCubSN001/dcm_walking_iFeel_joint_retargeting_navigation_strict.iniSee also: walking-controllers docs
yarp rpc /walking-coordinator/rpc
>> prepareRobotLower the robot to the ground, then:
>> startWalkingSSH to the robot head: ssh -X ergocub-head. Run yarprobotinterface --config sensors.xml in the head robot_configuration folder.
On the laptop, from $ROBOT_CODE/ros2_ws/src/ergocub_navigation/config/yarp, launch each in a separate terminal:
yarprobotinterface --config depth_compressed_ros2.xml
yarprobotinterface --config head_imu_ros2.xml
yarprobotinterface --config lidar_compressed_ros2.xml# Full stack (setup + AMCL + Nav2 + plane detector + keepout filters)
ros2 launch ergocub_navigation bringup.launch.py
# Path converter (YARP bridge to walking-controller)
ros2 launch ergocub_navigation path_converter.launch.py
# Head gaze controller (optional)
ros2 launch ergocub_navigation head_orientation_controller.launch.py
# Footstep visualizer (optional)
ros2 launch ergocub_navigation footprints_viewer.launch.py
ros2 launch <pkg> <file>matches on the file name only, never on a path inside the package.setup_robot/path_converter.launch.pydoes not resolve;path_converter.launch.pydoes.
Inside the docker container do:
yarpserver --write# Launches all the gz sim simulation with the robot spawned and the sensor modules started
ros2 launch ergocub_navigation setup_simulation.launch.py# Start the walking controller
export YARP_CLOCK=/clock && WalkingModule --from /home/$USERNAME/robotology-superbuild/src/walking-controllers/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_iFeel_joint_retargeting.ini# Full simulation stack (setup + localization + Nav2)
ros2 launch ergocub_navigation bringup.launch.py world:=simMap and params for simulation are in config/param/simulation/ and config/maps/.
bringup.launch.py is the single entry point. It selects a profile rather than
having one launch file per combination:
ros2 launch ergocub_navigation bringup.launch.py world:=sim localization:=slam
ros2 launch ergocub_navigation bringup.launch.py localization:=odom_only rviz:=false
ros2 launch ergocub_navigation bringup.launch.py map:=floor1_ergoCub_2.yamlRun ros2 launch ergocub_navigation bringup.launch.py --show-args for the full
list. The main ones:
| Argument | Values | Default | Effect |
|---|---|---|---|
world |
robot, sim |
robot |
Sets use_sim_time and the per-world map, AMCL params, scan params and scan projector set |
localization |
amcl, slam, odom_only, none |
amcl |
odom_only serves a blank map plus an identity map→odom transform |
map |
name under maps/, or an absolute path |
per world | empty_map.yaml for odom_only/none |
nav2_params |
path | per world + localization | |
rviz |
true, false |
true |
|
use_nav2 |
true, false |
true |
|
use_keepout |
true, false |
per profile | Costmap filter servers for the keepout mask |
keepout_mask |
path | maps/floor0_ergoCub_modded_keepout_full.yaml |
|
use_plane_detector |
true, false |
per profile | |
use_pointcloud_filter |
true, false |
per profile | |
use_planner_trigger |
true, false |
true |
Every argument left unset falls back to the profile default, so overriding one does not force you to restate the others.
Per-profile defaults. The keepout filters, the plane detector and the depth
pointcloud filter are real-robot + static-map features — the keepout mask is a map
artifact, and the other two need the RealSense. They default to on for
robot + amcl and off for every other combination:
| Profile | keepout | plane detector | pointcloud filter |
|---|---|---|---|
robot + amcl (the default) |
on | on | on |
| everything else | off | off | off |
Override any of them explicitly, e.g. to run the robot without the keepout zones:
ros2 launch ergocub_navigation bringup.launch.py use_keepout:=falseThese still exist as thin wrappers, and start exactly what they used to:
| Launch file | Equivalent |
|---|---|
launch_all.launch.py |
bringup.launch.py (the default profile) |
launch_all_slam.launch.py |
bringup.launch.py localization:=slam use_planner_trigger:=false |
launch_all_odom_only.launch.py |
bringup.launch.py localization:=odom_only use_planner_trigger:=false |
launch_sim.launch.py |
bringup.launch.py world:=sim localization:=amcl |
launch_slam_sim.launch.py |
bringup.launch.py world:=sim localization:=slam |
launch_all_odom_only_sim.launch.py |
bringup.launch.py world:=sim localization:=none use_planner_trigger:=false |
The vicon and human_avoidance variants have been removed.
launch/
bringup.launch.py single entry point
setup_robot.launch.py URDF + scan filtering + odometry
setup_localization.launch.py map server, optional AMCL, optional map->odom TF
nav2_stack.launch.py the nav2 servers
setup_robot/ one leaf launch per node
amcl/ slam/ imu/ localization and sensor leaves
simulation/ gz-sim bringup and the sim entry-point wrappers
ergocub_navigation/
launch_utils.py pkg_share(), include(), lifecycle_bringup(),
SCAN_PROJECTORS -- shared by all launch files
ergocub_navigation/launch_utils.py is installed to site-packages via
ament_python_install_package, which is what lets the launch files share code
instead of copy-pasting lifecycle boilerplate.
colcon test --packages-select ergocub_navigation && colcon test-result --verbosetest/test_launch_files.py resolves every launch file's include graph and checks
that the params, maps and behavior trees they name are actually installed. It
needs no robot and no simulator.