Rsewiki - User contributions [en]

Fejemis Maploc Ros2

2026-05-28T11:41:40Z

S252724: /* Cleaning */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

=== Mapping ===

''t_mapping'' currently checks for ''mission mapping/*'' and then returns to idle.

*Requires mission mapping/*

*Currently acts as a placeholder

*Does not yet launch the SLAM or exploration pipeline

*Returns to idle immediately

This branch should later be expanded to perform autonomous mapping, localization checks, and map saving.

=== 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.
[[File:BT-cleaning.jpeg| 800px]]

Cleaning Node 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 ==

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.
* Test and validate the complete cleaning behaviour structure in the final hardware setup, as it has not yet been fully evaluated due to current Raspberry Pi limitations.

Fejemis Maploc Ros2

2026-05-28T11:41:20Z

S252724: /* Cleaning */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

=== Mapping ===

''t_mapping'' currently checks for ''mission mapping/*'' and then returns to idle.

*Requires mission mapping/*

*Currently acts as a placeholder

*Does not yet launch the SLAM or exploration pipeline

*Returns to idle immediately

This branch should later be expanded to perform autonomous mapping, localization checks, and map saving.

=== 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.
[[File:BT-cleaning.jepg| 800px]]

Cleaning Node 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 ==

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.
* Test and validate the complete cleaning behaviour structure in the final hardware setup, as it has not yet been fully evaluated due to current Raspberry Pi limitations.

File:BT-cleaning.jpeg

2026-05-28T11:40:02Z

S252724:

Mechanics

2026-05-28T11:12:59Z

S252724: /* Main Mechanical Parts */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

=== Front Wheel ===
The front wheel plays an important role in the robot’s movement and overall cleaning performance. It helps guide and steer the robot as it moves through the environment, allowing it to turn, follow paths, and navigate around obstacles while cleaning. As a result, the stability and rigidity of the front wheel directly affect both motion accuracy and mapping quality.

==== Previous Design ====
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.

==== Current Design ====
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available at this link: https://cad.onshape.com/documents/d35a3b26eb917ad7000d2d0c/w/821c3ada1e18c2055d92ea08/e/35b8a6b1c67d3640704aa6e7 as a downloadable ZIP file for future modification and improvement.

==== Future Work ====
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Mechanics

2026-05-28T11:06:14Z

S252724: /* Front Wheel Design */

Mechanics

2026-05-28T11:05:39Z

S252724: /* Front Wheel Design */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

== Front Wheel Design ==
=== Previous Design ===
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.
=== Current Design ===
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available on this website as a downloadable ZIP file for future modification and improvement.

=== Future Works ===
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Mechanics

2026-05-28T11:05:25Z

S252724: /* Main Mechanical Parts */

Mechanics

2026-05-28T11:05:10Z

S252724: /* Front Wheel Design */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

=== Front Wheel Design ===

=== Previous Design ===
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.
=== Current Design ===
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available on this website as a downloadable ZIP file for future modification and improvement.

=== Future Works ===
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Mechanics

2026-05-28T11:05:01Z

S252724: /* Front Wheel Design */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

=== Front Wheel Design ===
=== Previous Design ===
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.
=== Current Design ===
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available on this website as a downloadable ZIP file for future modification and improvement.

=== Future Works ===
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Mechanics

2026-05-28T11:04:54Z

S252724: /* Main Mechanical Parts */

Mechanics

2026-05-28T11:04:45Z

S252724: /* Front Wheel Design */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

=== Front Wheel Design ===

=== Previous Design ===
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.
=== Current Design ===
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available on this website as a downloadable ZIP file for future modification and improvement.

=== Future Works ===
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Mechanics

2026-05-28T11:02:53Z

S252724: /* Front Wheel Design */

Back to [[Fejemis_2026]]

== General Description ==

Fejemis is a three wheels mobile robot. The front part in yellow and black is a commercial brush for industry floor. The manual handle has been replaced with the Fejemis drive system. The front wheel can be lowered to allow the brush to be lifted when not in use.

It has been modeled in the cad program OnShape, as you can see in the next figure.

[[File:fejemis-from-onshape.png | 600px]]

The drawings can be accessed here: https://cad.onshape.com/documents/c3f44096b0f5e4646ed48e3f/w/670467f68cac02b087631e3d/e/e591dbcff8879adc4528ee2b
== Main Mechanical Parts ==

=== Front Wheel Design ===
=== Previous Design ===
The original front wheel assembly was one of the main mechanical weaknesses of the robot. Its low rigidity caused wobbling during motion, which negatively affected odometry and reduced movement stability. This introduced drift in the generated map and lowered the overall quality of localization and navigation.
=== Current Design ===
The wheel assembly was redesigned by adding ball bearings, increasing the infill of the 3D-printed parts, and mounting the magnetic encoder more securely. These changes improved rigidity, produced steadier motion, and reduced map drift.

The updated design is available on this website as a downloadable ZIP file for future modification and improvement.

=== Future Works ===
Although the redesigned front wheel performs better, the current structure still relies on plastic components that may loosen, deform, or wear over time during repeated use. Future work should evaluate whether a tighter mechanical design, a stronger filament material, or a different material altogether would provide better long-term rigidity and durability.

Another component that should be improved is the support that attaches the linear actuator to the robot. This part should be reprinted with higher infill and thicker side walls to increase its strength and reduce flex during operation. Future team members can assess whether these further changes are necessary based on long-term use and observed wear.

Fejemis Maploc Ros2

2026-05-28T10:29:39Z

S252724: /* Notes and Future Work */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

=== Mapping ===

''t_mapping'' currently checks for ''mission mapping/*'' and then returns to idle.

*Requires mission mapping/*

*Currently acts as a placeholder

*Does not yet launch the SLAM or exploration pipeline

*Returns to idle immediately

This branch should later be expanded to perform autonomous mapping, localization checks, and map saving.

=== 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.
[[File:cleaning.png| 800px]]

Cleanign Node 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 ==

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.
* Test and validate the complete cleaning behaviour structure in the final hardware setup, as it has not yet been fully evaluated due to current Raspberry Pi limitations.

Fejemis Maploc Ros2

2026-05-28T10:27:32Z

S252724: /* Cleaning */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

=== Mapping ===

''t_mapping'' currently checks for ''mission mapping/*'' and then returns to idle.

*Requires mission mapping/*

*Currently acts as a placeholder

*Does not yet launch the SLAM or exploration pipeline

*Returns to idle immediately

This branch should later be expanded to perform autonomous mapping, localization checks, and map saving.

=== 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.
[[File:cleaning.png| 800px]]

Cleanign Node 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 ==

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

File:Cleaning.png

2026-05-28T10:25:23Z

S252724:

Fejemis Maploc Ros2

2026-05-28T10:13:43Z

S252724: /* Mapping */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

=== Mapping ===

''t_mapping'' currently checks for ''mission mapping/*'' and then returns to idle.

*Requires mission mapping/*

*Currently acts as a placeholder

*Does not yet launch the SLAM or exploration pipeline

*Returns to idle immediately

This branch should later be expanded to perform autonomous mapping, localization checks, and map saving.

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

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

Fejemis Maploc Ros2

2026-05-28T10:11:17Z

S252724: /* Calibration */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

=== Calibration ===

''t_calib'' chooses between line calibration and square calibration.

Line calibration, ''t_calib_line'':

* Requires mission ''calib/line''.
* Drives forward using ''DriveOnHeading''
* ''calib_sq_dist'' is set to 0.05 m/s during initialization.
* 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.
Each ''f_calib_side'' contains:

*DriveOnHeading
*Spin
*A quarter-turn style calibration step
Calibration succeeds when each action in the sequence completes successfully.

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

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.

Fejemis Maploc Ros2

2026-05-28T10:09:23Z

S252724: /* Idle */

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.

=== 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:fejemis_maploc_ekf_block_diagram.png| 800px]]

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

=== launch/realsense.launch.py ===

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

Launch arguments:

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

Important RealSense settings:

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

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

=== launch/rtabmap_slam.launch.py ===

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

Launch arguments:

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

Sensor remappings:

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

Nodes:

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

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

=== launch/map.launch.py ===

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

Launch arguments:

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

The map server is configured with:

* ''topic_name=/loc/map''
* ''frame_id=map''
* ''yaml_filename=""''

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

=== launch/planning.launch.py ===

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

Launch arguments:

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

Nodes:

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

Important remappings:

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

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

The Nav2 configuration in ''config/nav2_servers.yaml'' uses:

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

=== launch/lifecycle.launch.py ===

Starts Nav2 lifecycle managers.

Launch arguments:

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

Lifecycle managers:

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

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

=== launch/lifecycle_managers_only.launch.py ===

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

Launch arguments:

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

Included launch files:

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

=== launch/behavior.launch.py ===

Starts the Fejemis behaviour tree engine from ''raubase_core''.

Launch argument:

* ''use_sim_time'' - forwarded to the BT engine.

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

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

The node launched is:

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

The BT plugin configuration loads:

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

=== launch/robot.launch.py inclusion ===

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

Typical distribution:

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

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

== Script Nodes ==

=== nav2_cmd_vel_deg_relay.py ===

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

Subscriptions:

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

Publications:

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

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

=== brush_control_service.py ===

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

Parameters:

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

Publications:

* ''manage_topic'' - publishes ''SteerManage.BRUSH_ON'' or ''SteerManage.BRUSH_OFF''.

Services:

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

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

* ''/control/brush/controller/on''
* ''/control/brush/controller/off''

=== rviz_goal_relay.py ===

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

Subscriptions:

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

Action clients:

* ''/fejemis/navigate_to_pose'' - ''nav2_msgs/action/NavigateToPose''.

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

=== mission_trigger.py ===

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

Parameters:

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

Publications:

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

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

<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. Sensors, telemetry, and subscriptions remain active, but no movement commands are issued. The robot leaves this state when a new mission is received through ''/behavior/set_mission''.

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

* Clarify which Raspberry Pi should run each launch file in the final deployment.
* Expand the cleaning behaviour section once ''t_cleaning'' is connected to ''f_get_area_to_clean'' and ''f_clean_area'' in the active tree.