+x

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

.devcontainer/devcontainer.json

@@ -6,7 +6,7 @@
     //         "WORKSPACE": "${containerWorkspaceFolder}"
     //     }
     // },
-    "image": "irth7/ros2aria-dev",
+    "image": "delicjusz/ros2aria",
     "remoteUser": "ros",
     "runArgs": [
         "--network=host",

.env

@@ -0,0 +1 @@
+PIONEER_ID=4

.vscode/c_cpp_properties.json

@@ -10,9 +10,11 @@
                 "/workspaces/ros2aria/src/ros2aria/include/**",
                 "/workspaces/ros2aria/install/ros2aria_msgs/include/**",
                 "/usr/include/**",
-        "/usr/local/Aria/include"
+                "/usr/local/Aria/include",
+                "${workspaceFolder}/src/ros2aria/include/**"
             ],
-      "name": "ROS"
+            "name": "ROS",
+            "configurationProvider": "ms-vscode.cmake-tools"
         }
     ],
     "version": 4

.vscode/settings.json

@@ -61,9 +61,34 @@
         "thread": "cpp",
         "cinttypes": "cpp",
         "typeindex": "cpp",
-        "typeinfo": "cpp"
+        "typeinfo": "cpp",
+        "any": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "forward_list": "cpp",
+        "unordered_set": "cpp",
+        "source_location": "cpp",
+        "iomanip": "cpp",
+        "numbers": "cpp",
+        "semaphore": "cpp",
+        "stop_token": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "regex": "cpp",
+        "ranges": "cpp"
     },
     "cSpell.words": [
         "rclcpp"
-    ]
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/foxy/lib/python3.8/site-packages"
+    ],
+    "ros.distro": "humble",
+    "cmake.sourceDirectory": "/home/jdelicat/lab_repos/ros2aria/"
 }

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

@@ -0,0 +1,19 @@
+ARG ROS_DISTRO=humble
+FROM husarnet/ros:humble-ros-core
+ARG ROS_DISTRO
+
+SHELL ["/bin/bash", "-c"]
+
+RUN mkdir -p /ros2_ws/src && cd /ros2_ws/src
+WORKDIR /ros2_ws
+
+RUN apt-get update && apt-get install -y \
+		build-essential
+
+COPY ./src/AriaCoda /usr/local/Aria
+RUN cd /usr/local/Aria && make -j$(nproc) && export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria
+	# clear ubuntu packages
+RUN	apt-get clean && \
+	apt-get remove -y \
+		build-essential && \
+	rm -rf /var/lib/apt/lists/*

Dockerfile.hokuyo

@@ -0,0 +1,7 @@
+ARG ROS_DISTRO=humble
+FROM husarnet/ros:humble-ros-core
+SHELL ["/bin/bash", "-c"]
+RUN apt-get update && apt-get install -y \
+		ros-$ROS_DISTRO-urg-node
+
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc

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

@@ -6,17 +6,34 @@ HEADERS := $(wildcard src/ros2aria/src/*.hpp)
 build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
 	colcon build
 
-build: build/ros2aria/ros2aria
+build: 
+	docker build -f Dockerfile -t delicjusz/pioneer .
 
 # .uploaded: build/ros2aria/ros2aria
 upload:
-	rsync -r  . lab1_5@pionier6:~/ros2aria
+	rsync -r  . lab1_5@pionier4:~/ros2aria
 	touch .uploaded
 
 # upload: .uploaded
 
 run: upload
-	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
+	ssh lab1_5@pionier2 -t -- docker run --rm --network=host -it --device /dev/ttyS0  delicjusz/ros2aria /bin/bash /ros2_ws/ros2aria/run.sh
+# export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=2
 
 legacy:
-	ssh lab1_5@pionier6 -t -- ./run.sh
+	ssh lab1_5@pionier2 -t -- ./run.sh
+
+push_ros2aria:
+	./build_devcontainer.sh
+	docker push delicjusz/ros2aria:jazzy
+
+push:
+	docker build -f Dockerfile.pioneer -t delicjusz/pioneer:jazzy .
+	docker push delicjusz/pioneer:jazzy
+	docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo:jazzy .
+	docker push delicjusz/hokuyo:jazzy
+pull:
+	docker pull delicjusz/ros2aria
+rosbridge:
+	docker build -f Dockerfile.rosbridge -t delicjusz/rosbridge .
+	docker push delicjusz/rosbridge

README.md

@@ -0,0 +1,37 @@
+# Instruction on station
+
+## FastRTPS
+
+Export discovery server configuration file:
+```bash
+export FASTRTPS_DEFAULT_PROFILES_FILE=$(pwd)/fastdds.xml
+```
+
+Export discovery server ip:
+```
+export ROS_DISCOVERY_SERVER=10.104.16.240:11811
+```
+
+Export fastdds ros middleware implementation:
+```
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+```
+
+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 --build-arg WORKSPACE=/ws -f Dockerfile.devcontainer -t irth7/ros2aria-dev .
+docker build -f Dockerfile.aria -t delicjusz/ros2aria:jazzy .
 

compose.decawave.yaml

@@ -0,0 +1,17 @@
+# docker compose -f compose.decawave.yaml up
+services:
+  decawave:
+   image: delicjusz/decawave
+   network_mode: host
+   ipc: host
+   restart: unless-stopped
+   environment:
+    - 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}/ serial_port:=/dev/decawave

compose.hokuyo.yaml

@@ -0,0 +1,21 @@
+# docker compose -f compose.hokuyo.yaml up
+services:
+  hokuyo:
+   image: delicjusz/hokuyo
+   network_mode: host
+   ipc: host
+   restart: unless-stopped
+   environment:
+    - 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: >
+      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/hokuyo

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.ros2aria.yaml

@@ -0,0 +1,18 @@
+# docker compose -f compose.ros2aria.yaml up
+services:
+  ros2aria-dev:
+   image: delicjusz/pioneer:humble-18012024
+   network_mode: host
+   ipc: host
+   restart: unless-stopped
+   environment:
+    - 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

@@ -0,0 +1,17 @@
+[Unit]
+Description=%i service with docker compose
+PartOf=docker.service
+After=docker.service
+
+[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

fastdds.xml

@@ -0,0 +1,25 @@
+<dds>
+    <profiles>
+        <participant profile_name="super_client_profile" is_default_profile="true">
+            <rtps>
+                <builtin>
+                    <discovery_config>
+                        <discoveryProtocol>SUPER_CLIENT</discoveryProtocol>
+                        <discoveryServersList>
+                            <RemoteServer prefix="44.53.00.5f.45.50.52.4f.53.49.4d.41">
+                                <metatrafficUnicastLocatorList>
+                                    <locator>
+                                        <udpv4>
+                                            <address>10.104.16.240</address>
+                                            <port>11811</port>
+                                        </udpv4>
+                                    </locator>
+                                </metatrafficUnicastLocatorList>
+                            </RemoteServer>
+                        </discoveryServersList>
+                    </discovery_config>
+                </builtin>
+            </rtps>
+        </participant>
+    </profiles>
+</dds>

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,46 @@
+#!/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
+
+cd /home/lab1_5/ros2aria/
+
+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/
+mkdir -p maps
+
+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

node_info_ros1.txt

@@ -1,61 +0,0 @@
---------------------------------------------------------------------------------
-Node [/PIONIER6/RosAria]
-Publications: 
- * /PIONIER6/RosAria/battery_recharge_state [std_msgs/Int8]
- * /PIONIER6/RosAria/battery_state_of_charge [std_msgs/Float32]
- * /PIONIER6/RosAria/bumper_state [rosaria/BumperState]
- * /PIONIER6/RosAria/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
- * /PIONIER6/RosAria/parameter_updates [dynamic_reconfigure/Config]
- * /PIONIER6/RosAria/pose [nav_msgs/Odometry]
- * /PIONIER6/RosAria/robot_info [rosaria_msgs/RobotInfoMsg]
- * /PIONIER6/RosAria/sonar [sensor_msgs/PointCloud]
- * /PIONIER6/RosAria/sonar_pointcloud2 [sensor_msgs/PointCloud2]
- * /PIONIER6/RosAria/wheels [sensor_msgs/JointState]
- * /rosout [rosgraph_msgs/Log]
- * /tf [tf2_msgs/TFMessage]
-
-Subscriptions: 
- * /PIONIER/master_stop [std_msgs/Bool]
- * /PIONIER/restrictions [rosaria_msgs/RestrictionsMsg]
- * /PIONIER6/RosAria/clutch [std_msgs/Bool]
- * /PIONIER6/RosAria/cmd_vel [unknown type]
- * /PIONIER6/RosAria/user_stop [std_msgs/Bool]
-
-Services: 
- * /PIONIER6/RosAria/get_loggers
- * /PIONIER6/RosAria/gripper_close
- * /PIONIER6/RosAria/gripper_down
- * /PIONIER6/RosAria/gripper_open
- * /PIONIER6/RosAria/gripper_up
- * /PIONIER6/RosAria/set_logger_level
- * /PIONIER6/RosAria/set_parameters
-
-
-contacting node http://10.104.16.45:35307/ ...
-Pid: 52
-Connections:
- * topic: /rosout
-    * to: /rosout
-    * direction: outbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/robot_info
-    * to: /master_plugin_lab15_19_12824_1114070233125035517
-    * direction: outbound
-    * transport: TCPROS
- * topic: /PIONIER/restrictions
-    * to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/user_stop
-    * to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER/master_stop
-    * to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/clutch
-    * to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
-    * direction: inbound
-    * transport: TCPROS
-

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()

run.sh

@@ -1,6 +1,6 @@
 #!/bin/bash
 export LD_LIBRARY_PATH=/usr/local/Aria/lib
 source /opt/ros/foxy/setup.bash
-source /ws/ros2aria/install/setup.bash
+source /ros2_ws/ros2aria/install/setup.bash
 ros2 run ros2aria ros2aria
 

src/pioneer_bringup/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(pioneer_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY 
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/pioneer_bringup/launch/robot.launch.py

@@ -0,0 +1,53 @@
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch.conditions import IfCondition, UnlessCondition
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+import launch
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch_ros.actions import Node, PushRosNamespace
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pioneer_description = get_package_share_directory('pioneer_description')
+    default_model_path = os.path.join(pioneer_description, 'urdf/pioneer3dx.urdf.xacro')
+
+    namespace = LaunchConfiguration('namespace')
+    namespace_arg = DeclareLaunchArgument(
+        'namespace',
+        default_value='pioneer0/'
+    )
+
+    robot_state_publisher_node = Node(
+        namespace=namespace,
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[
+            {'robot_description': Command(['xacro ', default_model_path])},
+            {'frame_prefix': namespace}
+        ]
+    )
+
+    ros2aria = Node(
+        namespace=namespace,
+        package='ros2aria',
+        executable='ros2aria',
+        parameters=[
+            {'use_sonar': False}
+        ]
+    )
+
+    return LaunchDescription([
+        namespace_arg,
+        robot_state_publisher_node,
+        ros2aria
+    ])

src/pioneer_bringup/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pioneer_bringup</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jakub.delicat@husarion.com">deli</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>xacro</depend>
+  <depend>tf2_ros</depend>
+  <depend>robot_state_publisher</depend>
+
+  <depend>pioneer_description</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/pioneer_description/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(pioneer_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+  meshes
+  urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/pioneer_description/package.xml

@@ -0,0 +1,27 @@
+<package>
+  <name>pioneer_description</name>
+  <version>1.1.0</version>
+  <description>URDF file descriptions for various Adept MobileRobots/ActivMedia robots</description>
+  <maintainer email="reed.hedges@adept.com">Reed Hedges</maintainer> 
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/Robots/Pioneer</url>
+
+  <author email="hunter.allen@Vanderbilt.edu">Hunter Allen</author>
+  <author email="dfseifer@usc.edu">David Feil-Seifer</author>
+  <author email="reed.hedges@adept.com">Reed Hedges</author>
+  <author>Brian Gerkey</author>
+  <author>Kasper Stoy</author>
+  <author>Richard Vaughan</author>
+  <author>Andrew Howard</author>
+  <author>Tucker Hermans</author>
+  <author>ActivMedia Robotics LLC</author>
+  <author>MobileRobots Inc.</author>
+  <author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
+
+
+  <export>                               
+   <build_type>ament_cmake</build_type> 
+  </export> 
+</package>

src/pioneer_description/urdf/pioneer3dx.urdf.xacro

@@ -0,0 +1,393 @@
+<?xml version="1.0"?>
+
+<robot name="pioneer3dx"
+	xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+	xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
+  <xacro:include filename="$(find pioneer_description)/urdf/pioneer3dx_wheel.xacro"/>
+
+<!-- Chassis -->
+	<link name="base_link">
+		<inertial>
+			<mass value="3.5"/>
+			<!--<origin xyz="-0.025 0 -0.223"/>-->
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0"
+				 izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="-0.045 0 0.148" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/chassis.stl"/>
+			</geometry>
+			<material name="ChassisRed">
+				<color rgba="0.851 0.0 0.0 1.0"/>
+			</material>
+		</visual>
+		<collision>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+				<geometry>
+					<box size="0.01 0.01 0.01"/>
+				</geometry>
+		</collision>
+	</link>
+
+	<link name="top_plate">
+		<inertial>
+			<mass value="0.01"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0"
+ 				izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/top.stl"/>
+			</geometry>
+			<material name="TopBlack">
+					<color rgba="0.038 0.038 0.038 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<gazebo reference="top_plate">
+		<!-- material value="Gazebo/Black"/ -->
+		<material>Gazebo/Black</material>
+	</gazebo>
+
+    <joint name="base_top_joint" type="fixed">
+		<origin xyz="-0.045 0 0.234" rpy="0 0 0"/>
+		<axis xzy="0 0 1"/>
+		<parent link="base_link"/>
+		<child link="top_plate"/>
+	</joint>
+
+	<link name="front_sonar">
+		<inertial>
+			<mass value="0.0001"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0" izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/front_sonar.stl"/>
+			</geometry>
+			<material name="SonarYellow">
+				<color rgba="0.715 0.583 0.210 1.0"/>
+			</material>
+		</visual>
+	</link>
+
+	<gazebo reference="front_sonar">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="base_front_joint" type="fixed">
+		<origin xyz="-0.198 0 0.208" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="front_sonar"/>
+	</joint>
+
+	<joint name="base_back_joint" type="fixed">
+		<origin xyz="0.109 0 0.209" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="back_sonar"/>
+	</joint>
+	<link name="back_sonar">
+		<inertial>
+			<mass value="0"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0" izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/back_sonar.stl"/>
+			</geometry>
+			<material name="SonarYellow">
+				<color rgba="0.715 0.583 0.210 1.0"/>
+			</material>
+		</visual>
+	</link>
+
+	<!-- Caster -->
+	<joint name="base_caster_swivel_joint" type="continuous">
+		<origin xyz="-0.185 0 0.055" rpy="0 0 0"/>
+		<anchor xyz="0 0 0"/>
+    	<limit effort="100" velocity="100" k_velocity="0" />
+    	<joint_properties damping="0.0" friction="0.0" />
+		<parent link="base_link"/>
+		<child link="caster_swivel"/>
+	</joint>
+	<link name="caster_swivel">
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.01" ixy="0" ixz="0"
+				 iyy="0.01" iyz="0" izz="0.01"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_swivel.stl"/>
+		</geometry>
+		<material name="caster_swivel">
+			<color rgba="0.5 0.5 0.5 1"/>
+		</material>
+	</visual>
+	</link>
+	<gazebo reference="caster_swivel">
+		<material value="Gazebo/Grey"/>
+	</gazebo>
+
+<!-- Center Wheel + Hubcap -->
+	<link name="caster_hubcap">
+		<inertial>
+			<mass value="0.01"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
+				 iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_hubcap.stl"/>
+		</geometry>
+		<material name="caster_swivel">
+			<color rgba="0.5 0.5 0.5 1"/>
+		</material>
+	</visual>
+	<collision>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry>
+			<box size="0.05 0.05 0.05"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="caster_hubcap">
+		<material value="Gazebo/Grey"/>
+	</gazebo>
+
+	<joint name="caster_swivel_hubcap_joint" type="continuous">
+		<origin xyz="-0.026 0 -0.016" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<anchor xyz="0 0 0"/>
+    <limit effort="100" velocity="100" k_velocity="0" />
+    <joint_properties damping="0.0" friction="0.0" />
+	  <parent link="caster_swivel"/>
+		<child link="caster_wheel"/>
+	</joint>
+
+	<link name="caster_wheel">
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
+				 iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>
+		</geometry>
+		<material name="WheelBlack">
+			<color rgba="0.117 0.117 0.117 1"/>
+		</material>
+	</visual>
+	<collision>
+		<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+		<geometry>
+			<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+			<cylinder radius="0.0375" length="0.01"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="left_hub">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="caster_wheel_joint" type="fixed">
+		<origin xyz="-0.0035 0 -0.001" rpy="0 0 0"/>
+		<parent link="caster_wheel"/>
+		<child link="caster_hubcap"/>
+	</joint>
+
+	<link name="left_hub">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/left_hubcap.stl"/>
+			</geometry>
+			<material name="HubcapYellow">
+				<color rgba="1.0 0.811 0.151 1.0"/>
+			</material>
+		</visual>
+		<collision>
+			<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+			<geometry>
+				<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+				<cylinder radius="0.09" length="0.01"/>
+			</geometry>
+		</collision>
+	</link>
+
+	<joint name="left_hub_joint" type="fixed">
+		<origin xyz="0 0.15 0.08" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="left_hub"/>
+		<axis xyz="0 1 0"/>
+	</joint>
+
+	<link name="left_wheel">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/left_wheel.stl"/>
+			</geometry>
+			<material name="WheelBlack">
+				<color rgba="0.117 0.117 0.117 1"/>
+			</material>
+		</visual>
+	</link>
+
+	<joint name="left_wheel_joint" type="continuous"> <!-- type="continuous" -->
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<parent link="left_hub"/>
+		<child link="left_wheel"/>
+	</joint>
+	
+	<gazebo reference="left_wheel_joint">
+		<material value="Gazebo/Black"/>
+	</gazebo>
+
+	<link name="right_hub">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/right_hubcap.stl"/>
+			</geometry>
+			<material name="HubcapYellow">
+				<color rgba="1.0 0.811 0.151 1.0"/>
+			</material>
+		</visual>
+		<collision>
+		<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+		<geometry>
+			<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+			<cylinder radius="0.09" length="0.01"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="right_hub">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="right_hub_joint" type="fixed">
+		<origin xyz="0 -0.15 0.08" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="right_hub"/>
+		<axis xyz="0 1 0"/>
+	</joint>
+
+	<link name="right_wheel">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/right_wheel.stl"/>
+			</geometry>
+			<material name="WheelBlack">
+				<color rgba="0.117 0.117 0.117 1"/>
+			</material>
+		</visual>
+	</link>
+
+	<joint name="right_wheel_joint" type="continuous"> <!-- type="continuous" -->
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<parent link="right_hub"/>
+		<child link="right_wheel"/>
+	</joint>
+	<gazebo reference="right_wheel_joint">
+		<material value="Gazebo/Black"/>
+	</gazebo>
+
+	<link name="laser">
+		<collision>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry>
+			<box size="0.1 0.1 0.1"/>
+		</geometry>
+		</collision>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+			<mesh filename="package://amr_robots_description/meshes/p3dx_meshes/hokuyo_convex.stl"/>
+			</geometry>
+			<material name="Hokuyo">
+				<color rgba="0 0 0 1"/>
+			</material>
+		</visual>
+		<inertial>
+			<mass value="1e-5" />
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+		</inertial>
+	</link>
+	<joint name="hokuyo_joint" type="fixed">
+		<axis xyz="0 0 0" />
+		<origin xyz="0.040 0 0.0625" rpy="0 0 0"/>
+		<parent link="front_sonar"/>
+		<child link="laser"/>
+	</joint>
+
+
+	<link name="camera_frame">
+		<collision>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<box size="0.095 0.03 0.03"/>
+			</geometry>
+		</collision>
+	</link>
+	<joint name="camera_joint" type="fixed">
+		<axis xyz="0 0 0" />
+		<origin xyz="0.040 0 0.1125" rpy="0 0 0"/>
+		<parent link="front_sonar"/>
+		<child link="camera_frame"/>
+	</joint>
+
+
+	<create>
+	<back_sonar parent="base_link"/>
+  	<top_plate  parent="base_link"/>
+  	</create>
+
+<gazebo>
+	<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
+    <alwaysOn>true</alwaysOn>
+		<updateRate>100</updateRate>
+		<leftJoint>left_wheel_joint</leftJoint>
+		<rightJoint>right_wheel_joint</rightJoint>
+		<wheelSeparation>0.158</wheelSeparation>
+		<wheelDiameter>0.12</wheelDiameter>
+		<torque>5</torque>
+		<!-- interface:position name="position_iface_0"/ -->
+    <commandTopic>cmd_vel</commandTopic>
+    <robotBaseFrame>base_link</robotBaseFrame>
+    <odometryTopic>odom</odometryTopic>
+    <odometryFrame>odom</odometryFrame>
+	</plugin>
+
+  <!-- TODO include P3D (ground truth) plugin -->
+  
+
+  <!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
+</gazebo>
+
+</robot>

src/pioneer_description/urdf/pioneer3dx_wheel.xacro

@@ -0,0 +1,108 @@
+<?xml version="1.0"?>
+
+<robot
+  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+  xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+  xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+  xmlns:xacro="http://ros.org/wiki/xacro"
+  >
+
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
+
+  <!-- Properties (Constants) -->
+  <property name="M_PI" value="3.14159"/>
+
+<!-- Right/Left Hubcap + Wheel -->
+<xacro:macro name="p3dx_wheel" params="suffix reflect">
+  <link name="p3dx_${suffix}_wheel">
+    <inertial>
+      <mass value="0.5"/> 
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="0" ixz="0"
+         iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
+      </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="${-3.1415927/2} 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>-->
+        <cylinder radius="0.09" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="base_${suffix}_hubcap_joint" type="fixed">
+    <!--<origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>-->
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+
+    <parent link="p3dx_${suffix}_wheel"/>
+    <child link="p3dx_${suffix}_hubcap"/>
+  </joint>
+
+  <link name="p3dx_${suffix}_hubcap">
+    <inertial>
+      <mass value="0.01"/>  
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="0" ixz="0"
+         iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
+      </inertial>
+  <visual>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry name="pioneer_geom">
+      <mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_hubcap.stl"/>
+    </geometry>
+    <material name="HubcapYellow">
+      <color rgba="1.0 0.811 0.151 1.0"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry>
+      <box size="0 0 0"/>
+    </geometry>
+  </collision>
+  </link>
+
+  <transmission type="pr2_mechanism_model/SimpleTransmission" name="${suffix}_wheel_trans">
+    <actuator name="base_${suffix}_wheel_motor" />
+    <joint name="base_${suffix}_wheel_joint" />
+    <mechanicalReduction>${reflect * 624/35 * 80/19}</mechanicalReduction>
+  </transmission>
+
+  <joint name="base_${suffix}_wheel_joint" type="continuous">
+    <axis xyz="0 1 0"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" velocity="100" />
+    <joint_properties damping="0.0" friction="0.0" />
+    <origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>
+    <parent link="p3dx_${suffix}_hubcap"/>
+    <child link="p3dx_${suffix}_wheel"/>
+  </joint>
+
+
+  <gazebo reference="p3dx_${suffix}_hubcap">
+      <material value="Gazebo/Yellow"/>
+  </gazebo>
+
+
+  <gazebo reference="p3dx_${suffix}_wheel">
+      <material value="Gazebo/Black"/>
+      <elem key="mu1" value="0.5" />
+      <elem key="mu2" value="50.0" />
+      <elem key="kp"  value="100000000.0" />
+      <elem key="kd"  value="1.0" />
+  </gazebo>
+
+</xacro:macro>
+
+
+</robot>
+

src/ros2aria/CMakeLists.txt

@@ -23,10 +23,9 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(ros2aria_msgs REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
 
 
 add_executable(ros2aria
@@ -40,7 +39,10 @@ add_executable(ros2aria
   src/gripper.cpp
   src/wheels.cpp
   src/clutch.cpp
-  src/state.cpp)
+  src/state.cpp
+  src/restrictions.cpp
+  src/scan.cpp
+)
 
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
@@ -48,6 +50,8 @@ ament_target_dependencies(ros2aria geometry_msgs)
 ament_target_dependencies(ros2aria sensor_msgs)
 ament_target_dependencies(ros2aria nav_msgs)
 ament_target_dependencies(ros2aria tf2)
+ament_target_dependencies(ros2aria tf2_ros)
+ament_target_dependencies(ros2aria tf2_geometry_msgs)
 ament_target_dependencies(ros2aria ros2aria_msgs)
 
 target_include_directories(ros2aria PUBLIC
@@ -59,7 +63,7 @@ target_include_directories(ros2aria PUBLIC
 target_link_libraries(ros2aria /usr/local/Aria/lib/libAria.so)
 
 install(TARGETS ros2aria
-  DESTINATION lib/${PROJECT_NAME})
+  DESTINATION lib/${PROJECT_NAME} )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/ros2aria/include/ros2aria/raiibot.hpp

@@ -0,0 +1,36 @@
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include "Aria/Aria.h"
+#include "rclcpp/rclcpp.hpp"
+
+
+class RAIIBot {
+   public:
+    typedef std::shared_ptr<RAIIBot> SharedPtr;
+
+    RAIIBot(rclcpp::Node *node, std::string port);
+    ~RAIIBot();
+    ArRobot *getRobot();
+    ArGripper *getGripper();
+    rclcpp::Clock::SharedPtr getClock();
+    void lock();
+    void unlock();
+    void pokeWatchdog();
+
+   private:
+    ArRobot *robot = nullptr;
+    ArRobotConnector *robotConn = nullptr;
+    ArArgumentBuilder *args = nullptr;
+    ArArgumentParser *argparser = nullptr;
+    ArGripper *gripper = nullptr;
+    rclcpp::Node *node = nullptr;
+    rclcpp::TimerBase::SharedPtr watchdogTimer;
+    rclcpp::Time lastWatchdogPoke;
+    rclcpp::Clock::SharedPtr clock;
+
+    rclcpp::Logger get_logger();
+    void startWatchdog();
+    void watchdog_cb();
+};

src/ros2aria/include/ros2aria/ros2aria.hpp

@@ -1,21 +1,39 @@
-#include "rclcpp/rclcpp.hpp"
-#include "Aria/Aria.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <utility>
+#include<string>
 
-#include "sensor_msgs/msg/point_cloud.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
-#include "sensor_msgs/msg/joint_state.hpp"
+#include "ros2aria/raiibot.hpp"
+#include <Aria/Aria.h>
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "std_srvs/srv/empty.hpp"
-#include "std_msgs/msg/bool.hpp"
-#include "std_msgs/msg/int8.hpp"
-#include "std_msgs/msg/float32.hpp"
-
+#include "rclcpp/rclcpp.hpp"
 #include "ros2aria_msgs/msg/robot_info_msg.hpp"
-#include "./raiibot.cpp"
+#include "ros2aria_msgs/msg/restrictions_msg.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "std_msgs/msg/int8.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2/LinearMath/Quaternion.h"
 
 #define UNUSED(x) (void)(x)
 
+constexpr double mm_per_sec_to_rad_per_sec(double mm_per_sec){
+    return mm_per_sec  / 195.0 / 2.0; /*diameter in mm*/ 
+}
+
+constexpr double encoder_to_rad(long int encoder){
+    return static_cast<double>(encoder) * M_PI / 32768.0;
+}
+
+constexpr float distance(float a, float b){
+    return std::sqrt(a*a + b*b);
+}
+
 class Ros2Aria : public rclcpp::Node
 {
 public:
@@ -23,14 +41,20 @@ public:
     ~Ros2Aria();
 
 private:
-    // robot
+    std::size_t id;
     RAIIBot::SharedPtr robot;
     ArFunctorC<Ros2Aria> sensorCb;
+    ros2aria_msgs::msg::RestrictionsMsg restrictions;
+    float minimal_distance = 0.0;
+    bool obstacle_too_close = true;
+    bool master_stop = true;
+    bool user_stop = true;
+    bool use_sonar = true;
+    uint8_t num_of_sonars = 0;
 
-    // config
-    std::string sonar_frame_id;
+    std::size_t extract_number_from_string(const std::string& str);
 
-    // publishers
+    void handle_parameters();
     void publish();
 
     sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
@@ -39,9 +63,11 @@ private:
     void publishSonar(sensor_msgs::msg::PointCloud cloud);
     void publishSonarPointCloud2(sensor_msgs::msg::PointCloud cloud);
 
-    nav_msgs::msg::Odometry handlePose(rclcpp::Time stamp);
+    std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> handlePose(rclcpp::Time stamp);
+    std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
     void publishPose(nav_msgs::msg::Odometry pose);
+    void publishTf(geometry_msgs::msg::TransformStamped tf);
 
     sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
@@ -50,15 +76,28 @@ private:
     rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr battery_recharge_state_pub_;
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state_of_charge_pub_;
     rclcpp::Publisher<ros2aria_msgs::msg::RobotInfoMsg>::SharedPtr robot_info_pub_;
-    void publishState(rclcpp::Time stamp);
+    void publishState();
 
     // subscribers
+    rclcpp::Subscription<ros2aria_msgs::msg::RestrictionsMsg>::SharedPtr restrictions_sub_;
+    void restrictions_callback(const ros2aria_msgs::msg::RestrictionsMsg::SharedPtr msg);
+    void init_restrictions();
+
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr user_stop_sub_;
+    void user_stop_callback(const std_msgs::msg::Bool::SharedPtr msg);
+
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr master_stop_sub_;
+    void master_stop_callback(const std_msgs::msg::Bool::SharedPtr msg);
+
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
 
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr clutch_sub_;
     void clutch_callback(const std_msgs::msg::Bool::SharedPtr msg);
 
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+    void scan_callback(const sensor_msgs::msg::LaserScan msg);
+
     // services
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
     void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;

src/ros2aria/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>ros2aria_msgs</depend>

src/ros2aria/src/clutch.cpp

@@ -1,7 +1,6 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
+void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg) {
     std::lock_guard<RAIIBot> lock(*robot.get());
 
     auto r = robot->getRobot();

src/ros2aria/src/cmd_vel.cpp

@@ -1,7 +1,7 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
+#include <cmath>
 
-void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
-{
+void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
     float x, y, z;
     x = msg->linear.x;
     y = msg->linear.y;
@@ -13,8 +13,23 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
     this->robot->pokeWatchdog();
 
     auto r = robot->getRobot();
-    r->setVel(x * 1e3);
+
+    if (master_stop or obstacle_too_close or user_stop){
+        x = 0.0;
+        y = 0.0;
+        z = 0.0;
+    }
+    else{
+        // apply limits
+        x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x;
+        y = std::abs(y) > restrictions.linear_velocity.data ? std::abs(y)/y * restrictions.linear_velocity.data : y;
+        z = std::abs(z) > restrictions.angular_velocity.data ?  std::abs(z)/z * restrictions.angular_velocity.data : z;
+    }
+    x *= 1e3;
+    y *= 1e3;
+    z *= 180 / M_PI;
+    r->setVel(x);
     if (r->hasLatVel())
-        r->setLatVel(y * 1e3);
-    r->setRotVel(z * 180 / M_PI);
+        r->setLatVel(y);
+    r->setRotVel(z);
 }

src/ros2aria/src/gripper.cpp

@@ -1,31 +1,30 @@
-#include "./ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
+void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
     UNUSED(request);
     UNUSED(response);
     std::lock_guard<RAIIBot> lock(*robot.get());
     auto g = robot->getGripper();
     g->gripOpen();
 }
-void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
+
+void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
     UNUSED(request);
     UNUSED(response);
     std::lock_guard<RAIIBot> lock(*robot.get());
     auto g = robot->getGripper();
     g->gripClose();
 }
-void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
+
+void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
     UNUSED(request);
     UNUSED(response);
     std::lock_guard<RAIIBot> lock(*robot.get());
     auto g = robot->getGripper();
     g->liftUp();
 }
-void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
+
+void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
     UNUSED(request);
     UNUSED(response);
     std::lock_guard<RAIIBot> lock(*robot.get());

src/ros2aria/src/main.cpp

@@ -1,7 +1,7 @@
 #include <cstdio>
 #include <memory>
 
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 int main(int argc, char **argv)
 {

src/ros2aria/src/pose.cpp

@@ -1,46 +1,47 @@
-#include "ros2aria.hpp"
-#include "tf2/utils.h"
-#include "tf2/transform_datatypes.h"
+#include "ros2aria/ros2aria.hpp"
 
-inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
-{
-    geometry_msgs::msg::Transform msg = tf2::toMsg(trans);
-    pose.orientation = msg.rotation;
-    pose.position.x = msg.translation.x;
-    pose.position.y = msg.translation.y;
-    pose.position.z = msg.translation.z;
-}
-
-nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
-{
-    nav_msgs::msg::Odometry pose;
+std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> Ros2Aria::handlePose(rclcpp::Time stamp) {
+    nav_msgs::msg::Odometry odom_msg;
 
     auto r = robot->getRobot();
     ArPose p = r->getPose();
 
     tf2::Quaternion rotation;
+    auto position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
     rotation.setRPY(0, 0, p.getTh() * M_PI / 180);
 
-    tf2::Vector3 position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
+    odom_msg.pose.pose.orientation = tf2::toMsg(rotation);
+    odom_msg.pose.pose.position.x = position.getX();
+    odom_msg.pose.pose.position.y = position.getY();
+    odom_msg.pose.pose.position.z = position.getZ();
 
-    convert_transform_to_pose(tf2::Transform(rotation, position),
-                              pose.pose.pose);
+    odom_msg.twist.twist.linear.x = r->getVel() / 1000;
+    odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
+    odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
 
-    pose.twist.twist.linear.x = r->getVel() / 1000;
-    pose.twist.twist.linear.y = r->getLatVel() / 1000;
-    pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
+    odom_msg.header.frame_id = std::string(get_namespace()) + "/odom";
+    odom_msg.child_frame_id = std::string(get_namespace()) + "/base_link";
+    odom_msg.header.stamp = stamp;
 
-    pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
-    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_frame_id
-    pose.header.stamp = stamp;
+    geometry_msgs::msg::TransformStamped transform;
+    transform.header = odom_msg.header;
+    transform.child_frame_id= odom_msg.child_frame_id;
+    transform.transform.translation.x = odom_msg.pose.pose.position.x;
+    transform.transform.translation.y = odom_msg.pose.pose.position.y;
+    transform.transform.translation.z = odom_msg.pose.pose.position.z;
+    transform.transform.rotation = odom_msg.pose.pose.orientation;
 
-    return pose;
+    std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> pair_msgs = {odom_msg, transform};
+    return pair_msgs;
 }
 
-void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
-{
+void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) {
     if (this->pose_pub_->get_subscription_count() == 0)
         return;
 
     this->pose_pub_->publish(pose);
 }
+
+void Ros2Aria::publishTf(geometry_msgs::msg::TransformStamped tf){
+    odom_tf_broadcaster_->sendTransform(tf);
+}

src/ros2aria/src/publish.cpp

@@ -1,19 +1,21 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 void Ros2Aria::publish()
 {
-    // RCLCPP_INFO(this->get_logger(), "publish");
     rclcpp::Time t = robot->getClock()->now();
 
+    if(use_sonar){
         sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
         publishSonar(sonarData);
         publishSonarPointCloud2(sonarData);
+    }
 
-    nav_msgs::msg::Odometry pose = handlePose(t);
-    publishPose(pose);
+    auto pose = handlePose(t);
+    publishPose(pose.first);
+    publishTf(pose.second);
 
     sensor_msgs::msg::JointState wheels = handleWheels(t);
     publishWheels(wheels);
 
-    publishState(t);
+    publishState();
 }

src/ros2aria/src/raiibot.cpp

@@ -1,19 +1,7 @@
-#include <string>
-#include <memory>
-#include <iostream>
+#include "ros2aria/raiibot.hpp"
 
-#include "Aria/Aria.h"
-#include "rclcpp/rclcpp.hpp"
 
-using namespace std::chrono_literals;
-
-class RAIIBot
-{
-public:
-    typedef std::shared_ptr<RAIIBot> SharedPtr;
-
-    RAIIBot(rclcpp::Node *node, std::string port)
-    {
+RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
     this->robot = new ArRobot();
     this->gripper = new ArGripper(this->robot);
     this->node = node;
@@ -22,16 +10,15 @@ public:
     args = new ArArgumentBuilder();
     this->argparser = new ArArgumentParser(args);
     argparser->loadDefaultArguments();  // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
-        args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
+    std::string port_arg = "-robotPort " + port;
+    args->add(port_arg.c_str());  // pass robot's serial port to Aria // TODO: use `port` variable.
     // args->add("-robotLogPacketsReceived");                    // log received packets
     // args->add("-robotLogPacketsSent");                        // log sent packets
     // args->add("-robotLogVelocitiesReceived"); // log received velocities
     // args->add("-robotLogMovementSent");
     // args->add("-robotLogMovementReceived");
     this->robotConn = new ArRobotConnector(argparser, robot);
-        if (!this->robotConn->connectRobot())
-        {
-
+    if (!this->robotConn->connectRobot()) {
         // RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
         // rclcpp::shutdown();
     }
@@ -40,23 +27,20 @@ public:
     this->robot->enableMotors();
 
     this->startWatchdog();
-    }
+}
 
-    ~RAIIBot()
-    {
+RAIIBot ::~RAIIBot() {
     std::cout << std::endl
               << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
 
-        if (this->robot != nullptr)
-        {
+    if (this->robot != nullptr) {
         this->robot->lock();
         std::cout << "disabling motors" << std::endl;
         this->robot->disableMotors();
         std::cout << "disabled motors" << std::endl;
         Aria::shutdown();
     }
-        if (this->robotConn != nullptr)
-        {
+    if (this->robotConn != nullptr) {
         std::cout << "disconnecting" << std::endl;
         this->robotConn->disconnectAll();
         std::cout << "disconnected" << std::endl;
@@ -70,68 +54,50 @@ public:
         delete this->robotConn;
     if (this->robot != nullptr)
         delete this->robot;
-    }
+}
 
-    ArRobot *getRobot()
-    {
+ArRobot *RAIIBot::getRobot() {
     return this->robot;
-    }
+}
 
-    ArGripper *getGripper()
-    {
+ArGripper *RAIIBot::getGripper() {
     return this->gripper;
-    }
+}
 
-    rclcpp::Clock::SharedPtr getClock()
-    {
+rclcpp::Clock::SharedPtr RAIIBot::getClock() {
     return this->clock;
-    }
+}
 
-    void lock()
-    {
+void RAIIBot::lock() {
     if (this->robot != nullptr)
         this->robot->lock();
-    }
+}
 
-    void unlock()
-    {
+void RAIIBot::unlock() {
     if (this->robot != nullptr)
         this->robot->unlock();
-    }
+}
 
-    void pokeWatchdog()
-    {
+void RAIIBot::pokeWatchdog() {
     // this should probably be made thread safe, or something?
     this->lastWatchdogPoke = this->clock->now();
-    }
+}
 
-private:
-    ArRobot *robot = nullptr;
-    ArRobotConnector *robotConn = nullptr;
-    ArArgumentBuilder *args = nullptr;
-    ArArgumentParser *argparser = nullptr;
-    ArGripper *gripper = nullptr;
-    rclcpp::Node *node = nullptr;
-    rclcpp::TimerBase::SharedPtr watchdogTimer;
-    rclcpp::Time lastWatchdogPoke;
-    rclcpp::Clock::SharedPtr clock;
-
-    rclcpp::Logger get_logger()
-    {
+rclcpp::Logger RAIIBot::get_logger() {
     return this->node->get_logger();
-    }
+}
 
-    void startWatchdog()
-    {
+void RAIIBot::startWatchdog() {
+    using namespace std::chrono_literals;
     this->lastWatchdogPoke = clock->now();
     watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
-    }
+}
+
+void RAIIBot::watchdog_cb() {
+    using namespace std::chrono_literals;
 
-    void watchdog_cb()
-    {
     auto now = this->clock->now();
-        if (now - this->lastWatchdogPoke > 1s)
-        {
+    if (now - this->lastWatchdogPoke > 1s) {
         std::lock_guard<RAIIBot> lock(*this);
         auto r = this->getRobot();
         r->setVel(0);
@@ -139,5 +105,4 @@ private:
         if (r->hasLatVel())
             r->setLatVel(0);
     }
-    }
-};
+}

src/ros2aria/src/restrictions.cpp

@@ -0,0 +1,21 @@
+#include "ros2aria/ros2aria.hpp"
+#include <limits>
+void Ros2Aria::restrictions_callback(const ros2aria_msgs::msg::RestrictionsMsg::SharedPtr msg){
+    restrictions = *msg;
+    RCLCPP_INFO(this->get_logger(), "restrictions: x:%f y:%f z:%f", restrictions.linear_velocity.data, restrictions.angular_velocity.data, 
+                                                                 restrictions.distance.data);
+}
+
+void Ros2Aria::init_restrictions(){
+    restrictions.linear_velocity.data = 0.0;
+    restrictions.angular_velocity.data = 0.0;
+    restrictions.distance.data = std::numeric_limits<float>::max();
+}
+
+void Ros2Aria::user_stop_callback(const std_msgs::msg::Bool::SharedPtr msg){
+    user_stop = msg->data;
+}
+
+void Ros2Aria::master_stop_callback(const std_msgs::msg::Bool::SharedPtr msg){
+    master_stop = msg->data;
+}

src/ros2aria/src/ros2aria.cpp

@@ -1,5 +1,7 @@
-#include "ros2aria.hpp"
-
+#include "ros2aria/ros2aria.hpp"
+#include <iostream>
+#include <string>
+#include <sstream>
 using std::placeholders::_1;
 using std::placeholders::_2;
 
@@ -7,20 +9,42 @@ Ros2Aria::Ros2Aria()
     : Node("ros2aria"),
       sensorCb(this, &Ros2Aria::publish)
 {
-    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); // TODO: use args for port
+    id = extract_number_from_string(std::string(get_namespace()));
+
+    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
+    handle_parameters();
 
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
+
+    restrictions_sub_ = this->create_subscription<ros2aria_msgs::msg::RestrictionsMsg>(
+        "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1));
+    init_restrictions();
+
+    master_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1));
+    user_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1));
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
     clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
         "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
+    scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+        "scan", 10, std::bind(&Ros2Aria::scan_callback, this, _1));
 
+    auto r = robot->getRobot();
+    if(use_sonar){
         sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
         sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
+    }
+    else{
+        r->disableSonar();
+    }
+    r->disableMotors();
 
-    pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
+    pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 1000);
+    odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
 
-    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
+    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1000);
 
     battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10);
     battery_state_of_charge_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery_state_of_charge", 10);
@@ -33,11 +57,35 @@ Ros2Aria::Ros2Aria()
     gripper_up_service_ = this->create_service<std_srvs::srv::Empty>("gripper_up", std::bind(&Ros2Aria::gripper_up_callback, this, _1, _2));
     gripper_down_service_ = this->create_service<std_srvs::srv::Empty>("gripper_down", std::bind(&Ros2Aria::gripper_down_callback, this, _1, _2));
 
-    // listen to sensors
-    auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
 
-    sonar_frame_id = "pionier6"; // TODO: use args
+    RCLCPP_INFO(get_logger(), "NAMESPACE = %s", get_namespace());
+    RCLCPP_INFO(get_logger(), "ID = %d", id);
+
+}
+
+std::size_t Ros2Aria::extract_number_from_string(const std::string& str) {
+    std::string numericPart;
+    for (char c : str) {
+        if (isdigit(c)) {
+            numericPart += c;
+        } else if (!numericPart.empty()) {
+            break; 
+        }
+    }
+    int number;
+    std::istringstream(numericPart) >> number;
+    return number;
+}
+
+void Ros2Aria::handle_parameters(){
+    declare_parameter("use_sonar", true);
+    declare_parameter("num_of_sonars", 0);
+    use_sonar = get_parameter("use_sonar").as_bool();
+    num_of_sonars = get_parameter("num_of_sonars").as_int();
+
+    RCLCPP_INFO(get_logger(), "use_sonar = %d", use_sonar);
+    RCLCPP_INFO(get_logger(), "num_of_sonars = %d", num_of_sonars);
 }
 
 Ros2Aria::~Ros2Aria()

src/ros2aria/src/scan.cpp

@@ -0,0 +1,19 @@
+#include "ros2aria/ros2aria.hpp"
+#include <algorithm>
+#include <limits>
+
+void Ros2Aria::scan_callback(const sensor_msgs::msg::LaserScan msg){
+    float actual_minimal_distance = std::numeric_limits<float>::max();
+    for(const auto &range: msg.ranges){
+        if(not std::isnan(range) and range > msg.range_min and range < msg.range_max){
+            actual_minimal_distance = std::min(actual_minimal_distance, range);
+        }
+    }
+    minimal_distance = actual_minimal_distance;
+    if(actual_minimal_distance < restrictions.distance.data){
+        RCLCPP_INFO(this->get_logger(), "obstacle detected, minimal_distance: %f", actual_minimal_distance);
+        obstacle_too_close = true;
+    }else{
+        obstacle_too_close = false;
+    }
+}

src/ros2aria/src/sonar.cpp

@@ -1,17 +1,16 @@
-#include "ros2aria.hpp"
-
+#include "ros2aria/ros2aria.hpp"
 #include "sensor_msgs/point_cloud_conversion.hpp"
 
 sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
 {
-    sensor_msgs::msg::PointCloud cloud; //sonar readings.
+    sensor_msgs::msg::PointCloud cloud; 
     cloud.header.stamp = stamp;
-    // sonar sensors relative to base_link
-    cloud.header.frame_id = this->sonar_frame_id;
+    cloud.header.frame_id = std::string(get_namespace()) + "/front_sonar";
 
     auto r = robot->getRobot();
 
-    for (int i = 0; i < r->getNumSonar(); i++)
+    auto lenght = r->getNumSonar()/2;
+    for (int i = 0; i < num_of_sonars; i++)
     {
         ArSensorReading *reading = NULL;
         reading = r->getSonarReading(i);
@@ -38,6 +37,10 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
         p.y = reading->getLocalY() / 1000.0;
         p.z = 0.0;
         cloud.points.push_back(p);
+
+        if(distance(p.x, p.y) < minimal_distance){
+            obstacle_too_close = true;
+        }
     }
     return cloud;
 }

src/ros2aria/src/state.cpp

@@ -1,6 +1,6 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::publishState(rclcpp::Time stamp)
+void Ros2Aria::publishState()
 {
     auto r = this->robot->getRobot();
 
@@ -13,32 +13,21 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
         this->battery_recharge_state_pub_->publish(recharge_state);
     }
 
-    // TODO: our robots don't have this, cannot test
-    if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0)
-    {
-        std_msgs::msg::Float32 soc;
-        soc.data = r->getStateOfCharge() / 100.0;
-        this->battery_state_of_charge_pub_->publish(soc);
-    }
 
-    if (this->robot_info_pub_->get_subscription_count() > 0)
+    if (robot_info_pub_->get_subscription_count() > 0)
     {
         ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
 
-        // TODO: allow setting the robot_id
-        robot_info_msg.robot_id.data = 6;
+        robot_info_msg.robot_id.data =  id;
         robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
+        robot_info_msg.minimal_distance.data = minimal_distance;
         robot_info_msg.twist.linear.x = r->getVel() / 1000;
         robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
         robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180;
-
-        // TODO: actually keep track of robot state (true -> unlocked, false ->
-        // locked). This requires safety plugin to be implemented
-        robot_info_msg.state.data = true;
+        robot_info_msg.state.data = not user_stop;
         robot_info_msg.clutch.data = r->areMotorsEnabled();
-        // TODO: actually look for obstacles
-        robot_info_msg.obstacle_detected.data = false;
+        robot_info_msg.obstacle_detected.data = obstacle_too_close;
 
-        this->robot_info_pub_->publish(robot_info_msg);
+        robot_info_pub_->publish(robot_info_msg);
     }
 }

src/ros2aria/src/wheels.cpp

@@ -1,5 +1,5 @@
-#include "ros2aria.hpp"
-
+#include "ros2aria/ros2aria.hpp"
+#include <cmath>
 sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
 {
     sensor_msgs::msg::JointState wheels;
@@ -13,13 +13,14 @@ sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
     wheels.velocity.resize(2);
     wheels.effort.resize(0);
 
-    wheels.name[0] = "Wheel_L";
-    wheels.name[1] = "Wheel_R";
+    // robot_state_publisher gives namespace
+    wheels.name[0] = "left_wheel_joint";
+    wheels.name[1] =  "right_wheel_joint";
+
     wheels.position[0] = r->getLeftEncoder();
     wheels.position[1] = r->getRightEncoder();
     wheels.velocity[0] = r->getLeftVel();
     wheels.velocity[1] = r->getRightVel();
-
     return wheels;
 }
 

src/ros2aria_msgs/msg/RobotInfoMsg.msg

@@ -4,3 +4,4 @@ geometry_msgs/Twist twist
 std_msgs/Bool state
 std_msgs/Bool clutch
 std_msgs/Bool obstacle_detected
+std_msgs/Float64 minimal_distance

wymagania.md

@@ -0,0 +1,6 @@
+Student:
+- odblokowanie robota
+- odblokowanie 
+
+
+- stan przejściowy na przyciskach