Fejemis Maploc Ros2

From Rsewiki

Back to Fejemis ROS2 Software

Purpose

fejemis_maploc is the mapping, localisation, navigation, and behaviour package for Fejemis. It connects the robot's sensor data to the navigation stack and exposes the high-level behaviour tree used to run calibration, mapping, waypoint navigation, and cleaning missions.

The package sits between the low-level bridge and the high-level mission logic:

  • The bridge publishes wheel odometry, IMU data, and accepts velocity / brush commands.
  • fejemis_maploc filters odometry, starts LiDAR and RealSense sensing, runs RTAB-Map, starts Nav2, and runs the Fejemis behaviour tree.
  • The behaviour tree sends commands to Nav2, the coverage planner, the mixer, and the brush service.

Overall fejemis_maploc data flow from bridge, LiDAR, and RealSense into odometry, RTAB-Map, Nav2, coverage planning, behaviour tree, and robot commands.

Package Layout

Important package folders:

  • resources/launch - ROS 2 launch files for sensors, odometry, SLAM/localisation, planning, lifecycle managers, and behaviour trees.
  • resources/config - YAML configuration for odometry, Nav2, RTAB-Map, RealSense, lifecycle managers, and BT plugins.
  • resources/behavior - behaviour tree XML files. main.xml is the older BT.CPP format and main4.xml is the BT.CPP v4 version used on newer ROS distributions.
  • scripts - Python helper nodes for mission triggering, RViz goal relays, brush services, velocity conversion, and coverage tests.
  • src/bt_plugins - C++ behaviour tree plugin used by the cleaning behaviour.

Main Topics and Frames

Common frame names:

  • map - global map frame produced by RTAB-Map / Nav2.
  • odom - local odometry frame.
  • base_link - robot base frame.
  • lidar_link - LiDAR frame.
  • camera_camera_link and RealSense optical frames - camera frames used by the RealSense driver and RTAB-Map.

Important topics and actions:

  • /fejemis/odom - wheel odometry from the bridge.
  • /fejemis/imu and /fejemis/mag - raw IMU and magnetometer inputs.
  • /loc/imu - filtered IMU output from Madgwick.
  • /loc/odom - fused odometry output from the EKF.
  • /fejemis/scan - LiDAR scan.
  • /camera/color/image_raw, /camera/depth/image_rect_raw, and camera info topics - RealSense RGB-D input.
  • /camera_scan - depth-image-to-laserscan output used as an extra obstacle source.
  • /loc/map - occupancy map used by Nav2.
  • /control/nav2/cmd_vel - Nav2 velocity command before Fejemis-specific conversion.
  • /control/joy/cmd_vel - velocity command sent into the existing Fejemis control path.
  • /behavior/set_mission - mission command topic consumed by the behaviour tree.
  • /blackboard/... - blackboard values mirrored into the behaviour tree.
  • /fejemis/navigate_to_pose - Nav2 action used for waypoint goals.
  • /fejemis/navigate_complete_coverage - OpenNav coverage action used for coverage missions.
  • /control/brush/controller/on and /control/brush/controller/off - brush control trigger services.

Launch Files

The package is designed so the top-level fejemis/launch/robot.launch.py can include only the parts needed on each Raspberry Pi. The main Pi typically handles bridge control and odometry, while the auxiliary Pi handles heavier sensing such as LiDAR, RealSense, RTAB-Map, and vision.

launch/lidar.launch.py

Starts the LD19 LiDAR driver from ldlidar_stl_ros2.

Launch arguments:

  • ld_make_container - whether the LiDAR launch should create a component container. Default: True.
  • ld_container - component container name. Default: _laser.
  • ld_container_ns - namespace for the component container. Default: loc.
  • port_name - serial port for the LiDAR. Default: /dev/ttyUSB0.
  • namespace - launch argument declared for compatibility. The included launch currently receives fejemis as namespace.

Important configuration passed to the driver:

  • laser_frame=lidar_link
  • clockwise_dir=false
  • range_min=0.2

The resulting scan is used by RTAB-Map and by the Nav2 costmaps as the main 2D obstacle source.

launch/tf.launch.py

Publishes static transforms for the physical sensor layout.

Published transforms:

  • base_link -> camera_camera_link at approximately x=-0.24 m, y=0.0 m, z=0.44 m.
  • On the real robot, base_link -> lidar_link at approximately x=-0.29 m, y=0.0 m, z=0.25 m, yaw=-1.5708 rad. The yaw correction compensates for the LiDAR's physical mounting angle.
  • In simulation, laser_frame -> lidar_link is published instead, because Gazebo already provides laser_frame.

The launch file uses the in_sim launch configuration to select the real or simulated LiDAR transform.

launch/odometry.launch.py

Starts the odometry estimation path. On the real robot it runs both the Madgwick IMU filter and the robot_localization EKF. In simulation it only runs the EKF with the simulation-specific configuration.

Launch argument:

  • use_sim_time - selects real or simulated odometry configuration. Default: False.

Real robot nodes:

  • imu_filter_madgwick_node named imu_filter in namespace loc.
  • robot_localization/ekf_node named ekf in namespace loc.

Real robot remappings:

  • imu/mag -> /fejemis/mag
  • imu/data_raw -> /fejemis/imu
  • imu/data -> imu, which resolves to /loc/imu.
  • /loc/odom_in -> /fejemis/odom
  • odometry/filtered -> odom, which resolves to /loc/odom.
  • diagnostics are remapped to /loc/ekf/_diagnostics.

The EKF configuration in config/odometry.yaml runs at 30 Hz, uses two-dimensional mode, publishes TF, and fuses:

  • Wheel odometry from /fejemis/odom.
  • RTAB-Map ICP odometry from /loc/rtabmap_odom.
  • Filtered IMU input is present as /loc/imu, although the current real configuration disables the IMU state variables in the EKF and relies more heavily on odometry sources for motion estimation.

Simulation mode uses config/odometry_sim.yaml and avoids the Madgwick IMU orientation because the simulated IMU orientation can conflict with Gazebo odometry frame conventions.

EKF input/output diagram: /fejemis/odom, /loc/rtabmap_odom, /loc/imu into /loc/ekf, output /loc/odom and TF.

launch/realsense.launch.py

Includes the official realsense2_camera launch file with the package's config/realsense.yaml.

Launch arguments:

  • camera_namespace - namespace for the camera. Default: empty string.
  • use_sim_time - declared for consistency, although the included launch is mainly configured through the RealSense launch arguments.

Important RealSense settings:

  • publish_tf=true
  • enable_sync=true
  • unite_imu_method=2
  • config_file=fejemis_maploc/config/realsense.yaml

The camera publishes the RGB, depth, camera info, IMU, and TF data used by RTAB-Map and by the depth-to-scan obstacle conversion.

launch/rtabmap_slam.launch.py

Starts the RTAB-Map mapping/localisation pipeline and includes tf.launch.py.

Launch arguments:

  • use_sim_time - selects real or simulation config. Default: false.
  • localization - if true, RTAB-Map loads an existing database and does not increment the map. If false, it starts in mapping mode and deletes the database on start. Default: false.
  • namespace - namespace for RTAB-Map. Default: loc.
  • map_location - selects the database path. lab maps to ~/.ros/rtabmap.db and asta maps to ~/.ros/rtabmap_asta.db.

Sensor remappings:

  • rgb/image -> /camera/color/image_raw
  • rgb/camera_info -> /camera/color/camera_info
  • depth/image -> /camera/depth/image_rect_raw
  • depth/camera_info -> /camera/depth/camera_info
  • imu -> /loc/imu
  • scan -> /fejemis/scan
  • initialpose -> /initialpose
  • grid_prob_map -> /map

Nodes:

  • depthimage_to_laserscan_node converts the depth image into /camera_scan.
  • rtabmap_sync/rgbd_sync synchronises RGB-D data on the real robot.
  • rtabmap_odom/icp_odometry publishes ICP odometry to /loc/rtabmap_odom on the real robot.
  • rtabmap_slam/rtabmap runs either SLAM mode or localisation mode.

In SLAM mode, delete_db_on_start=True and RTAB-Map is launched with -d, so the selected database is overwritten. In localisation mode, Mem/IncrementalMemory=False and Mem/InitWMWithAllNodes=True, so the existing map is loaded for localisation.

launch/map.launch.py

Starts a component-based Nav2 map server in namespace loc, using config/nav2_servers.yaml.

Launch arguments:

  • use_sim_time - forwarded to the map server.
  • map_make_container, map_container, and map_container_ns - declared as container-related options. The current launch creates a container named _map with prefix/namespace map.

The map server is configured with:

  • topic_name=/loc/map
  • frame_id=map
  • yaml_filename=""

In the current system, the map is primarily produced by RTAB-Map, while the Nav2 map server exposes map-server functionality for the navigation stack.

launch/planning.launch.py

Starts the Nav2 planning and navigation servers, the Fejemis velocity relay, the brush service, and optionally the OpenNav coverage server.

Launch arguments:

  • namespace - namespace for Nav2 nodes. Default: fejemis.
  • use_sim_time - forwarded to nodes. Default: False.
  • enable_coverage - enables opennav_coverage. The default is detected from whether the coverage packages are installed.

Nodes:

  • nav2_controller/controller_server named controller.
  • fejemis_maploc/nav2_cmd_vel_deg_relay.py.
  • nav2_behaviors/behavior_server named behavior.
  • nav2_planner/planner_server named planner_server.
  • nav2_bt_navigator/bt_navigator named bt_navigator.
  • nav2_map_server/map_server.
  • fejemis_maploc/brush_control_service.py in namespace control/brush, named controller.
  • opennav_coverage/opennav_coverage named coverage_server, when enabled.

Important remappings:

  • Nav2 odometry is remapped from /behavior/odom to /loc/odom.
  • Nav2 map input is remapped from /behavior/map to /loc/map.
  • Nav2 controller output cmd_vel is remapped to /control/nav2/cmd_vel.

The velocity relay then converts angular velocity units and republishes to /control/joy/cmd_vel for the existing Fejemis control path.

The Nav2 configuration in config/nav2_servers.yaml uses:

  • Regulated Pure Pursuit as the path follower.
  • Navfn as the global planner.
  • Local costmap sources from LiDAR, camera scan, net scan, and cable scan.
  • Global costmap with static map, LiDAR obstacle layer, and inflation layer.
  • The OpenNav coverage navigator when coverage support is enabled.

launch/lifecycle.launch.py

Starts Nav2 lifecycle managers.

Launch arguments:

  • in_sim - forwarded as use_sim_time.
  • enable_loc_lfc - controls whether the localisation lifecycle manager is started. Default: false.

Lifecycle managers:

  • /fejemis/beh_lfc - behaviour/navigation lifecycle manager.
  • /loc/loc_lfc - localisation lifecycle manager, only when enable_loc_lfc=true.
  • /fejemis/hard_lfc - hardware lifecycle manager, disabled in simulation.

Diagnostics are remapped to /diag/beh_lfc, /diag/loc_lfc, and /diag/hard_lfc.

launch/lifecycle_managers_only.launch.py

Starts the navigation-side launch files without the heavy sensor components. This is useful when another machine is responsible for LiDAR, RealSense, or RTAB-Map.

Launch arguments:

  • in_sim - enables simulation time.
  • localization - when true, starts the planning stack on the main Pi. Default: false.
  • enable_coverage - enables coverage navigation. Default: true.

Included launch files:

  • planning.launch.py is included only when in_sim=true or localization=true.
  • lifecycle.launch.py is included immediately.
  • behavior.launch.py is included after a 30 second delay, giving Nav2 servers time to become active before the BT engine connects to action servers.

launch/behavior.launch.py

Starts the Fejemis behaviour tree engine from raubase_core.

Launch argument:

  • use_sim_time - forwarded to the BT engine.

The launch file selects the behaviour tree XML based on the ROS distribution:

  • Older BehaviourTree.CPP format: resources/behavior/main.xml.
  • BehaviourTree.CPP v4 format: resources/behavior/main4.xml.

The node launched is:

  • raubase_core/bt_engine, configured by config/bt_engine.yaml in namespace fejemis.

The BT plugin configuration loads:

  • raubase logging, mission, blackboard, coverage, and mixer plugins.
  • Nav2 action plugins for single trigger, follow path, drive on heading, spin, and navigate to pose.
  • OpenNav coverage path computation plugin.
  • The package's custom fejemis_call_trigger_service_bt_node plugin.

launch/robot.launch.py inclusion

The top-level fejemis package includes fejemis_maploc from fejemis/launch/robot.launch.py.

Typical distribution:

  • Main Pi: bridge control, odometry, lifecycle managers, behaviour tree, and planning when localisation is enabled.
  • Auxiliary Pi: LiDAR, RTAB-Map, RealSense, and optional vision.

The top-level launch waits 3 seconds after bridge startup before launching the rest of the stack.

Script Nodes

nav2_cmd_vel_deg_relay.py

Purpose: adapts Nav2 velocity commands to the Fejemis control convention.

Subscriptions:

  • /control/nav2/cmd_vel - geometry_msgs/Twist, with angular velocity in rad/s.

Publications:

  • /control/joy/cmd_vel - geometry_msgs/Twist, with angular velocity converted to deg/s.

The linear fields are copied unchanged. The angular x, y, and z fields are multiplied by 180/pi. This is needed because Nav2 uses the normal ROS convention of rad/s, while the existing Fejemis bridge control path expects angular velocity in deg/s.

brush_control_service.py

Purpose: exposes simple ROS services for turning the brush on and off.

Parameters:

  • manage_topic - topic for fejemis_bridge/msg/SteerManage. Default: /fejemis/front/manage.
  • qos_depth - publisher QoS depth. Default: 10.
  • initial_brush_on - initial internal state. Default: False.

Publications:

  • manage_topic - publishes SteerManage.BRUSH_ON or SteerManage.BRUSH_OFF.

Services:

  • ~/set - std_srvs/SetBool, true means brush on and false means brush off.
  • ~/on - std_srvs/Trigger, turns the brush on.
  • ~/off - std_srvs/Trigger, turns the brush off.

In planning.launch.py this node runs as /control/brush/controller, which creates the services used by the behaviour tree:

  • /control/brush/controller/on
  • /control/brush/controller/off

rviz_goal_relay.py

Purpose: lets an operator send a Nav2 goal from RViz.

Subscriptions:

  • /goal_pose - RViz 2D Nav Goal as geometry_msgs/PoseStamped.

Action clients:

  • /fejemis/navigate_to_pose - nav2_msgs/action/NavigateToPose.

When a goal is clicked in RViz, the node checks that the action server is ready, forwards the goal, and logs whether it was accepted and whether it succeeded.

mission_trigger.py

Purpose: starts a cleaning mission by publishing blackboard values and then setting the active mission.

Parameters:

  • mission - mission string to publish. Default: cleaning/waypoint.
  • dock_pose - dock pose as [x, y, yaw]. Default: [2.62, 3.9, 0.0].
  • waypoint_pose - waypoint pose as [x, y, yaw]. Default: [2.10491, 0.497438, 3.11127].
  • clean_poly - polygon string used for cleaning area blackboard values.
  • startup_delay - delay before publishing. Default: 2.0 seconds.

Publications:

  • /blackboard/dock_x, /blackboard/dock_y, /blackboard/dock_yaw
  • /blackboard/waypoint_x, /blackboard/waypoint_y, /blackboard/waypoint_yaw
  • /blackboard/field_polygon
  • /blackboard/clean_poly
  • /blackboard/start_pose
  • /blackboard/dock_pose
  • /blackboard/waypoint_pose
  • /behavior/set_mission

The blackboard publishers use transient-local QoS so late subscribers can still receive the latest values. Pose strings are encoded as: 

stamp;frame;x;y;z;qx;qy;qz;qw

The node also subscribes to /behavior/set_mission. When it sees auto/idle after the mission has run, it logs completion and shuts down.

complete_coverage_planner.py

Purpose: interactive coverage test node for RViz.

Parameters:

  • rviz_click_topic - clicked point topic. Default: /clicked_point.
  • coverage_frame - frame for the coverage polygon. Default: map.
  • min_polygon_points - number of clicked points before sending a goal. Default: 4.
  • auto_close_polygon - whether to append the first point as the final point. Default: True.

Subscriptions:

  • /clicked_point by default - RViz point clicks.

Action clients:

  • fejemis/navigate_complete_coverage - opennav_coverage_msgs/action/NavigateCompleteCoverage.

The node waits for fejemis/bt_navigator to become active, collects clicked points in RViz, converts them to a geometry_msgs/Polygon, and sends a coverage navigation action once enough points have been collected.

fixed_complete_coverage_planner.py

Purpose: fixed-polygon coverage test node.

Parameters:

  • coverage_frame - frame for the coverage polygon. Default: map.
  • min_polygon_points - retained from the interactive tester. Default: 4.
  • auto_close_polygon - whether to append the first point as the final point. Default: True.

Action clients:

  • fejemis/navigate_complete_coverage - opennav_coverage_msgs/action/NavigateCompleteCoverage.

The node waits for fejemis/bt_navigator to become active, then sends a fixed four-point polygon: 

(-5.35,  2.45)
( 2.83,  3.18)
( 3.22, -5.08)
(-4.76, -5.97)

This is useful for testing coverage planning without clicking points manually in RViz.

C++ Behaviour Tree Plugin

fejemis_call_trigger_service_bt_node

The C++ plugin src/bt_plugins/call_trigger_service_bt_node.cpp registers the behaviour tree node CallTriggerService.

Inputs:

  • service_name - service to call.
  • wait_for_service_ms - timeout while waiting for service availability. Default: 1000.
  • call_timeout_ms - timeout for the service call. Default: 2000.

The node creates a std_srvs/Trigger client, waits for the target service, calls it, and returns:

  • SUCCESS when the service exists, responds before timeout, and returns success=true.
  • FAILURE when the service is unavailable, the call times out, or the response reports failure.

The cleaning subtree uses it to call:

  • /control/brush/controller/on
  • /control/brush/controller/off

Behaviour Tree

The active behaviour tree on newer ROS distributions is resources/behavior/main4.xml. It is loaded by behavior.launch.py through the raubase_core/bt_engine node.

Main Loop

The root tree is main: 

main
`-- Sequence
    |-- SingleTrigger
    |   `-- t_init
    `-- KeepRunningUntilFailure
        `-- ReactiveSequence
            |-- ForceSuccess
            |   `-- t_passive
            `-- t_runtime

The structure means:

  • t_init runs once at startup.
  • t_passive runs repeatedly and updates blackboard/mission state.
  • t_runtime selects the active behaviour based on the current mission.

Visualization placeholder: exported Groot image of main4.xml.

t_init

Initialises the robot behaviour state:

  • Sets mission to auto/idle.
  • Sets blackboard key initialized=1.
  • Sets calibration square distance calib_sq_dist=7.
  • Opens the mixer port using OpenMixerPort with priority 4 and topic /behavior.
  • Logs that the robot is initialized.

t_passive

Runs as the passive safety/update loop:

  • Logs Safety loop at debug level.
  • Refreshes start_pose, waypoint_pose, clean_poly, and field_polygon from /blackboard/... topics.
  • Refreshes the current mission from /behavior/set_mission.

t_runtime

Selects behaviour using a reactive fallback: 

t_runtime
|-- t_idle_runtime
|-- t_calib
|-- t_mapping
|-- t_cleaning
`-- unknown mission handler -> set idle

The first matching mission branch runs. If no known branch matches, the tree logs an error and returns to idle.

Idle

t_idle_runtime succeeds when the current mission is:

  • mission: auto
  • mission_sub: idle

This keeps the robot in a waiting state.

Calibration

t_calib chooses between line calibration and square calibration.

Line calibration, t_calib_line:

  • Requires mission calib/line.
  • Drives forward using DriveOnHeading for calib_sq_dist at 0.05 m/s.
  • Returns to idle.

Square calibration, t_calib_square:

  • Requires mission calib/*.
  • Runs f_calib_side four times.
  • Each side drives forward and spins 1.57 rad.
  • Returns to idle.

Mapping

t_mapping currently checks for mission mapping/* and then returns to idle. This branch is a placeholder for future autonomous mapping behaviour.

Cleaning

The current cleaning branch is t_cleaning:

  • Requires mission cleaning/waypoint with mission_sub idle.
  • Navigates to {waypoint_pose} using NavigateToPose.
  • Sets current mission to cleaning/area.
  • Returns to idle.

Supporting cleaning subtrees are already present:

  • f_get_area_to_clean sets a polygon and calls ComputeCoveragePath.
  • f_clean_area turns the brush on, follows {cover_nav_path}, and turns the brush off.
  • f_go_to_path_start is a placeholder that logs navigation to the path start.

These subtrees show the intended full cleaning sequence, although the active t_cleaning branch currently only navigates to a waypoint and changes mission state.

Diagram placeholder: cleaning behaviour sequence: mission trigger -> waypoint navigation -> coverage path computation -> brush on -> follow coverage path -> brush off -> idle.

Configuration Files

config/odometry.yaml and config/odometry_sim.yaml

Configure the EKF and IMU filter. The real configuration fuses wheel odometry and RTAB-Map ICP odometry in 2D mode. The simulation configuration is separated to avoid conflicting simulated IMU orientation.

config/rtabmap.yaml and config/rtabmap_sim.yaml

Configure RTAB-Map for real robot and simulation use. rtabmap_slam.launch.py selects between them using use_sim_time.

config/nav2_servers.yaml and config/nav2_sim.yaml

Configure Nav2 controller, planner, behaviour server, costmaps, BT navigator, coverage server, and map server. The main real-robot launch uses nav2_servers.yaml.

config/bt_engine.yaml

Lists behaviour tree plugins loaded by raubase_core/bt_engine, including the Fejemis custom trigger-service plugin.

config/lifecycle.yaml

Configures lifecycle manager node lists and autostart behaviour for the navigation, localisation, and hardware lifecycle managers.

config/realsense.yaml

RealSense camera configuration loaded by realsense.launch.py.

Typical Usage

Start the complete robot stack through the top-level launch: 

ros2 launch fejemis robot.launch.py localization:=true map_location:=asta

Run on both Raspberry Pis. The launch file decides which parts run on the main and auxiliary computers through the is_aux argument.

Start a waypoint cleaning mission after the system is up: 

ros2 run fejemis_maploc mission_trigger.py

Send an RViz navigation goal: 

ros2 run fejemis_maploc rviz_goal_relay.py

Test coverage navigation with RViz clicked points: 

ros2 run fejemis_maploc complete_coverage_planner.py

Test coverage navigation with the fixed polygon: 

ros2 run fejemis_maploc fixed_complete_coverage_planner.py

Notes and Future Work

  • Clarify which Raspberry Pi should run each launch file in the final deployment.
  • Expand the cleaning behaviour section once t_cleaning is connected to f_get_area_to_clean and f_clean_area in the active tree.
Retrieved from "https://rsewiki.electro.dtu.dk/index.php?title=Fejemis_Maploc_Ros2&oldid=8940"