Rsewiki - User contributions [en]

Fejemis 2026

2026-05-28T09:17:40Z

S253809: /* Mission Statement */

== Introduction ==

Fejemis is an autonomous cleaning robot developed to clean the Asta building. The building is divided into different cleaning areas, where the robot navigates autonomously to a selected point, plans a cleaning path, and performs the cleaning task using its brushing system while following the generated route.

[[File:Fejemis.jpeg | 600px]]

== Start up guide ==

To turn on Fejemis, you need to press the turn on button for more than 2 seconds. If not, both Raspberry Pis wont boot. To turn it off you need to press it for more than 7 seconds.

To work with Fejemis, you can connect directly using a monitor and keyboard. The monitor can be connected to one of the Raspberry Pis, while the keyboard connects through USB. After booting, log into the Ubuntu system using the provided credentials.

Fejemis uses two Raspberry Pis:

Main Raspberry Pi and Auxiliary Raspberry Pi

The Raspberry Pis can be also accessed through SSH.(The IP some time changes, check it by connecting directly with the monitor)
Main lab
local@10.197.213.227
Aux lab
local@10.197.213.226
Main asta
local@10.197.219.238
Aux lab
local@10.197.219.237

The last way of connecting is through Ethernet.
Main
197.168.1.1
Aux
197.168.1.2

If you are connected to the main Raspberry Pi,the auxiliary Raspberry Pi can also be accessed using:

ssh fejemis-aux

=== Launch===

Launching the bridge (from anywhere):
ros2 launch fejemis_bridge bridge.launch.py

Launching the mixer + gamepad control (from anywhere):
ros2 launch fejemis_bridge mixer.launch.py

You need to press the "BACK" button on the gamepad to actually start the control from the gamepad.

You can launch both at once with the following command: ros2 launch fejemis_bridge control.launch.py

This launch file runs the latest version of the autonomous cleaning software:

ros2 launch fejemis robot.launch.py

It must be launched on both Raspberry Pis, since each device handles different parts of the system.

Arguments:
localization:=true/false, map_location:=asta/lab

Default: localization true, map loc asta

If the argument "localization:=false" is provided, the robot will automatically start a new mapping process and overwrite the current map.

After ''fejemis robot.launch.py'' is running on both Raspberry Pis, you can optionally start a few helper nodes depending on what you want to test:
* <code>ros2 run fejemis_maploc rviz_goal_relay.py</code> lets you send a navigation goal from RViz if you want to drive the robot to a specific point.
* If the robot does not localize itself automatically, you can set the initial pose in RViz.
* <code>ros2 run fejemis_maploc mission_trigger.py</code> starts the cleaning behaviour once the robot is ready.
* <code>ros2 run fejemis_maploc fixed_complete_coverage_planner.py</code> or <code>ros2 run fejemis_maploc complete_coverage_planner.py</code> can be used to test only the cleaning path generation and coverage following.

If Fejemis crash (one of the battery display will turn off), you need to plug it in before turning it on again.

=== Troubleshooting / usual problems ===

Common issues seen during daily operation:

*'''Main Raspberry Pi crashes while building the full workspace'''
The main Raspberry Pi may crash or run out of resources when building everything at once.
Use symlink install and only build the packages you changed:

colcon build --symlink-install --packages-select <changed_package_1> <changed_package_2>

*'''Robot shuts down while driving (high speed / acceleration)'''
If commanded speed or acceleration is too aggressive, the battery management system can cut power and both Raspberry Pis may shut down.
Reduce drive aggressiveness and avoid sudden acceleration spikes.

*'''System behaves inconsistently between main and aux Pi'''
This is often caused by the two Raspberry Pis having unsynchronised clocks.
Run this from the main Raspberry Pi:

fejemis sync_clocks

*'''Only one Raspberry Pi appears to boot'''
You must hold the power button for a few seconds so both Raspberry Pis start correctly.

*'''Left battery does not engage after BMS cutoff'''
If the left battery turns off due to battery management protection, plug in the robot before booting again, otherwise that battery may not re-engage.

*'''Unstable wireless connection in ASTA'''
Wi-Fi in ASTA is often unreliable. Prefer connecting your PC over Ethernet for stable control and monitoring.

=== Other utilities ===

If you want to generate a square signal, you can use the node I made:

ros2 run arcana_tools signal_generator --ros-args -p frequency:=<signal_freq> -p out_type:=<output ROS2 type> -r /sig_out:=<output topic> -p bias:=<signal bias> -p range:=<vmax-vmin>

Typically you wight want to send it as if it was a gamepad input:

ros2 run arcana_tools signal_generator--ros-args -p frequency:=0.5 -p out_type:=geometry_msgs.msg.Twist.linear.x -r /sig_out:=/joy_control/cmd_vel -p range:=0.5

You can display graphs using the rqt tool (type rqt in a terminal). There's a plugin "Vizualization>Plot" that integrate the python 3 matplotlib package.

To enable the fetching of the wheels velocities from the bridge, you may need to edit the file "~/.config/fejemis_bridge/bridge.yaml" and set enable: true instead of false in the vel section.

== Hardware ==

=== Mechanics ===

You can access the technical description of the [[Mechanics]] in this page.

=== Electronics ===

You can access the technical description of the [[Electronics]] in this page.

== Software ==
The software is split into two components:
*'''[[Fejemis bridge]]''' — handles communication between the main Raspberry Pi and the Teensy controllers and provides a manual/remote-control interface. See [[Fejemis repository]] for source and docs.
*'''[[Fejemis ROS2 Software]]''' — higher-level programs for mapping, navigation, and robot behaviours located in '''Fejemis_workspace_2026'''.

==== Old repository ====
You can access the description of the legacy through this link.

== Mission Statement ==
Fejemis is developed to autonomously sweep the ASTA building in a safe and practical way for daily operations.

The robot should:
* Navigate autonomously through the building and reach assigned cleaning areas.
* Plan and execute a full cleaning strategy instead of only following a single manual path.
* Interrupt its cleaning mission when needed to return to a docking station for charging, then resume operation.
* Avoid interfering with ongoing work in the ASTA building by operating safely around people and active tasks.

Fejemis ROS2 Software

2026-05-28T09:07:45Z

S253809: /* Convenience commands */

Back to [[Fejemis_2026]]

== Fejemis Software Overview ==
The Fejemis software is split into multiple components. The full ROS2 workspace and related repositories are available on GitHub: https://github.com/Melvin-Angel/Fejemis_Workspace_2026.git

=== Introduction ===
This documentation summarises the main software components used on the robot and explains how the system can be distributed across multiple Raspberry Pis (Multi‑Pi setup).

The two primary concerns are:
* Hardware bridge and low-level microcontroller integration (the Fejemis bridge).
* The ROS 2 software stack for mapping, perception, and high-level behaviours (the Fejemis ROS2 workspace).

In this page we focus on the ROS 2 software stack to learn more about the low-level microcontroller integration see [[Fejemis bridge]] and [[Fejemis repository]]

== Fejemis ==
The top-level ''fejemis'' package is the entry point for the robot stack. It ties the bridge, description, map/localisation, simulation, and vision packages together and installs the main launch files for the complete system.

Launch files:
* ''launch/robot.launch.py'' - main bringup file for the real robot and multi-Pi setup. It starts the bridge control first and then launches odometry, LiDAR, SLAM, localisation, and vision depending on the selected options.
* ''launch/sim.launch.py'' - convenience entry point for simulation. It starts ''launch/robot.launch.py'' with ''in_sim=True''.

Configurable parameters in ''launch/robot.launch.py'':
* ''is_aux'' - marks whether the launch is running on the auxiliary Raspberry Pi. The default is detected automatically.
* ''in_sim'' - enables simulation mode.
* ''localization'' - runs RTAB-Map in localisation mode when ''true''.
* ''map_location'' - selects the map database to load (''lab'' or ''asta'').
* ''enable_vision'' - enables the vision stack for people, cable, and net detection.

== Fejemis Bridge ==
The ''fejemis_bridge'' package handles the low-level interface between the main Raspberry Pi and the Teensy controllers. It contains the serial bridge, the command mixer, and the joystick helper nodes used for manual control. See [[Fejemis Bridge ROS2]] for more detailed information

Launch files:
* ''launch/bridge.launch.py'' - starts the bridge node itself.
* ''launch/mixer.launch.py'' - starts the command mixer and joystick input nodes.
* ''launch/control.launch.py'' - launches the full control side of the bridge stack and forwards the serial-device arguments.

Nodes in this package:
* ''bridge'' - main bridge executable, published in the ''fejemis'' namespace.
* ''raubase_core/rcmixer'' - command mixer that combines control inputs.
* ''joy/game_controller_node'' - joystick/gamepad input node.
* ''raubase_core/joy_control'' - joystick mode switch node.
* ''joy_brush_toggle.py'' - button toggle for the brush control.
* ''joy_linear_actuator.py'' - button toggle for the linear actuator.
* ''keyboard_teleop_rover.py'' - keyboard teleoperation node that publishes RoverCommand messages.
* ''twist_to_rover_command.py'' - adapter that converts Twist commands into RoverCommand messages.

Configurable parameters:
* ''front_device'' - serial device for the front Teensy controller.
* ''drive_device'' - serial device for the drive Teensy controller.
* ''log_level'' - logging level for the bridge node.
* ''in_sim'' and ''is_aux'' - forwarded through the control launch file to select the correct runtime path.
* Joystick helper settings such as ''joy_topic'', ''steer_manage_topic'', ''linear_actuator_topic'', ''button_index'', ''initial_brush_on'', and ''initial_actuator_up''.

== Fejemis Description ==
The ''fejemis_description'' package provides the robot model and URDF/Xacro setup. It is responsible for publishing the robot description and state so the rest of the stack can use the same model. Fore more detailed information see [[Fejemis Description ROS2]]

Launch files:
* ''launch/model.launch.py'' - generates the URDF from the Xacro description and launches ''robot_state_publisher''.

Nodes in this package:
* ''robot_state_publisher'' - publishes the robot TF tree from the generated ''robot_description''.

Configurable parameters:
* ''use_sim_time'' - enables simulated time when running in Gazebo or another simulator.

== Fejemis Sim ==
The ''fejemis_sim'' package contains the Gazebo simulation resources and the simulation bringup. It combines the robot description with the simulation world and connects ROS topics to Gazebo. For more detailed information see [[Fejemis Sim ROS2]]

Launch files:
* ''launch/sim.launch.py'' - starts the full simulation stack.

Nodes in this package:
* ''ros_gz_sim/create'' - spawns the robot in Gazebo from ''/robot_description''.
* ''ros_gz_bridge/parameter_bridge'' - bridges ROS and Gazebo topics.
* ''raubase_core/rover_2_twist'' - optional adapter that converts rover commands into Twist commands for Gazebo.

Configurable parameters:
* ''world'' - world file to load, defaulting to ''fejemis_sim::worlds/room.world''.
* ''rover_cmd_adapter'' - enables or disables the RoverCommand-to-Twist adapter.
* ''rviz'' - optional RViz flag provided by the launch file.

== Fejemis Maploc ==
The ''fejemis_maploc'' package contains the mapping, localisation, navigation, and sensor bringup for the robot. It is the package that ties the RealSense camera, LiDAR, odometry, RTAB-Map, Nav2, and the behaviour-tree layer together. See [[Fejemis Maploc Ros2]] for more detailed information

Launch files:
* ''launch/lidar.launch.py'' - starts the LiDAR driver and forwards the serial-port and container settings.
* ''launch/tf.launch.py'' - publishes the static transforms between the robot, camera, and LiDAR frames.
* ''launch/odometry.launch.py'' - launches the EKF and IMU filter for wheel and IMU odometry.
* ''launch/map.launch.py'' - starts the map server component for the local map container.
* ''launch/lifecycle.launch.py'' - starts the Nav2 lifecycle managers.
* ''launch/planning.launch.py'' - starts the Nav2 planning stack, path follower, behavior server, and coverage server.
* ''launch/behavior.launch.py'' - starts the BT engine and behavior-tree-related runtime nodes.
* ''launch/realsense.launch.py'' - starts the RealSense camera driver.
* ''launch/rtabmap_slam.launch.py'' - starts RTAB-Map SLAM or localisation.

Main nodes and components:
* ''imu_filter_madgwick_node'' - filters raw IMU data before fusion.
* ''robot_localization/ekf_node'' - fuses wheel odometry and IMU data.
* ''ldlidar_stl_ros2'' LiDAR driver - publishes the laser scan used by navigation and SLAM.
* ''tf2_ros/static_transform_publisher'' - publishes the fixed camera and LiDAR transforms.
* ''depthimage_to_laserscan_node'' - converts the depth image to a laser scan for the costmaps.
* ''rtabmap_sync/rgbd_sync'' and ''rtabmap_odom/icp_odometry'' - prepare RGB-D data and odometry for RTAB-Map.
* ''rtabmap_slam/rtabmap'' - runs SLAM or localisation depending on the launch arguments.
* ''nav2_map_server::MapServer'' - provides the occupancy grid map.
* ''nav2_controller/controller_server'', ''nav2_planner/planner_server'', ''nav2_behaviors/behavior_server'', and ''nav2_bt_navigator/bt_navigator'' - navigation and behaviour nodes.
* ''nav2_lifecycle_manager/lifecycle_manager'' - manages the Nav2 lifecycle transitions.
* ''fejemis_maploc/nav2_cmd_vel_deg_relay.py'' - relays Nav2 velocity commands into the robot control format.
* ''fejemis_maploc/brush_control_service.py'' - exposes the brush control service used by the planning stack.
* ''fejemis_maploc/complete_coverage_planner.py'' and ''fejemis_maploc/fixed_complete_coverage_planner.py'' - coverage planners for generating full-field coverage actions.
* ''fejemis_maploc/mission_trigger.py'' - publishes the cleaning mission and supporting blackboard values.
* ''fejemis_maploc/rviz_goal_relay.py'' - converts RViz goal clicks into NavigateToPose actions.
* ''opennav_coverage/opennav_coverage'' - optional coverage server when the coverage packages are available.

Configurable parameters:
* ''use_sim_time'' - switches the stack to simulated time.
* ''is_aux'' - used by the top-level bringup to decide which side of the multi-Pi setup launches the sensor stack.
* ''localization'' - selects between SLAM mode and localisation mode in RTAB-Map.
* ''map_location'' - selects which RTAB-Map database to load (''lab'' or ''asta'').
* ''enable_vision'' - enables the vision stack from the top-level bringup.
* ''enable_coverage'' - enables the coverage navigation components.
* ''namespace'' - namespace used by the SLAM and navigation nodes.
* ''port_name'' - serial port for the LiDAR.
* ''ld_make_container'', ''ld_container'', and ''ld_container_ns'' - control how the LiDAR node is placed in its component container.
* ''camera_namespace'' - namespace passed to the RealSense driver.
* ''world'', ''rover_cmd_adapter'', and ''rviz'' - simulation parameters forwarded from the top-level bringup.

== Fejemis Vision ==
The ''fejemis_vision'' package provides the vision pipeline for the robot. In the current bringup, only the nodes launched from ''launch/main.launch.py'' are listed here. See [[Fejemis Vision ROS2]] for more detailed information.

Nodes launched in ''launch/main.launch.py'':
* ''tf2_ros/static_transform_publisher'' - publishes the test camera transform when both ''enable_vision'' and ''test_static_tf'' are true.
* ''rviz2/rviz2'' - opens RViz with the vision configuration when ''enable_vision'' and ''open_rviz'' are true.
* ''fejemis_vision/main.py'' - runs the main vision node when ''enable_vision'' is true.

Configurable parameters:
* ''enable_vision'' - master switch for the vision stack.
* ''open_rviz'' - opens RViz for the vision stack when enabled.
* ''test_static_tf'' - adds a temporary static transform for camera frame testing.
* The main vision node also loads ''config/main.yaml'' and uses ''use_sim_time=False'' in this launch file.

== Dependencies (src/deps) ==

The repository vendors a set of third-party and support packages under ``src/deps``. These are kept in-tree to ensure reproducible builds and known-good versions for Fejemis.

For a short index and per-package summaries see [[Dependencies Index]].

== Multi pi setup ==
This system supports a multi‑Pi arrangement where some hardware (for example, a RealSense camera or other sensors) runs on an auxiliary Raspberry Pi while the main robot and ROS 2 stack run on the primary Pi.

Recommended minimal steps for a Multi‑Pi setup:

1. Network and host setup
* Ensure all Pis are on the same local network (switch, VLAN or Wi‑Fi SSID) and can reach each other via IP.
* Prefer DHCP reservations or static IPs for each Pi to simplify configuration.

2. SSH and automation
* Install SSH keys on each Pi so you can run remote commands and automation without passwords.

3. Clone the workspace and install dependencies
On each Pi that needs software from the repository, clone the workspace and run the provided install scripts:

git clone https://github.com/Melvin-Angel/Fejemis_Workspace_2026.git
cd Fejemis_Workspace_2026
./install.sh
source ./activate.sh

4. Camera / sensor hosts
* If the camera is on an auxiliary Pi, run the camera driver or streaming node there and expose the expected ROS topics across the network.
* In this workspace the mapping components expect RealSense RGB‑D topics; when the camera is on an aux Pi you can launch the camera remotely. Example (run on the primary Pi to start the aux launch):

```bash
fejemis launch_aux fejemis_maploc realsense.launch.py
```

5. Time sync and reliability
* Enable NTP or systemd‑timesyncd on all Pis so timestamps on messages line up for logging and perception.

=== Convenience commands ===
These helper commands make working with a two‑Pi setup easier.

* ''fejemis launch_aux'' — start a launch file on an auxiliary Pi from the primary Pi. This is commonly used to start camera or sensor nodes remotely. Example:

fejemis launch_aux fejemis robot.launch.py

* ''fejemis sync_clocks'' — synchronise system clocks across Pis (uses SSH and sudo on targets). Run this before recording or starting perception stacks so timestamps align. Example:

fejemis sync_clocks

== Network Configuration ==

== ROS Network files ==

The bash scripts used for configuring the ROS 2 network are located in the bin/ directory:
* ''bash_setup.sh'' - Configures the ROS 2 network settings for the Main Raspberry Pi
* ''bash_setup_aux.sh'' - Configures the ROS 2 network settings for the Aux Raspberry Pi

These scripts set up:
* ROS 2 environment variables
* CycloneDDS configuration
* Static peer discovery
* Network interface binding for Ethernet communication

The scripts are automatically sourced during system startup, ensuring that the ROS 2 network is configured correctly whenever the Raspberry Pi units boot.

Additional information about Ethernet configuration and the required ROS 2 network setup on external computers can be found in the [[Electronics]] section.

Fejemis ROS2 Software

2026-05-28T09:07:24Z

S253809: /* Multi pi setup */

Back to [[Fejemis_2026]]

== Fejemis Software Overview ==
The Fejemis software is split into multiple components. The full ROS2 workspace and related repositories are available on GitHub: https://github.com/Melvin-Angel/Fejemis_Workspace_2026.git

=== Introduction ===
This documentation summarises the main software components used on the robot and explains how the system can be distributed across multiple Raspberry Pis (Multi‑Pi setup).

The two primary concerns are:
* Hardware bridge and low-level microcontroller integration (the Fejemis bridge).
* The ROS 2 software stack for mapping, perception, and high-level behaviours (the Fejemis ROS2 workspace).

In this page we focus on the ROS 2 software stack to learn more about the low-level microcontroller integration see [[Fejemis bridge]] and [[Fejemis repository]]

== Fejemis ==
The top-level ''fejemis'' package is the entry point for the robot stack. It ties the bridge, description, map/localisation, simulation, and vision packages together and installs the main launch files for the complete system.

Launch files:
* ''launch/robot.launch.py'' - main bringup file for the real robot and multi-Pi setup. It starts the bridge control first and then launches odometry, LiDAR, SLAM, localisation, and vision depending on the selected options.
* ''launch/sim.launch.py'' - convenience entry point for simulation. It starts ''launch/robot.launch.py'' with ''in_sim=True''.

Configurable parameters in ''launch/robot.launch.py'':
* ''is_aux'' - marks whether the launch is running on the auxiliary Raspberry Pi. The default is detected automatically.
* ''in_sim'' - enables simulation mode.
* ''localization'' - runs RTAB-Map in localisation mode when ''true''.
* ''map_location'' - selects the map database to load (''lab'' or ''asta'').
* ''enable_vision'' - enables the vision stack for people, cable, and net detection.

== Fejemis Bridge ==
The ''fejemis_bridge'' package handles the low-level interface between the main Raspberry Pi and the Teensy controllers. It contains the serial bridge, the command mixer, and the joystick helper nodes used for manual control. See [[Fejemis Bridge ROS2]] for more detailed information

Launch files:
* ''launch/bridge.launch.py'' - starts the bridge node itself.
* ''launch/mixer.launch.py'' - starts the command mixer and joystick input nodes.
* ''launch/control.launch.py'' - launches the full control side of the bridge stack and forwards the serial-device arguments.

Nodes in this package:
* ''bridge'' - main bridge executable, published in the ''fejemis'' namespace.
* ''raubase_core/rcmixer'' - command mixer that combines control inputs.
* ''joy/game_controller_node'' - joystick/gamepad input node.
* ''raubase_core/joy_control'' - joystick mode switch node.
* ''joy_brush_toggle.py'' - button toggle for the brush control.
* ''joy_linear_actuator.py'' - button toggle for the linear actuator.
* ''keyboard_teleop_rover.py'' - keyboard teleoperation node that publishes RoverCommand messages.
* ''twist_to_rover_command.py'' - adapter that converts Twist commands into RoverCommand messages.

Configurable parameters:
* ''front_device'' - serial device for the front Teensy controller.
* ''drive_device'' - serial device for the drive Teensy controller.
* ''log_level'' - logging level for the bridge node.
* ''in_sim'' and ''is_aux'' - forwarded through the control launch file to select the correct runtime path.
* Joystick helper settings such as ''joy_topic'', ''steer_manage_topic'', ''linear_actuator_topic'', ''button_index'', ''initial_brush_on'', and ''initial_actuator_up''.

== Fejemis Description ==
The ''fejemis_description'' package provides the robot model and URDF/Xacro setup. It is responsible for publishing the robot description and state so the rest of the stack can use the same model. Fore more detailed information see [[Fejemis Description ROS2]]

Launch files:
* ''launch/model.launch.py'' - generates the URDF from the Xacro description and launches ''robot_state_publisher''.

Nodes in this package:
* ''robot_state_publisher'' - publishes the robot TF tree from the generated ''robot_description''.

Configurable parameters:
* ''use_sim_time'' - enables simulated time when running in Gazebo or another simulator.

== Fejemis Sim ==
The ''fejemis_sim'' package contains the Gazebo simulation resources and the simulation bringup. It combines the robot description with the simulation world and connects ROS topics to Gazebo. For more detailed information see [[Fejemis Sim ROS2]]

Launch files:
* ''launch/sim.launch.py'' - starts the full simulation stack.

Nodes in this package:
* ''ros_gz_sim/create'' - spawns the robot in Gazebo from ''/robot_description''.
* ''ros_gz_bridge/parameter_bridge'' - bridges ROS and Gazebo topics.
* ''raubase_core/rover_2_twist'' - optional adapter that converts rover commands into Twist commands for Gazebo.

Configurable parameters:
* ''world'' - world file to load, defaulting to ''fejemis_sim::worlds/room.world''.
* ''rover_cmd_adapter'' - enables or disables the RoverCommand-to-Twist adapter.
* ''rviz'' - optional RViz flag provided by the launch file.

== Fejemis Maploc ==
The ''fejemis_maploc'' package contains the mapping, localisation, navigation, and sensor bringup for the robot. It is the package that ties the RealSense camera, LiDAR, odometry, RTAB-Map, Nav2, and the behaviour-tree layer together. See [[Fejemis Maploc Ros2]] for more detailed information

Launch files:
* ''launch/lidar.launch.py'' - starts the LiDAR driver and forwards the serial-port and container settings.
* ''launch/tf.launch.py'' - publishes the static transforms between the robot, camera, and LiDAR frames.
* ''launch/odometry.launch.py'' - launches the EKF and IMU filter for wheel and IMU odometry.
* ''launch/map.launch.py'' - starts the map server component for the local map container.
* ''launch/lifecycle.launch.py'' - starts the Nav2 lifecycle managers.
* ''launch/planning.launch.py'' - starts the Nav2 planning stack, path follower, behavior server, and coverage server.
* ''launch/behavior.launch.py'' - starts the BT engine and behavior-tree-related runtime nodes.
* ''launch/realsense.launch.py'' - starts the RealSense camera driver.
* ''launch/rtabmap_slam.launch.py'' - starts RTAB-Map SLAM or localisation.

Main nodes and components:
* ''imu_filter_madgwick_node'' - filters raw IMU data before fusion.
* ''robot_localization/ekf_node'' - fuses wheel odometry and IMU data.
* ''ldlidar_stl_ros2'' LiDAR driver - publishes the laser scan used by navigation and SLAM.
* ''tf2_ros/static_transform_publisher'' - publishes the fixed camera and LiDAR transforms.
* ''depthimage_to_laserscan_node'' - converts the depth image to a laser scan for the costmaps.
* ''rtabmap_sync/rgbd_sync'' and ''rtabmap_odom/icp_odometry'' - prepare RGB-D data and odometry for RTAB-Map.
* ''rtabmap_slam/rtabmap'' - runs SLAM or localisation depending on the launch arguments.
* ''nav2_map_server::MapServer'' - provides the occupancy grid map.
* ''nav2_controller/controller_server'', ''nav2_planner/planner_server'', ''nav2_behaviors/behavior_server'', and ''nav2_bt_navigator/bt_navigator'' - navigation and behaviour nodes.
* ''nav2_lifecycle_manager/lifecycle_manager'' - manages the Nav2 lifecycle transitions.
* ''fejemis_maploc/nav2_cmd_vel_deg_relay.py'' - relays Nav2 velocity commands into the robot control format.
* ''fejemis_maploc/brush_control_service.py'' - exposes the brush control service used by the planning stack.
* ''fejemis_maploc/complete_coverage_planner.py'' and ''fejemis_maploc/fixed_complete_coverage_planner.py'' - coverage planners for generating full-field coverage actions.
* ''fejemis_maploc/mission_trigger.py'' - publishes the cleaning mission and supporting blackboard values.
* ''fejemis_maploc/rviz_goal_relay.py'' - converts RViz goal clicks into NavigateToPose actions.
* ''opennav_coverage/opennav_coverage'' - optional coverage server when the coverage packages are available.

Configurable parameters:
* ''use_sim_time'' - switches the stack to simulated time.
* ''is_aux'' - used by the top-level bringup to decide which side of the multi-Pi setup launches the sensor stack.
* ''localization'' - selects between SLAM mode and localisation mode in RTAB-Map.
* ''map_location'' - selects which RTAB-Map database to load (''lab'' or ''asta'').
* ''enable_vision'' - enables the vision stack from the top-level bringup.
* ''enable_coverage'' - enables the coverage navigation components.
* ''namespace'' - namespace used by the SLAM and navigation nodes.
* ''port_name'' - serial port for the LiDAR.
* ''ld_make_container'', ''ld_container'', and ''ld_container_ns'' - control how the LiDAR node is placed in its component container.
* ''camera_namespace'' - namespace passed to the RealSense driver.
* ''world'', ''rover_cmd_adapter'', and ''rviz'' - simulation parameters forwarded from the top-level bringup.

== Fejemis Vision ==
The ''fejemis_vision'' package provides the vision pipeline for the robot. In the current bringup, only the nodes launched from ''launch/main.launch.py'' are listed here. See [[Fejemis Vision ROS2]] for more detailed information.

Nodes launched in ''launch/main.launch.py'':
* ''tf2_ros/static_transform_publisher'' - publishes the test camera transform when both ''enable_vision'' and ''test_static_tf'' are true.
* ''rviz2/rviz2'' - opens RViz with the vision configuration when ''enable_vision'' and ''open_rviz'' are true.
* ''fejemis_vision/main.py'' - runs the main vision node when ''enable_vision'' is true.

Configurable parameters:
* ''enable_vision'' - master switch for the vision stack.
* ''open_rviz'' - opens RViz for the vision stack when enabled.
* ''test_static_tf'' - adds a temporary static transform for camera frame testing.
* The main vision node also loads ''config/main.yaml'' and uses ''use_sim_time=False'' in this launch file.

== Dependencies (src/deps) ==

The repository vendors a set of third-party and support packages under ``src/deps``. These are kept in-tree to ensure reproducible builds and known-good versions for Fejemis.

For a short index and per-package summaries see [[Dependencies Index]].

== Multi pi setup ==
This system supports a multi‑Pi arrangement where some hardware (for example, a RealSense camera or other sensors) runs on an auxiliary Raspberry Pi while the main robot and ROS 2 stack run on the primary Pi.

Recommended minimal steps for a Multi‑Pi setup:

1. Network and host setup
* Ensure all Pis are on the same local network (switch, VLAN or Wi‑Fi SSID) and can reach each other via IP.
* Prefer DHCP reservations or static IPs for each Pi to simplify configuration.

2. SSH and automation
* Install SSH keys on each Pi so you can run remote commands and automation without passwords.

3. Clone the workspace and install dependencies
On each Pi that needs software from the repository, clone the workspace and run the provided install scripts:

git clone https://github.com/Melvin-Angel/Fejemis_Workspace_2026.git
cd Fejemis_Workspace_2026
./install.sh
source ./activate.sh

4. Camera / sensor hosts
* If the camera is on an auxiliary Pi, run the camera driver or streaming node there and expose the expected ROS topics across the network.
* In this workspace the mapping components expect RealSense RGB‑D topics; when the camera is on an aux Pi you can launch the camera remotely. Example (run on the primary Pi to start the aux launch):

```bash
fejemis launch_aux fejemis_maploc realsense.launch.py
```

5. Time sync and reliability
* Enable NTP or systemd‑timesyncd on all Pis so timestamps on messages line up for logging and perception.

=== Convenience commands ===
These helper commands make working with a two‑Pi setup easier.

* ''fejemis launch_aux'' — start a launch file on an auxiliary Pi from the primary Pi. This is commonly used to start camera or sensor nodes remotely. Example:

```bash
fejemis launch_aux fejemis robot.launch.py
```

* ''fejemis sync_clocks'' — synchronise system clocks across Pis (uses SSH and sudo on targets). Run this before recording or starting perception stacks so timestamps align. Example:

```bash
fejemis sync_clocks
```

== Network Configuration ==

== ROS Network files ==

The bash scripts used for configuring the ROS 2 network are located in the bin/ directory:
* ''bash_setup.sh'' - Configures the ROS 2 network settings for the Main Raspberry Pi
* ''bash_setup_aux.sh'' - Configures the ROS 2 network settings for the Aux Raspberry Pi

These scripts set up:
* ROS 2 environment variables
* CycloneDDS configuration
* Static peer discovery
* Network interface binding for Ethernet communication

The scripts are automatically sourced during system startup, ensuring that the ROS 2 network is configured correctly whenever the Raspberry Pi units boot.

Additional information about Ethernet configuration and the required ROS 2 network setup on external computers can be found in the [[Electronics]] section.

Fejemis Description ROS2

2026-05-28T09:04:54Z

S253809: /* Typical Usage */

Back to [[Fejemis ROS2 Software]]

== Purpose ==

''fejemis_description'' is the robot description package for Fejemis. It defines the robot model using URDF/Xacro and publishes the TF model state so the rest of the ROS 2 stack can use a consistent robot geometry.

The package is responsible for:

* Building the Fejemis robot model from modular xacro files.
* Publishing ''robot_description'' and TF through ''robot_state_publisher''.
* Providing a simulation-friendly wheel configuration through launch arguments.
* Keeping a shared frame and link structure used by simulation, mapping, and navigation.

== Package Layout ==

Important folders:

* ''launch'' - launch files for model generation and state publishing.
* ''xacro'' - modular robot model definition files.
* ''config'' - controller/simulation-related configuration.

Main xacro files:

* ''xacro/robot.urdf.xacro'' - top-level robot assembly file.
* ''xacro/robot_core.xacro'' - chassis and base links.
* ''xacro/robot_wheels.xacro'' - drive wheels and Gazebo diff-drive plugin.
* ''xacro/robot_castor.xacro'' - castor wheel definition.
* ''xacro/sensor_imu.xacro'' - IMU and magnetometer links/sensors.
* ''xacro/sensor_lidar.xacro'' - LiDAR link and simulated scan sensor.
* ''xacro/sensor_depth_camera.xacro'' - depth camera and optical frame.
* ''xacro/utils_inertial.xacro'' and ''xacro/utils_colors.xacro'' - reusable inertial and visual macros.

== Launch File ==

=== launch/model.launch.py ===

This launch file generates the URDF from xacro and starts ''robot_state_publisher''.

Launch arguments:

* ''use_sim_time'' - enables simulation clock when ''true''. Default: ''false''.

Behavior:

* Processes ''fejemis_description::xacro/robot.urdf.xacro'' with xacro.
* Passes ''fixed_wheels:=<use_sim_time>'' into xacro.
* Publishes ''robot_description'' parameter to ''robot_state_publisher''.
* Publishes the model TF tree for downstream packages.

== Model Composition ==

The top-level model in ''robot.urdf.xacro'' assembles the robot in this order:

* Core chassis: ''xacro:core''
* Drive wheels: ''xacro:wheels''
* Castor wheel: ''xacro:castor''
* IMU and magnetometer: ''xacro:imu_sensor''
* LiDAR: ''xacro:lidar''
* Depth camera: ''xacro:depth''

There is also an optional camera macro (currently commented in the top-level xacro).

== Main Links and Frames ==

Common links/frames provided by the description:

* ''base_link'' - primary base frame for navigation and odometry integration.
* ''chassis'' - main physical body link.
* ''left_wheel'' and ''right_wheel'' - drive wheel links.
* ''laser_frame'' - LiDAR mounting and scan frame in simulation.
* ''imu_link'' - IMU and magnetometer frame.
* ''depth_link'' and ''depth_link_optical'' - depth camera body and optical frame.

These frames are consumed by mapping/localisation and simulation bridge layers.

== Simulation-Specific Behavior ==

The wheel macro supports both fixed and rotating wheel joints:

* Real robot path: wheel joints are fixed in the model launch context.
* Simulation path: wheels are set to continuous joints when simulation mode is enabled.

Gazebo systems configured in ''robot_wheels.xacro'':

* ''gz::sim::systems::DiffDrive''
* ''gz::sim::systems::JointStatePublisher''

Diff-drive plugin settings include:

* Wheel separation: ''0.3847''
* Wheel diameter: ''0.2''
* Command topic: ''/model/fejemis/cmd_vel''
* Odometry topic: ''/model/fejemis/odom''
* Frame IDs: ''odom'' and ''base_link''

Drive wheel friction is increased in simulation to reduce slip, while castor behavior is defined separately in the castor xacro.

== Sensor Simulation Topics ==

The description also defines simulated sensor outputs used by the bridge layer:

* LiDAR: ''/model/fejemis/scan''
* IMU: ''/model/fejemis/imu''
* Magnetometer: ''/model/fejemis/mag''
* Depth camera: ''/model/fejemis/depth_camera''

These topics are later bridged into ROS topics by ''fejemis_sim''.

== Typical Usage ==

Start robot description publisher:
ros2 launch fejemis_description model.launch.py

Start with simulation time:
ros2 launch fejemis_description model.launch.py use_sim_time:=true

In most full-system runs, this launch file is included by top-level bringup (for example in simulation launch stacks).

== Relationship To Other Packages ==

* ''fejemis_sim'' uses this package to spawn the robot from ''/robot_description''.
* ''fejemis_maploc'' and Nav2 depend on the frame structure defined here.
* Sensor and odometry pipelines assume the link names and frame conventions from this model.

Fejemis Description ROS2

2026-05-28T09:03:59Z

S253809: Created page with "Back to Fejemis ROS2 Software == Purpose == ''fejemis_description'' is the robot description package for Fejemis. It defines the robot model using URDF/Xacro and publishes the TF model state so the rest of the ROS 2 stack can use a consistent robot geometry. The package is responsible for: * Building the Fejemis robot model from modular xacro files. * Publishing ''robot_description'' and TF through ''robot_state_publisher''. * Providing a simulation-friendly whee..."

Back to [[Fejemis ROS2 Software]]

== Purpose ==

''fejemis_description'' is the robot description package for Fejemis. It defines the robot model using URDF/Xacro and publishes the TF model state so the rest of the ROS 2 stack can use a consistent robot geometry.

The package is responsible for:

* Building the Fejemis robot model from modular xacro files.
* Publishing ''robot_description'' and TF through ''robot_state_publisher''.
* Providing a simulation-friendly wheel configuration through launch arguments.
* Keeping a shared frame and link structure used by simulation, mapping, and navigation.

== Package Layout ==

Important folders:

* ''launch'' - launch files for model generation and state publishing.
* ''xacro'' - modular robot model definition files.
* ''config'' - controller/simulation-related configuration.

Main xacro files:

* ''xacro/robot.urdf.xacro'' - top-level robot assembly file.
* ''xacro/robot_core.xacro'' - chassis and base links.
* ''xacro/robot_wheels.xacro'' - drive wheels and Gazebo diff-drive plugin.
* ''xacro/robot_castor.xacro'' - castor wheel definition.
* ''xacro/sensor_imu.xacro'' - IMU and magnetometer links/sensors.
* ''xacro/sensor_lidar.xacro'' - LiDAR link and simulated scan sensor.
* ''xacro/sensor_depth_camera.xacro'' - depth camera and optical frame.
* ''xacro/utils_inertial.xacro'' and ''xacro/utils_colors.xacro'' - reusable inertial and visual macros.

== Launch File ==

=== launch/model.launch.py ===

This launch file generates the URDF from xacro and starts ''robot_state_publisher''.

Launch arguments:

* ''use_sim_time'' - enables simulation clock when ''true''. Default: ''false''.

Behavior:

* Processes ''fejemis_description::xacro/robot.urdf.xacro'' with xacro.
* Passes ''fixed_wheels:=<use_sim_time>'' into xacro.
* Publishes ''robot_description'' parameter to ''robot_state_publisher''.
* Publishes the model TF tree for downstream packages.

== Model Composition ==

The top-level model in ''robot.urdf.xacro'' assembles the robot in this order:

* Core chassis: ''xacro:core''
* Drive wheels: ''xacro:wheels''
* Castor wheel: ''xacro:castor''
* IMU and magnetometer: ''xacro:imu_sensor''
* LiDAR: ''xacro:lidar''
* Depth camera: ''xacro:depth''

There is also an optional camera macro (currently commented in the top-level xacro).

== Main Links and Frames ==

Common links/frames provided by the description:

* ''base_link'' - primary base frame for navigation and odometry integration.
* ''chassis'' - main physical body link.
* ''left_wheel'' and ''right_wheel'' - drive wheel links.
* ''laser_frame'' - LiDAR mounting and scan frame in simulation.
* ''imu_link'' - IMU and magnetometer frame.
* ''depth_link'' and ''depth_link_optical'' - depth camera body and optical frame.

These frames are consumed by mapping/localisation and simulation bridge layers.

== Simulation-Specific Behavior ==

The wheel macro supports both fixed and rotating wheel joints:

* Real robot path: wheel joints are fixed in the model launch context.
* Simulation path: wheels are set to continuous joints when simulation mode is enabled.

Gazebo systems configured in ''robot_wheels.xacro'':

* ''gz::sim::systems::DiffDrive''
* ''gz::sim::systems::JointStatePublisher''

Diff-drive plugin settings include:

* Wheel separation: ''0.3847''
* Wheel diameter: ''0.2''
* Command topic: ''/model/fejemis/cmd_vel''
* Odometry topic: ''/model/fejemis/odom''
* Frame IDs: ''odom'' and ''base_link''

Drive wheel friction is increased in simulation to reduce slip, while castor behavior is defined separately in the castor xacro.

== Sensor Simulation Topics ==

The description also defines simulated sensor outputs used by the bridge layer:

* LiDAR: ''/model/fejemis/scan''
* IMU: ''/model/fejemis/imu''
* Magnetometer: ''/model/fejemis/mag''
* Depth camera: ''/model/fejemis/depth_camera''

These topics are later bridged into ROS topics by ''fejemis_sim''.

== Typical Usage ==

Start robot description publisher:

```bash
ros2 launch fejemis_description model.launch.py
```

Start with simulation time:

```bash
ros2 launch fejemis_description model.launch.py use_sim_time:=true
```

In most full-system runs, this launch file is included by top-level bringup (for example in simulation launch stacks).

== Relationship To Other Packages ==

* ''fejemis_sim'' uses this package to spawn the robot from ''/robot_description''.
* ''fejemis_maploc'' and Nav2 depend on the frame structure defined here.
* Sensor and odometry pipelines assume the link names and frame conventions from this model.

2026-05-27T14:37:48Z

S253809:

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.
[[File:fejemis_maploc_ekf_block_diagram.png| 1600px]]

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.

'''Diagram placeholder:''' TF tree showing ''map -> odom -> base_link'', then the camera and LiDAR static frames.

=== 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.

[[File:maploc_general_block_diagram.png| 1600px]]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.

'''Block diagram placeholder:''' RTAB-Map data path from RealSense, LiDAR scan, EKF odom, and initial pose into RTAB-Map, with outputs to ''map'', TF, and Nav2.

=== 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:

<pre>
stamp;frame;x;y;z;qx;qy;qz;qw
</pre>

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:

<pre>
(-5.35, 2.45)
( 2.83, 3.18)
( 3.22, -5.08)
(-4.76, -5.97)
</pre>

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'':

<pre>
main
`-- Sequence
|-- SingleTrigger
| `-- t_init
`-- KeepRunningUntilFailure
`-- ReactiveSequence
|-- ForceSuccess
| `-- t_passive
`-- t_runtime
</pre>

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:

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

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:

<pre>
ros2 launch fejemis robot.launch.py localization:=true map_location:=asta
</pre>

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:

<pre>
ros2 run fejemis_maploc mission_trigger.py
</pre>

Send an RViz navigation goal:

<pre>
ros2 run fejemis_maploc rviz_goal_relay.py
</pre>

Test coverage navigation with RViz clicked points:

<pre>
ros2 run fejemis_maploc complete_coverage_planner.py
</pre>

Test coverage navigation with the fixed polygon:

<pre>
ros2 run fejemis_maploc fixed_complete_coverage_planner.py
</pre>

== Notes and Future Work ==

* Add exported diagrams for the odometry EKF, RTAB-Map data flow, Nav2 planning stack, and behaviour tree.
* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Document the map database creation/update procedure for ''lab'' and ''asta''.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

File:Fejemis maploc ekf block diagram.png

2026-05-27T14:35:48Z

S253809:

Fejemis Maploc Ros2

2026-05-27T14:27:39Z

S253809: /* Purpose */

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.
[[File:maploc_general_block_diagram.png| 1600px]]

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.

'''Diagram placeholder:''' TF tree showing ''map -> odom -> base_link'', then the camera and LiDAR static frames.

=== 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.

'''Block diagram placeholder:''' 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.

'''Block diagram placeholder:''' RTAB-Map data path from RealSense, LiDAR scan, EKF odom, and initial pose into RTAB-Map, with outputs to ''map'', TF, and Nav2.

=== 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:

<pre>
stamp;frame;x;y;z;qx;qy;qz;qw
</pre>

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:

<pre>
(-5.35, 2.45)
( 2.83, 3.18)
( 3.22, -5.08)
(-4.76, -5.97)
</pre>

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'':

<pre>
main
`-- Sequence
|-- SingleTrigger
| `-- t_init
`-- KeepRunningUntilFailure
`-- ReactiveSequence
|-- ForceSuccess
| `-- t_passive
`-- t_runtime
</pre>

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:

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

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:

<pre>
ros2 launch fejemis robot.launch.py localization:=true map_location:=asta
</pre>

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:

<pre>
ros2 run fejemis_maploc mission_trigger.py
</pre>

Send an RViz navigation goal:

<pre>
ros2 run fejemis_maploc rviz_goal_relay.py
</pre>

Test coverage navigation with RViz clicked points:

<pre>
ros2 run fejemis_maploc complete_coverage_planner.py
</pre>

Test coverage navigation with the fixed polygon:

<pre>
ros2 run fejemis_maploc fixed_complete_coverage_planner.py
</pre>

== Notes and Future Work ==

* Add exported diagrams for the odometry EKF, RTAB-Map data flow, Nav2 planning stack, and behaviour tree.
* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Document the map database creation/update procedure for ''lab'' and ''asta''.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

Fejemis Maploc Ros2

2026-05-27T14:27:09Z

S253809: /* Purpose */

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.
[[File:maploc_general_block_diagram.png| 1600px]]

'''Block diagram placeholder:''' 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.

'''Diagram placeholder:''' TF tree showing ''map -> odom -> base_link'', then the camera and LiDAR static frames.

=== 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.

'''Block diagram placeholder:''' 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.

'''Block diagram placeholder:''' RTAB-Map data path from RealSense, LiDAR scan, EKF odom, and initial pose into RTAB-Map, with outputs to ''map'', TF, and Nav2.

=== 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:

<pre>
stamp;frame;x;y;z;qx;qy;qz;qw
</pre>

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:

<pre>
(-5.35, 2.45)
( 2.83, 3.18)
( 3.22, -5.08)
(-4.76, -5.97)
</pre>

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'':

<pre>
main
`-- Sequence
|-- SingleTrigger
| `-- t_init
`-- KeepRunningUntilFailure
`-- ReactiveSequence
|-- ForceSuccess
| `-- t_passive
`-- t_runtime
</pre>

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:

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

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:

<pre>
ros2 launch fejemis robot.launch.py localization:=true map_location:=asta
</pre>

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:

<pre>
ros2 run fejemis_maploc mission_trigger.py
</pre>

Send an RViz navigation goal:

<pre>
ros2 run fejemis_maploc rviz_goal_relay.py
</pre>

Test coverage navigation with RViz clicked points:

<pre>
ros2 run fejemis_maploc complete_coverage_planner.py
</pre>

Test coverage navigation with the fixed polygon:

<pre>
ros2 run fejemis_maploc fixed_complete_coverage_planner.py
</pre>

== Notes and Future Work ==

* Add exported diagrams for the odometry EKF, RTAB-Map data flow, Nav2 planning stack, and behaviour tree.
* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Document the map database creation/update procedure for ''lab'' and ''asta''.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

Fejemis Maploc Ros2

2026-05-27T14:26:43Z

S253809: /* Purpose */

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.
[[File:maploc_general_block_diagram.png| 1200px]]

'''Block diagram placeholder:''' 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.

'''Diagram placeholder:''' TF tree showing ''map -> odom -> base_link'', then the camera and LiDAR static frames.

=== 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.

'''Block diagram placeholder:''' 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.

'''Block diagram placeholder:''' RTAB-Map data path from RealSense, LiDAR scan, EKF odom, and initial pose into RTAB-Map, with outputs to ''map'', TF, and Nav2.

=== 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:

<pre>
stamp;frame;x;y;z;qx;qy;qz;qw
</pre>

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:

<pre>
(-5.35, 2.45)
( 2.83, 3.18)
( 3.22, -5.08)
(-4.76, -5.97)
</pre>

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'':

<pre>
main
`-- Sequence
|-- SingleTrigger
| `-- t_init
`-- KeepRunningUntilFailure
`-- ReactiveSequence
|-- ForceSuccess
| `-- t_passive
`-- t_runtime
</pre>

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:

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

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:

<pre>
ros2 launch fejemis robot.launch.py localization:=true map_location:=asta
</pre>

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:

<pre>
ros2 run fejemis_maploc mission_trigger.py
</pre>

Send an RViz navigation goal:

<pre>
ros2 run fejemis_maploc rviz_goal_relay.py
</pre>

Test coverage navigation with RViz clicked points:

<pre>
ros2 run fejemis_maploc complete_coverage_planner.py
</pre>

Test coverage navigation with the fixed polygon:

<pre>
ros2 run fejemis_maploc fixed_complete_coverage_planner.py
</pre>

== Notes and Future Work ==

* Add exported diagrams for the odometry EKF, RTAB-Map data flow, Nav2 planning stack, and behaviour tree.
* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Document the map database creation/update procedure for ''lab'' and ''asta''.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

Fejemis Maploc Ros2

2026-05-27T14:26:19Z

S253809: /* Purpose */

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.
[[File:maploc_general_block_diagram.png| 600px]]
'''Block diagram placeholder:''' 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.

'''Diagram placeholder:''' TF tree showing ''map -> odom -> base_link'', then the camera and LiDAR static frames.

=== 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.

'''Block diagram placeholder:''' 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.

'''Block diagram placeholder:''' RTAB-Map data path from RealSense, LiDAR scan, EKF odom, and initial pose into RTAB-Map, with outputs to ''map'', TF, and Nav2.

=== 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:

<pre>
stamp;frame;x;y;z;qx;qy;qz;qw
</pre>

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:

<pre>
(-5.35, 2.45)
( 2.83, 3.18)
( 3.22, -5.08)
(-4.76, -5.97)
</pre>

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'':

<pre>
main
`-- Sequence
|-- SingleTrigger
| `-- t_init
`-- KeepRunningUntilFailure
`-- ReactiveSequence
|-- ForceSuccess
| `-- t_passive
`-- t_runtime
</pre>

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:

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

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:

<pre>
ros2 launch fejemis robot.launch.py localization:=true map_location:=asta
</pre>

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:

<pre>
ros2 run fejemis_maploc mission_trigger.py
</pre>

Send an RViz navigation goal:

<pre>
ros2 run fejemis_maploc rviz_goal_relay.py
</pre>

Test coverage navigation with RViz clicked points:

<pre>
ros2 run fejemis_maploc complete_coverage_planner.py
</pre>

Test coverage navigation with the fixed polygon:

<pre>
ros2 run fejemis_maploc fixed_complete_coverage_planner.py
</pre>

== Notes and Future Work ==

* Add exported diagrams for the odometry EKF, RTAB-Map data flow, Nav2 planning stack, and behaviour tree.
* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Document the map database creation/update procedure for ''lab'' and ''asta''.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

2026-05-27T12:41:57Z

S253809: