cyclonedds config

Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>

.env

@@ -1 +1 @@
-PIONEER_ID=5
+PIONEER_ID=4

10-cyclone-max.conf

@@ -0,0 +1,6 @@
+# Increase the maximum receive buffer size for network packets
+net.core.rmem_max=2147483647  # 2 GiB, default is 208 KiB
+
+# IP fragmentation settings
+net.ipv4.ipfrag_time=3  # in seconds, default is 30 s
+net.ipv4.ipfrag_high_thresh=134217728  # 128 MiB, default is 256 KiB

Dockerfile.pioneer

@@ -0,0 +1,30 @@
+ARG ROS_DISTRO=jazzy
+FROM delicjusz/ros2aria:$ROS_DISTRO AS pkg-builder
+SHELL ["/bin/bash", "-c"]
+
+WORKDIR /ros2_ws
+COPY ./src /ros2_ws/src
+RUN apt update &&  apt install -y ros-dev-tools && source /opt/ros/$ROS_DISTRO/setup.bash && \
+    rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
+    rosdep init && \
+    rosdep update --rosdistro $ROS_DISTRO && \
+    rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+    colcon build
+
+FROM delicjusz/ros2aria:$ROS_DISTRO
+ARG ROS_DISTRO
+
+SHELL ["/bin/bash", "-c"]
+WORKDIR /ros2_ws
+
+COPY --from=pkg-builder /ros2_ws /ros2_ws
+RUN apt-get update && apt-get install -y ros-dev-tools  && \
+    rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
+    rosdep init && \
+    rosdep update --rosdistro $ROS_DISTRO && \
+    rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
+
+RUN apt-get clean && \
+	apt-get remove -y \
+		ros-dev-tools  && \
+	rm -rf /var/lib/apt/lists/*

Makefile

@@ -11,7 +11,7 @@ build:
 
 # .uploaded: build/ros2aria/ros2aria
 upload:
-	rsync -r  . lab1_5@pionier2:~/ros2aria
+	rsync -r  . lab1_5@pionier4:~/ros2aria
 	touch .uploaded
 
 # upload: .uploaded
@@ -25,13 +25,13 @@ legacy:
 
 push_ros2aria:
 	./build_devcontainer.sh
-	docker push delicjusz/ros2aria
+	docker push delicjusz/ros2aria:jazzy
 
 push:
-	docker build -f Dockerfile -t delicjusz/pioneer .
+	docker build -f Dockerfile.pioneer -t delicjusz/pioneer:jazzy .
-	docker push delicjusz/pioneer
+	docker push delicjusz/pioneer:jazzy
-	docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo .
+	docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo:jazzy .
-	docker push delicjusz/hokuyo
+	docker push delicjusz/hokuyo:jazzy
 pull:
 	docker pull delicjusz/ros2aria
 rosbridge:

README.md

@@ -1,4 +1,7 @@
 # Instruction on station
+
+## FastRTPS
+
 Export discovery server configuration file:
 ```bash
 export FASTRTPS_DEFAULT_PROFILES_FILE=$(pwd)/fastdds.xml
@@ -19,3 +22,16 @@ Restart ROS 2 daemon:
 ros2 daemon stop
 ros2 daemon start
 ```
+
+## Cyclonedds
+
+```
+export CYCLONEDDS_URI=file://$(pwd)/cyclonedds_pc.xml
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+```
+
+Restart ROS 2 daemon:
+```
+ros2 daemon stop
+ros2 daemon start
+```

build_devcontainer.sh

@@ -1,3 +1,3 @@
 #!/bin/sh
-docker build -f Dockerfile.aria -t delicjusz/ros2aria .
+docker build -f Dockerfile.aria -t delicjusz/ros2aria:jazzy .
 

compose.decawave.yaml

@@ -4,11 +4,14 @@ services:
    image: delicjusz/decawave
    network_mode: host
    ipc: host
+   restart: unless-stopped
    environment:
-    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+    - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+    - CYCLONEDDS_URI=file:///cyclonedds.xml
    volumes:
     - ./fastdds.xml:/fastdds.xml
+    - ./cyclonedds.xml:/cyclonedds.xml
    devices:
     - /dev/decawave
    command:  ros2 launch decawave_driver decawave.launch.py namespace:=pioneer${PIONEER_ID}/

compose.hokuyo.yaml

@@ -4,11 +4,14 @@ services:
    image: delicjusz/hokuyo
    network_mode: host
    ipc: host
+   restart: unless-stopped
    environment:
-    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+    - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+    - CYCLONEDDS_URI=file:///cyclonedds.xml
    volumes:
     - ./fastdds.xml:/fastdds.xml
+    - ./cyclonedds.xml:/cyclonedds.xml
    devices:
     - /dev/hokuyo
    command: >

compose.navigation2.yaml

@@ -0,0 +1,26 @@
+# docker compose -f compose.navigation2.yaml up
+
+services:
+  navigation:
+    image: husarion/navigation2:humble-1.1.16-20240927
+    network_mode: host
+    ipc: host
+    restart: unless-stopped
+    volumes:
+      - ./config/nav2_params.yaml:/nav2_params.yaml
+      - ./maps:/maps
+      - ./husarion_utils:/husarion_utils
+      - ./fastdds.xml:/fastdds.xml
+      - ./cyclonedds.xml:/cyclonedds.xml
+    environment:
+      - CYCLONEDDS_URI=file:///cyclonedds.xml
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+      - SAVE_MAP_PERIOD=${SAVE_MAP_PERIOD-10}
+    command: >
+      ros2 launch /husarion_utils/bringup_launch.py
+        namespace:=pioneer${PIONEER_ID}
+        observation_topic:=scan
+        map:=/maps/map.yaml
+        params_file:=/nav2_params.yaml
+        slam:=${SLAM:-False}
+        use_sim_time:=False

compose.pioneer.yaml

@@ -1,35 +0,0 @@
-# docker compose -f compose.ros2aria.yaml up
-services:
-  ros2aria-dev:
-   image: delicjusz/pioneer
-   network_mode: host
-   ipc: host
-   environment:
-    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-    - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
-    - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
-    - ROS_DISCOVERY_SERVER=10.104.16.240:11811
-   volumes:
-    - ./fastdds.xml:/fastdds.xml
-   devices:
-    - /dev/ttyS0
-   command:  ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/
-
-  hokuyo:
-   image: delicjusz/hokuyo
-   network_mode: host
-   ipc: host
-   environment:
-    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-    - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
-    - ROS_DISCOVERY_SERVER=10.104.16.240:11811
-   volumes:
-    - ./fastdds.xml:/fastdds.xml
-   devices:
-    # - /dev/hokuyo
-    - /dev/ttyACM0
-   command: >
-      ros2 run urg_node urg_node_driver  --ros-args -r 
-       __ns:=/pioneer${PIONEER_ID} -p
-      laser_frame_id:=pioneer${PIONEER_ID}/laser -p 
-      serial_port:=/dev/ttyACM0

compose.ros2aria.yaml

@@ -4,12 +4,15 @@ services:
    image: delicjusz/pioneer:humble-18012024
    network_mode: host
    ipc: host
+   restart: unless-stopped
    environment:
-    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
     - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
+    - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+    - CYCLONEDDS_URI=file:///cyclonedds.xml
    volumes:
     - ./fastdds.xml:/fastdds.xml
+    - ./cyclonedds.xml:/cyclonedds.xml
    devices:
     - /dev/ttyS0
    command:  ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/

config/nav2_params.yaml

@@ -0,0 +1,456 @@
+---
+### NAVIGATION ###
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: false
+    global_frame: <robot_namespace>/map
+    robot_base_frame: <robot_namespace>/base_link
+    odom_topic: /<robot_namespace>/odom
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+controller_server:
+  ros__parameters:
+    use_sim_time: false
+
+    controller_frequency: 10.0
+    odom_topic: /<robot_namespace>/odom
+
+    # if velocity is below threshold value it is set to 0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+
+    progress_checker_plugin: progress_checker
+    progress_checker:
+      plugin: nav2_controller::SimpleProgressChecker
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    goal_checker_plugin: goal_checker
+    goal_checker:
+      plugin: nav2_controller::SimpleGoalChecker
+      xy_goal_tolerance: 0.1
+      yaw_goal_tolerance: 0.25
+      stateful: true # if stateful is true goal checker will not check if the xy position matches again once it is found to be true.
+
+    controller_plugins: [FollowPath]
+    FollowPath:
+      # Shim controller first rotates robot in place to orientation of global path
+      # and the switches to DWB - otherwise DWB won't turn in place, but with arc, which
+      # is less optimal
+      plugin: nav2_rotation_shim_controller::RotationShimController
+      primary_controller: dwb_core::DWBLocalPlanner
+      angular_dist_threshold: 0.4 # quite low value to avoid moving in arc
+      forward_sampling_distance: 0.5
+      rotate_to_heading_angular_vel: 1.5
+      # I'm not sure why, but ang vel is limited max_angular_accel param divided by 10
+      # actual acceleration will be limited by controller, so it can be set to higher value
+      max_angular_accel: 10.0
+      simulate_ahead_time: 1.0
+
+      debug_trajectory_details: true
+
+      # Velocity/acceleration limits also have to be adjusted in the velocity smoother
+      min_vel_x: -0.25
+      max_vel_x: 0.5
+      acc_lim_x: 1.0
+      decel_lim_x: -1.0
+      min_speed_xy: 0.007
+      max_speed_xy: 0.5
+
+      max_vel_theta: 1.5
+      min_speed_theta: 0.4
+      acc_lim_theta: 4.0
+      decel_lim_theta: -4.0
+
+      min_vel_y: 0.0
+      max_vel_y: 0.0
+      acc_lim_y: 0.0
+      decel_lim_y: 0.0
+
+      vx_samples: 20
+      vy_samples: 0
+      vtheta_samples: 20
+
+      transform_tolerance: 0.2
+      short_circuit_trajectory_evaluation: true
+
+      trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator
+      sim_time: 0.75 # low value to avoid slow movement near goal
+      discretize_by_time: false
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+
+      # Critics used for scoring trajectories created by trajectory generator
+      # Final score is a sum of critics' score
+      critics: [BaseObstacle, ObstacleFootprint, GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal,
+        Oscillation]
+
+      BaseObstacle.scale: 0.02
+
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      GoalDist.scale: 24.0
+
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+
+      xy_goal_tolerance: 0.1
+      trans_stopped_velocity: 0.25
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: false
+
+      global_frame: <robot_namespace>/odom
+      robot_base_frame: <robot_namespace>/base_link
+
+      update_frequency: 5.0
+      publish_frequency: 2.0
+
+      width: 5
+      height: 5
+      resolution: 0.05
+
+      always_send_full_costmap: true
+      rolling_window: true
+
+      transform_tolerance: 0.1
+      footprint: '[[0.45, 0.47], [0.45, -0.47], [-0.45, -0.47], [-0.45, 0.47]]'
+
+      plugins: [obstacle_layer, inflation_layer]
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /<robot_namespace>/scan
+          max_obstacle_height: 2.0
+          clearing: true
+          marking: true
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        enabled: true
+        cost_scaling_factor: 1.3
+        inflation_radius: 1.5
+
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: false
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: false
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: false
+
+      global_frame: <robot_namespace>/map
+      robot_base_frame: <robot_namespace>/base_link
+
+      update_frequency: 1.0
+      publish_frequency: 1.0
+
+      resolution: 0.05
+
+      always_send_full_costmap: true
+      track_unknown_space: true # if false, treats unknown space as free space, else as unknown space
+
+      transform_tolerance: 0.1
+      footprint: '[[0.45, 0.47], [0.45, -0.47], [-0.45, -0.47], [-0.45, 0.47]]'
+
+      plugins: [static_layer, obstacle_layer, inflation_layer]
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        enabled: true
+        map_subscribe_transient_local: true
+        map_topic: /<robot_namespace>/map
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /<robot_namespace>/scan
+          max_obstacle_height: 2.0
+          clearing: true
+          marking: true
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        enabled: true
+        cost_scaling_factor: 1.5
+        inflation_radius: 1.5
+
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: false
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: false
+
+planner_server:
+  ros__parameters:
+    use_sim_time: false
+
+    planner_plugins: [GridBased]
+    GridBased:
+      plugin: nav2_smac_planner/SmacPlanner2D
+      tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters
+      downsample_costmap: false # whether or not to downsample the map
+      downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
+      allow_unknown: true # allow traveling in unknown space
+      max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
+      max_planning_time: 2.0 # max time in s for planner to plan, smooth
+      motion_model_for_search: MOORE   # 2D Moore, Von Neumann
+      cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+      use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+recoveries_server:
+  ros__parameters:
+    use_sim_time: false
+
+    global_frame: <robot_namespace>/odom
+    robot_base_frame: <robot_namespace>/base_link
+    transform_tolerance: 0.1
+
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+
+    recovery_plugins: [spin, backup, wait]
+    spin:
+      plugin: nav2_recoveries/Spin
+    backup:
+      plugin: nav2_recoveries/BackUp
+    wait:
+      plugin: nav2_recoveries/Wait
+
+    # spin & backup
+    simulate_ahead_time: 2.0
+
+    # spin
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+behavior_server:
+  ros__parameters:
+    use_sim_time: false
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: false
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: wait_at_waypoint
+    wait_at_waypoint:
+      plugin: nav2_waypoint_follower::WaitAtWaypoint
+      enabled: true
+      waypoint_pause_duration: 200
+
+### LOCALIZATION ###
+
+amcl:
+  ros__parameters:
+    use_sim_time: false
+
+    global_frame_id: <robot_namespace>/map
+    odom_frame_id: <robot_namespace>/odom
+    base_frame_id: <robot_namespace>/base_link
+
+    scan_topic: /<robot_namespace>/scan
+
+    robot_model_type: nav2_amcl::DifferentialMotionModel
+
+    set_initial_pose: true
+    always_reset_initial_pose: true
+    initial_pose:
+      x: 0.0
+      y: 0.0
+      yaw: 0.0
+
+    tf_broadcast: true
+    transform_tolerance: 1.0
+
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+
+    # Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
+    # prevents correct particles from getting down weighted because of unexpected obstacles
+    # such as humans
+    do_beamskip: false
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+
+    lambda_short: 0.1
+
+    laser_model_type: likelihood_field
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 12.0
+    laser_min_range: -1.0
+
+    max_beams: 60
+
+    max_particles: 2000
+    min_particles: 500
+
+    pf_err: 0.05
+    pf_z: 0.99
+
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+
+    resample_interval: 1
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+
+    update_min_a: 0.1
+    update_min_d: 0.15
+
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: false
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+### MAP SERVER ###
+
+map_server:
+  ros__parameters:
+    use_sim_time: false
+    yaml_filename: map.yaml
+    frame_id: <robot_namespace>/map
+
+### SLAM ###
+
+<robot_namespace>/slam_toolbox:
+  ros__parameters:
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None #HuberLoss
+
+    # ROS Parameters
+    odom_frame: <robot_namespace>/odom
+    map_frame: <robot_namespace>/map
+    base_frame: <robot_namespace>/base_link
+    scan_topic: /<robot_namespace>/scan
+    mode: mapping #localization
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.04
+    map_update_interval: 1.0
+    resolution: 0.1
+    max_laser_range: 12.0 #for rastering images
+    minimum_time_interval: 0.05
+    transform_timeout: 0.5
+    tf_buffer_duration: 20.0
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: false
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.2
+    minimum_travel_heading: 0.1
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 0.5
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 0.75
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+
+    use_sim_time: false

cyclonedds.xml

@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+  <Domain Id="any">
+    <General>
+      <Interfaces>
+        <NetworkInterface autodetermine="false" name="wlp1s0" priority="default" multicast="default" />
+      </Interfaces>
+      <AllowMulticast>false</AllowMulticast>
+      <EnableMulticastLoopback>false</EnableMulticastLoopback>
+      <MaxMessageSize>65500B</MaxMessageSize>
+    </General>
+    <Internal>
+        <SocketReceiveBufferSize min="10MB"/>
+        <Watermarks>
+                <WhcHigh>500kB</WhcHigh>
+        </Watermarks>
+    </Internal>
+    <Discovery>
+        <ParticipantIndex>auto</ParticipantIndex>
+        <Peers>
+            <Peer address="lab15-1"/>
+            <Peer address="lab15-2"/>
+            <Peer address="lab15-3"/>
+            <Peer address="lab15-4"/>
+            <Peer address="lab15-5"/>
+            <Peer address="lab15-6"/>
+            <Peer address="lab15-7"/>
+            <Peer address="lab15-8"/>
+            <Peer address="lab15-9"/>
+            <Peer address="lab15-10"/>
+            <Peer address="lab15-11"/>
+            <Peer address="lab15-12"/>
+            <Peer address="lab15-13"/>
+            <Peer address="lab15-14"/>
+            <Peer address="lab15-15"/>
+            <Peer address="lab15-16"/>
+            <Peer address="lab15-17"/>
+            <Peer address="lab15-18"/>
+            <Peer address="lab15-19"/>
+            <Peer address="lab15-20"/>
+
+            <Peer address="pionier1"/>
+            <Peer address="pionier2"/>
+            <Peer address="pionier3"/>
+            <Peer address="pionier4"/>
+            <Peer address="pionier5"/>
+            <Peer address="pionier6"/>
+        </Peers>
+    </Discovery>
+  </Domain>
+</CycloneDDS>

cyclonedds_pc.xml

@@ -0,0 +1,48 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+  <Domain Id="any">
+    <General>
+      <NetworkInterfaceAddress>eno1</NetworkInterfaceAddress>
+      <AllowMulticast>false</AllowMulticast>
+      <EnableMulticastLoopback>false</EnableMulticastLoopback>
+      <MaxMessageSize>65500B</MaxMessageSize>
+    </General>
+    <Internal>
+        <Watermarks>
+                <WhcHigh>500kB</WhcHigh>
+        </Watermarks>
+    </Internal>
+    <Discovery>
+        <ParticipantIndex>auto</ParticipantIndex>
+        <Peers>
+            <Peer address="lab15-1"/>
+            <Peer address="lab15-2"/>
+            <Peer address="lab15-3"/>
+            <Peer address="lab15-4"/>
+            <Peer address="lab15-5"/>
+            <Peer address="lab15-6"/>
+            <Peer address="lab15-7"/>
+            <Peer address="lab15-8"/>
+            <Peer address="lab15-9"/>
+            <Peer address="lab15-10"/>
+            <Peer address="lab15-11"/>
+            <Peer address="lab15-12"/>
+            <Peer address="lab15-13"/>
+            <Peer address="lab15-14"/>
+            <Peer address="lab15-15"/>
+            <Peer address="lab15-16"/>
+            <Peer address="lab15-17"/>
+            <Peer address="lab15-18"/>
+            <Peer address="lab15-19"/>
+            <Peer address="lab15-20"/>
+
+            <Peer address="pionier1"/>
+            <Peer address="pionier2"/>
+            <Peer address="pionier3"/>
+            <Peer address="pionier4"/>
+            <Peer address="pionier5"/>
+            <Peer address="pionier6"/>
+        </Peers>
+    </Discovery>
+  </Domain>
+</CycloneDDS>

docker-compose@.service

@@ -7,8 +7,11 @@ After=docker.service
 Type=oneshot
 RemainAfterExit=true
 WorkingDirectory=/etc/docker/compose/%i
+ExecStartPre= /bin/sh -c 'until ping -c1 denali; do sleep 1; done;'
 ExecStart=/usr/bin/docker compose up -d --remove-orphans
 ExecStop=/usr/bin/docker compose down
+Restart=on-failure
+RestartSec=5s
 
 [Install]
 WantedBy=multi-user.target

husarion_utils/bringup_launch.py

@@ -0,0 +1,184 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.actions import PushRosNamespace
+from launch_ros.descriptions import ParameterFile
+from nav2_common.launch import RewrittenYaml, ReplaceString
+
+
+def launch_setup(context, *args, **kwargs):
+    # Get the launch directory
+    launch_dir = "/husarion_utils"
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration("namespace").perform(context)
+    observation_topic = LaunchConfiguration("observation_topic").perform(context)
+    map_yaml_file = LaunchConfiguration("map").perform(context)
+    use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
+    params_file = LaunchConfiguration("params_file").perform(context)
+    slam = LaunchConfiguration("slam").perform(context)
+    autostart = LaunchConfiguration("autostart").perform(context)
+    use_composition = LaunchConfiguration("use_composition").perform(context)
+    use_respawn = LaunchConfiguration("use_respawn").perform(context)
+    log_level = LaunchConfiguration("log_level").perform(context)
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file}
+
+    if namespace:
+        params_file = ReplaceString(
+            source_file=params_file, replacements={"<robot_namespace>": namespace}
+        )
+    else:
+        params_file = ReplaceString(
+            source_file=params_file, replacements={"<robot_namespace>/": ""}
+        )
+
+    params_file = ReplaceString(
+        source_file=params_file, replacements={"<observation_topic>": observation_topic}
+    )
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    # Specify the actions
+    bringup_cmd_group = GroupAction(
+        [
+            PushRosNamespace(namespace),
+            Node(
+                condition=IfCondition(use_composition),
+                name="nav2_container",
+                package="rclcpp_components",
+                executable="component_container_isolated",
+                parameters=[configured_params, {"autostart": autostart}],
+                arguments=["--ros-args", "--log-level", log_level],
+                output="screen",
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")),
+                condition=IfCondition(slam),
+                launch_arguments={
+                    "namespace": namespace,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "use_respawn": use_respawn,
+                    "params_file": params_file,
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "localization_launch.py")),
+                condition=IfCondition(PythonExpression(["not ", slam])),
+                launch_arguments={
+                    "namespace": namespace,
+                    "map": map_yaml_file,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_composition": use_composition,
+                    "use_respawn": use_respawn,
+                    "container_name": "nav2_container",
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")),
+                launch_arguments={
+                    "namespace": namespace,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_composition": use_composition,
+                    "use_respawn": use_respawn,
+                    "container_name": "nav2_container",
+                }.items(),
+            ),
+        ]
+    )
+
+    healthcheck_node = Node(
+        package="healthcheck_pkg",
+        executable="healthcheck_node",
+        name="healthcheck_navigation",
+        namespace=namespace,
+        output="screen",
+    )
+
+    return [bringup_cmd_group, healthcheck_node]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
+            DeclareLaunchArgument(
+                "namespace",
+                default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+                description="Top-level namespace",
+            ),
+            DeclareLaunchArgument(
+                "observation_topic",
+                default_value="",
+                description="Topic name for PointCloud2 messages.",
+            ),
+            DeclareLaunchArgument("slam", default_value="False", description="Whether run a SLAM"),
+            DeclareLaunchArgument("map", description="Full path to map yaml file to load"),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation (Gazebo) clock if true",
+            ),
+            DeclareLaunchArgument(
+                "params_file",
+                default_value=os.path.join("/params.yaml"),
+                description="Full path to the ROS2 parameters file to use for all launched nodes",
+            ),
+            DeclareLaunchArgument(
+                "autostart",
+                default_value="true",
+                description="Automatically startup the nav2 stack",
+            ),
+            DeclareLaunchArgument(
+                "use_composition",
+                default_value="True",
+                description="Whether to use composed bringup",
+            ),
+            DeclareLaunchArgument(
+                "use_respawn",
+                default_value="False",
+                description="Whether to respawn if a node crashes. Applied when composition is disabled.",
+            ),
+            DeclareLaunchArgument("log_level", default_value="info", description="log level"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

husarion_utils/localization_launch.py

@@ -0,0 +1,193 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("nav2_bringup")
+
+    namespace = LaunchConfiguration("namespace")
+    map_yaml_file = LaunchConfiguration("map")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    autostart = LaunchConfiguration("autostart")
+    params_file = LaunchConfiguration("params_file")
+    use_composition = LaunchConfiguration("use_composition")
+    container_name = LaunchConfiguration("container_name")
+    container_name_full = (namespace, "/", container_name)
+    use_respawn = LaunchConfiguration("use_respawn")
+    log_level = LaunchConfiguration("log_level")
+
+    lifecycle_nodes = ["map_server", "amcl"]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1")
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace", default_value="", description="Top-level namespace"
+    )
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        "map", description="Full path to map yaml file to load"
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true"
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart", default_value="true", description="Automatically startup the nav2 stack"
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        "use_composition", default_value="False", description="Use composed bringup if True"
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        "container_name",
+        default_value="nav2_container",
+        description="the name of container that nodes will load in if use composition",
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        "use_respawn",
+        default_value="False",
+        description="Whether to respawn if a node crashes. Applied when composition is disabled.",
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        "log_level", default_value="info", description="log level"
+    )
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(["not ", use_composition])),
+        actions=[
+            Node(
+                package="nav2_map_server",
+                executable="map_server",
+                name="map_server",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_amcl",
+                executable="amcl",
+                name="amcl",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_lifecycle_manager",
+                executable="lifecycle_manager",
+                name="lifecycle_manager_localization",
+                output="screen",
+                arguments=["--ros-args", "--log-level", log_level],
+                parameters=[
+                    {"use_sim_time": use_sim_time},
+                    {"autostart": autostart},
+                    {"node_names": lifecycle_nodes},
+                ],
+            ),
+        ],
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name_full,
+        composable_node_descriptions=[
+            ComposableNode(
+                package="nav2_map_server",
+                plugin="nav2_map_server::MapServer",
+                name="map_server",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_amcl",
+                plugin="nav2_amcl::AmclNode",
+                name="amcl",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_lifecycle_manager",
+                plugin="nav2_lifecycle_manager::LifecycleManager",
+                name="lifecycle_manager_localization",
+                parameters=[
+                    {
+                        "use_sim_time": use_sim_time,
+                        "autostart": autostart,
+                        "node_names": lifecycle_nodes,
+                    }
+                ],
+            ),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Add the actions to launch all of the localiztion nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

husarion_utils/navigation_launch.py

@@ -0,0 +1,279 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("nav2_bringup")
+
+    namespace = LaunchConfiguration("namespace")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    autostart = LaunchConfiguration("autostart")
+    params_file = LaunchConfiguration("params_file")
+    use_composition = LaunchConfiguration("use_composition")
+    container_name = LaunchConfiguration("container_name")
+    container_name_full = (namespace, "/", container_name)
+    use_respawn = LaunchConfiguration("use_respawn")
+    log_level = LaunchConfiguration("log_level")
+
+    lifecycle_nodes = [
+        "controller_server",
+        "smoother_server",
+        "planner_server",
+        "behavior_server",
+        "bt_navigator",
+        "waypoint_follower",
+        "velocity_smoother",
+    ]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {"use_sim_time": use_sim_time, "autostart": autostart}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1")
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Top-level namespace",
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true"
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart", default_value="true", description="Automatically startup the nav2 stack"
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        "use_composition", default_value="False", description="Use composed bringup if True"
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        "container_name",
+        default_value="nav2_container",
+        description="the name of container that nodes will load in if use composition",
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        "use_respawn",
+        default_value="False",
+        description="Whether to respawn if a node crashes. Applied when composition is disabled.",
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        "log_level", default_value="info", description="log level"
+    )
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(["not ", use_composition])),
+        actions=[
+            Node(
+                package="nav2_controller",
+                executable="controller_server",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+                remappings=[("cmd_vel", "cmd_vel_nav")],
+            ),
+            Node(
+                package="nav2_smoother",
+                executable="smoother_server",
+                name="smoother_server",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_planner",
+                executable="planner_server",
+                name="planner_server",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_behaviors",
+                executable="behavior_server",
+                name="behavior_server",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_bt_navigator",
+                executable="bt_navigator",
+                name="bt_navigator",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_waypoint_follower",
+                executable="waypoint_follower",
+                name="waypoint_follower",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+            ),
+            Node(
+                package="nav2_velocity_smoother",
+                executable="velocity_smoother",
+                name="velocity_smoother",
+                output="screen",
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=["--ros-args", "--log-level", log_level],
+                remappings=[("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")],
+            ),
+            Node(
+                package="nav2_lifecycle_manager",
+                executable="lifecycle_manager",
+                name="lifecycle_manager_navigation",
+                output="screen",
+                arguments=["--ros-args", "--log-level", log_level],
+                parameters=[
+                    {"use_sim_time": use_sim_time},
+                    {"autostart": autostart},
+                    {"node_names": lifecycle_nodes},
+                ],
+            ),
+        ],
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name_full,
+        composable_node_descriptions=[
+            ComposableNode(
+                package="nav2_controller",
+                plugin="nav2_controller::ControllerServer",
+                name="controller_server",
+                parameters=[configured_params],
+                remappings=[("cmd_vel", "cmd_vel_nav")],
+            ),
+            ComposableNode(
+                package="nav2_smoother",
+                plugin="nav2_smoother::SmootherServer",
+                name="smoother_server",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_planner",
+                plugin="nav2_planner::PlannerServer",
+                name="planner_server",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_behaviors",
+                plugin="behavior_server::BehaviorServer",
+                name="behavior_server",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_bt_navigator",
+                plugin="nav2_bt_navigator::BtNavigator",
+                name="bt_navigator",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_waypoint_follower",
+                plugin="nav2_waypoint_follower::WaypointFollower",
+                name="waypoint_follower",
+                parameters=[configured_params],
+            ),
+            ComposableNode(
+                package="nav2_velocity_smoother",
+                plugin="nav2_velocity_smoother::VelocitySmoother",
+                name="velocity_smoother",
+                parameters=[configured_params],
+                remappings=[("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")],
+            ),
+            ComposableNode(
+                package="nav2_lifecycle_manager",
+                plugin="nav2_lifecycle_manager::LifecycleManager",
+                name="lifecycle_manager_navigation",
+                parameters=[
+                    {
+                        "use_sim_time": use_sim_time,
+                        "autostart": autostart,
+                        "node_names": lifecycle_nodes,
+                    }
+                ],
+            ),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

husarion_utils/slam_launch.py

@@ -0,0 +1,145 @@
+# Copyright (c) 2020 Samsung Research Russia
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, SetRemap
+from launch_ros.descriptions import ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Input parameters declaration
+    namespace = LaunchConfiguration("namespace")
+    params_file = LaunchConfiguration("params_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    autostart = LaunchConfiguration("autostart")
+    use_respawn = LaunchConfiguration("use_respawn")
+    log_level = LaunchConfiguration("log_level")
+
+    # Variables
+    lifecycle_nodes = ["map_saver"]
+
+    # Getting directories and launch-files
+    slam_toolbox_dir = get_package_share_directory("slam_toolbox")
+    slam_launch_file = os.path.join(slam_toolbox_dir, "launch", "online_sync_launch.py")
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {"use_sim_time": use_sim_time}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        "namespace", default_value="robot", description="Top-level namespace"
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value="/params.yaml",
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true"
+    )
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        "autostart", default_value="True", description="Automatically startup the nav2 stack"
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        "use_respawn",
+        default_value="False",
+        description="Whether to respawn if a node crashes. Applied when composition is disabled.",
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        "log_level", default_value="info", description="log level"
+    )
+
+    # Nodes launching commands
+
+    start_map_saver_server_cmd = Node(
+        package="nav2_map_server",
+        executable="map_saver_server",
+        output="screen",
+        respawn=use_respawn,
+        respawn_delay=2.0,
+        arguments=["--ros-args", "--log-level", log_level],
+        parameters=[configured_params],
+    )
+
+    start_lifecycle_manager_cmd = Node(
+        package="nav2_lifecycle_manager",
+        executable="lifecycle_manager",
+        name="lifecycle_manager_slam",
+        output="screen",
+        arguments=["--ros-args", "--log-level", log_level],
+        parameters=[
+            {"use_sim_time": use_sim_time},
+            {"autostart": autostart},
+            {"node_names": lifecycle_nodes},
+        ],
+    )
+
+    start_slam_toolbox_cmd_with_params = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(slam_launch_file),
+        launch_arguments={"use_sim_time": use_sim_time, "slam_params_file": params_file}.items(),
+    )
+
+    group_actions = []
+
+    # Remapping
+    group_actions.append(SetRemap("/map", "map"))
+    group_actions.append(SetRemap("/map_metadata", "map_metadata"))
+    group_actions.append(SetRemap("/scan", "scan"))
+
+    # Running Map Saver Server
+    group_actions.append(start_map_saver_server_cmd)
+    group_actions.append(start_lifecycle_manager_cmd)
+
+    # Running SLAM Toolbox (Only one of them will be run)
+    group_actions.append(start_slam_toolbox_cmd_with_params)
+
+    slam_ns = GroupAction(actions=group_actions)
+
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Running SLAM
+    ld.add_action(slam_ns)
+
+    return ld

install.sh

@@ -0,0 +1,45 @@
+#!/bin/bash
+
+# https://autowarefoundation.github.io/autoware-documentation/main/installation/additional-settings-for-developers/network-configuration/dds-settings/
+# Check if root
+if [ "$EUID" -ne 0 ]
+        then echo "Please run as root"
+        exit
+fi
+
+cp docker-compose@.service /etc/systemd/system/
+
+mkdir -p /etc/docker/compose/ros2aria/
+mkdir -p /etc/docker/compose/hokuyo/
+mkdir -p /etc/docker/compose/decawave/
+mkdir -p /etc/docker/compose/navigation2/
+
+cp compose.ros2aria.yaml /etc/docker/compose/ros2aria/compose.yaml
+cp compose.hokuyo.yaml   /etc/docker/compose/hokuyo/compose.yaml
+cp compose.decawave.yaml /etc/docker/compose/decawave/compose.yaml
+
+cp compose.navigation2.yaml /etc/docker/compose/navigation2/compose.yaml
+
+
+
+cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/ros2aria/
+cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/hokuyo/
+cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/decawave/
+cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/navigation2/
+
+cp -r husarion_utils config maps /etc/docker/compose/navigation2/
+
+cp 10-cyclone-max.conf /etc/sysctl.d/10-cyclone-max.conf
+
+sysctl --system | grep 10-cyclone-max.conf
+systemctl daemon-reload
+
+systemctl enable docker-compose@ros2aria.service
+systemctl enable docker-compose@hokuyo.service
+systemctl disable docker-compose@navigation2.service
+systemctl enable docker-compose@decawave.service
+
+systemctl restart docker-compose@ros2aria.service
+systemctl restart docker-compose@hokuyo.service
+systemctl stop docker-compose@navigation2.service
+systemctl restart docker-compose@decawave.service

pioneer_status.py

@@ -0,0 +1,136 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+import time
+import sys
+from PyQt5.QtWidgets import QApplication, QWidget, QLabel, QGridLayout, QVBoxLayout, QMainWindow
+from PyQt5.QtCore import QTimer, Qt
+
+class PioneerStatusNode(Node):
+    def __init__(self, gui):
+        super().__init__('pioneer_status_node')
+        self.gui = gui  # Reference to the GUI
+        self.topic_data = {}
+
+        # Subscribing to topics for pioneers 1-6
+        for i in range(1, 7):
+            for topic_type, msg_type in [("odom", Odometry), ("joint_states", JointState), ("scan", LaserScan)]:
+                topic_name = f'/pioneer{i}/{topic_type}'
+                self.topic_data[topic_name] = {"last_message_time": 0, "message_count": 0}
+                self.create_subscription(msg_type, topic_name, lambda msg, topic=topic_name: self.message_callback(msg, topic), 10)
+
+    def message_callback(self, msg, topic_name):
+        # Update the last received time and message count for each topic
+        self.topic_data[topic_name]["last_message_time"] = time.time()
+        self.topic_data[topic_name]["message_count"] += 1
+
+    def update_gui(self):
+        current_time = time.time()
+        
+        for topic_name, data in self.topic_data.items():
+            # Calculate frequency
+            time_diff = current_time - data["last_message_time"]
+            frequency = data["message_count"] / time_diff if time_diff > 0 else 0
+            
+            # Determine status (active if received within last 2 seconds)
+            status = "Active" if time_diff < 2 else "Inactive"
+            
+            # Update the GUI labels for this topic
+            self.gui.update_status(topic_name, frequency, status)
+            
+            # Reset message count to calculate frequency anew in the next interval
+            data["message_count"] = 0
+
+class PioneerStatusGUI(QMainWindow):
+    def __init__(self):
+        super().__init__()
+        
+        self.setWindowTitle("Pioneer Status Monitor")
+        self.setGeometry(100, 100, 600, 400)
+        
+        # Main layout and widget setup
+        central_widget = QWidget()
+        self.setCentralWidget(central_widget)
+        layout = QVBoxLayout()
+        central_widget.setLayout(layout)
+        
+        self.grid = QGridLayout()
+        layout.addLayout(self.grid)
+
+        # Header labels
+        self.grid.addWidget(QLabel("Topic"), 0, 0, alignment=Qt.AlignCenter)
+        self.grid.addWidget(QLabel("Frequency (Hz)"), 0, 1, alignment=Qt.AlignCenter)
+        self.grid.addWidget(QLabel("Status"), 0, 2, alignment=Qt.AlignCenter)
+        
+        # Dictionary to store labels for each topic's frequency and status
+        self.topic_labels = {}
+        
+        # Populate rows with placeholders for each topic
+        row = 1
+        for i in range(1, 7):
+            for topic_type in ["odom", "joint_states", "scan"]:
+                topic_name = f'/pioneer{i}/{topic_type}'
+                
+                # Topic name label
+                self.grid.addWidget(QLabel(topic_name), row, 0)
+                
+                # Frequency label
+                freq_label = QLabel("0.00")
+                freq_label.setAlignment(Qt.AlignCenter)
+                self.grid.addWidget(freq_label, row, 1)
+                
+                # Status label
+                status_label = QLabel("Inactive")
+                status_label.setAlignment(Qt.AlignCenter)
+                status_label.setStyleSheet("color: red;")
+                self.grid.addWidget(status_label, row, 2)
+                
+                # Store references to labels for updates
+                self.topic_labels[topic_name] = {
+                    "frequency": freq_label,
+                    "status": status_label
+                }
+                row += 1
+
+    def update_status(self, topic_name, frequency, status):
+        # Update frequency label
+        if topic_name in self.topic_labels:
+            self.topic_labels[topic_name]["frequency"].setText(f"{frequency:.2f}")
+            
+            # Update status label color and text based on status
+            status_label = self.topic_labels[topic_name]["status"]
+            status_label.setText(status)
+            status_label.setStyleSheet("color: green;" if status == "Active" else "color: red;")
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    # PyQt5 application setup
+    app = QApplication(sys.argv)
+    gui = PioneerStatusGUI()
+    gui.show()
+
+    # Create the ROS 2 node
+    node = PioneerStatusNode(gui)
+    
+    # QTimer to call rclpy.spin_once periodically
+    timer = QTimer()
+    timer.timeout.connect(lambda: rclpy.spin_once(node, timeout_sec=0.1))
+    timer.start(100)  # Check for ROS messages every 100 ms
+
+    # QTimer to update the GUI every second
+    update_timer = QTimer()
+    update_timer.timeout.connect(node.update_gui)
+    update_timer.start(500)  # Update GUI every 1 second
+
+    # Run the PyQt application
+    try:
+        sys.exit(app.exec_())
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()