added use_safety_system parameter

Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>

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

.vscode/c_cpp_properties.json

@@ -1,19 +1,21 @@
 {
-  "configurations": [
+    "configurations": [
-    {
+        {
-      "browse": {
+            "browse": {
-        "databaseFilename": "",
+                "databaseFilename": "",
-        "limitSymbolsToIncludedHeaders": true
+                "limitSymbolsToIncludedHeaders": true
-      },
+            },
-      "includePath": [
+            "includePath": [
-        "/opt/ros/foxy/include/**",
+                "/opt/ros/foxy/include/**",
-        "/workspaces/ros2aria/src/ros2aria/include/**",
+                "/workspaces/ros2aria/src/ros2aria/include/**",
-        "/workspaces/ros2aria/install/ros2aria_msgs/include/**",
+                "/workspaces/ros2aria/install/ros2aria_msgs/include/**",
-        "/usr/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
+        }
+    ],
+    "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/"
 }

Dockerfile

@@ -0,0 +1,33 @@
+ARG ROS_DISTRO=humble
+FROM delicjusz/ros2aria 
+SHELL ["/bin/bash", "-c"]
+RUN apt-get update && apt-get install -y \
+		git \
+        python3-colcon-common-extensions \
+        python3-rosdep \
+		build-essential
+
+WORKDIR /ros2_ws
+
+COPY src/ /ros2_ws/src/
+RUN 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
+
+
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
+# COPY ros_entrypoint.sh /
+# ENTRYPOINT ["../ros_entrypoint.sh"]
+# RUN chmod +x /ros_entrypoint.sh
+
+# clear ubuntu packages
+# RUN	export SUDO_FORCE_REMOVE=yes && \
+#     apt-get clean && \
+# 	apt-get remove -y \
+# 		python3-colcon-common-extensions \
+# 		python3-rosdep \
+# 		build-essential && \
+# 	rm -rf /var/lib/apt/lists/*

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

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@pionier2:~/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
+
+push:
+	docker build -f Dockerfile -t delicjusz/pioneer .
+	docker push delicjusz/pioneer
+	docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo .
+	docker push delicjusz/hokuyo
+pull:
+	docker pull delicjusz/ros2aria
+rosbridge:
+	docker build -f Dockerfile.rosbridge -t delicjusz/rosbridge .
+	docker push delicjusz/rosbridge

README.md

@@ -0,0 +1,21 @@
+# Instruction on station
+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
+```

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 .
 

compose.hokuyo.yaml

@@ -0,0 +1,19 @@
+# docker compose -f compose.hokuyo.yaml up
+services:
+  hokuyo:
+   image: delicjusz/hokuyo
+   network_mode: host
+   ipc: host
+   environment:
+    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+    - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+   volumes:
+    - ./fastdds.xml:/fastdds.xml
+   devices:
+    # - /dev/hokuyo
+    - /dev/ttyACM0
+   command: >
+      ros2 run urg_node urg_node_driver  --ros-args -r 
+       __ns:=/pioneer${PIONEER_ID} -p
+      laser_frame_id:=pioneer${PIONEER_ID}/laser -p 
+      serial_port:=/dev/ttyACM0

compose.pioneer.yaml

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

compose.ros2aria.yaml

@@ -0,0 +1,15 @@
+# docker compose -f compose.ros2aria.yaml up
+services:
+  ros2aria-dev:
+   image: delicjusz/pioneer:powerbot
+   network_mode: host
+   ipc: host
+   environment:
+    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+    - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
+    - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+   volumes:
+    - ./fastdds.xml:/fastdds.xml
+   devices:
+    - /dev/ttyS0
+   command:  ros2 run ros2aria ros2aria --ros-args -p num_of_sonars:=28

docker-compose@.service

@@ -0,0 +1,14 @@
+[Unit]
+Description=%i service with docker compose
+PartOf=docker.service
+After=docker.service
+
+[Service]
+Type=oneshot
+RemainAfterExit=true
+WorkingDirectory=/etc/docker/compose/%i
+ExecStart=/usr/bin/docker compose up -d --remove-orphans
+ExecStop=/usr/bin/docker compose down
+
+[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>

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
-

pioneer3dx.urdf

@@ -0,0 +1,352 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from src/pioneer_description/urdf/pioneer3dx.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pioneer3dx" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.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 rpy="0 0 0" xyz="-0.045 0 0.148"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/chassis.stl"/>
+      </geometry>
+      <material name="ChassisRed">
+        <color rgba="0.851 0.0 0.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="-0.045 0 0.234"/>
+    <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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="-0.198 0 0.208"/>
+    <parent link="base_link"/>
+    <child link="front_sonar"/>
+  </joint>
+  <joint name="base_back_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.109 0 0.209"/>
+    <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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="-0.185 0 0.055"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" k_velocity="0" velocity="100"/>
+    <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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/caster_hubcap.stl"/>
+      </geometry>
+      <material name="caster_swivel">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="-0.026 0 -0.016"/>
+    <axis xyz="0 1 0"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" k_velocity="0" velocity="100"/>
+    <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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/caster_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.0375"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="left_hub">
+    <material value="Gazebo/Yellow"/>
+  </gazebo>
+  <joint name="caster_wheel_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.0035 0 -0.001"/>
+    <parent link="caster_wheel"/>
+    <child link="caster_hubcap"/>
+  </joint>
+  <link name="left_hub">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/left_hubcap.stl"/>
+      </geometry>
+      <material name="HubcapYellow">
+        <color rgba="1.0 0.811 0.151 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.09"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hub_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.15 0.08"/>
+    <parent link="base_link"/>
+    <child link="left_hub"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <link name="left_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/right_hubcap.stl"/>
+      </geometry>
+      <material name="HubcapYellow">
+        <color rgba="1.0 0.811 0.151 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.09"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="right_hub">
+    <material value="Gazebo/Yellow"/>
+  </gazebo>
+  <joint name="right_hub_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.15 0.08"/>
+    <parent link="base_link"/>
+    <child link="right_hub"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <link name="right_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0.040 0 0.0625"/>
+    <parent link="front_sonar"/>
+    <child link="laser"/>
+  </joint>
+  <link name="camera_frame">
+    <collision>
+      <origin rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0.040 0 0.1125"/>
+    <parent link="front_sonar"/>
+    <child link="camera_frame"/>
+  </joint>
+  <create>
+    <back_sonar parent="base_link"/>
+    <top_plate parent="base_link"/>
+  </create>
+  <gazebo>
+    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
+      <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>

ros_entrypoint.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+set -e
+
+# setup ros environment
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+source "install/setup.bash"
+exec "$@"

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 <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include "Aria/Aria.h"
+#include <utility>
+#include<string>
 
-#include "sensor_msgs/msg/point_cloud.hpp"
+#include "ros2aria/raiibot.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <Aria/Aria.h>
-#include "sensor_msgs/msg/joint_state.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "std_srvs/srv/empty.hpp"
+#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/bool.hpp"
-#include "std_msgs/msg/int8.hpp"
-#include "std_msgs/msg/float32.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,22 @@ public:
     ~Ros2Aria();
 
 private:
-    // robot
+    std::size_t id{0};
     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;
+    bool use_safety_system = true;
+    uint8_t num_of_sonars = 0;
+    std::string ros_namespace="";
 
-    // config
+    std::size_t extract_number_from_string(const std::string& str);
-    std::string sonar_frame_id;
 
-    // publishers
+    void handle_parameters();
     void publish();
 
     sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp);
@@ -39,9 +65,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 +78,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,25 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
     this->robot->pokeWatchdog();
 
     auto r = robot->getRobot();
-    r->setVel(x * 1e3);
+
+    if(use_safety_system){
+        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->setLatVel(y);
-    r->setRotVel(z * 180 / M_PI);
+    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 "ros2aria/ros2aria.hpp"
-#include "tf2/utils.h"
-#include "tf2/transform_datatypes.h"
 
-inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
+std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> Ros2Aria::handlePose(rclcpp::Time stamp) {
-{
+    nav_msgs::msg::Odometry odom_msg;
-    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;
 
     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),
+    odom_msg.twist.twist.linear.x = r->getVel() / 1000;
-                              pose.pose.pose);
+    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;
+    odom_msg.header.frame_id = ros_namespace + "/odom";
-    pose.twist.twist.linear.y = r->getLatVel() / 1000;
+    odom_msg.child_frame_id = ros_namespace + "/base_link";
-    pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
+    odom_msg.header.stamp = stamp;
 
-    pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
+    geometry_msgs::msg::TransformStamped transform;
-    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_frame_id
+    transform.header = odom_msg.header;
-    pose.header.stamp = stamp;
+    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();
 
-    sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
+    if(use_sonar){
-    publishSonar(sonarData);
+        sensor_msgs::msg::PointCloud sonarData = handleSonar(t);
-    publishSonarPointCloud2(sonarData);
+        publishSonar(sonarData);
+        publishSonarPointCloud2(sonarData);
+    }
 
-    nav_msgs::msg::Odometry pose = handlePose(t);
+    auto pose = handlePose(t);
-    publishPose(pose);
+    publishPose(pose.first);
+    publishTf(pose.second);
 
     sensor_msgs::msg::JointState wheels = handleWheels(t);
     publishWheels(wheels);
 
-    publishState(t);
+    publishState();
 }

src/ros2aria/src/raiibot.cpp

@@ -1,143 +1,108 @@
-#include <string>
+#include "ros2aria/raiibot.hpp"
-#include <memory>
-#include <iostream>
 
-#include "Aria/Aria.h"
-#include "rclcpp/rclcpp.hpp"
 
-using namespace std::chrono_literals;
+RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
+    this->robot = new ArRobot();
+    this->gripper = new ArGripper(this->robot);
+    this->node = node;
+    this->clock = rclcpp::Clock::make_shared();
 
-class RAIIBot
+    args = new ArArgumentBuilder();
-{
+    this->argparser = new ArArgumentParser(args);
-public:
+    argparser->loadDefaultArguments();  // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
-    typedef std::shared_ptr<RAIIBot> SharedPtr;
+    std::string port_arg = "-robotPort " + port;
-
+    args->add(port_arg.c_str());  // pass robot's serial port to Aria // TODO: use `port` variable.
-    RAIIBot(rclcpp::Node *node, std::string port)
+    // args->add("-robotLogPacketsReceived");                    // log received packets
-    {
+    // args->add("-robotLogPacketsSent");                        // log sent packets
-        this->robot = new ArRobot();
+    // args->add("-robotLogVelocitiesReceived"); // log received velocities
-        this->gripper = new ArGripper(this->robot);
+    // args->add("-robotLogMovementSent");
-        this->node = node;
+    // args->add("-robotLogMovementReceived");
-        this->clock = rclcpp::Clock::make_shared();
+    this->robotConn = new ArRobotConnector(argparser, robot);
-
+    if (!this->robotConn->connectRobot()) {
-        args = new ArArgumentBuilder();
+        // 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)");
-        this->argparser = new ArArgumentParser(args);
+        // rclcpp::shutdown();
-        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.
-        // 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())
-        {
-
-            // 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();
-        }
-
-        this->robot->runAsync(true);
-        this->robot->enableMotors();
-
-        this->startWatchdog();
     }
 
-    ~RAIIBot()
+    this->robot->runAsync(true);
-    {
+    this->robot->enableMotors();
-        std::cout << std::endl
-                  << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
 
-        if (this->robot != nullptr)
+    this->startWatchdog();
-        {
+}
-            this->robot->lock();
-            std::cout << "disabling motors" << std::endl;
-            this->robot->disableMotors();
-            std::cout << "disabled motors" << std::endl;
-            Aria::shutdown();
-        }
-        if (this->robotConn != nullptr)
-        {
-            std::cout << "disconnecting" << std::endl;
-            this->robotConn->disconnectAll();
-            std::cout << "disconnected" << std::endl;
-        }
 
-        if (this->args != nullptr)
+RAIIBot ::~RAIIBot() {
-            delete this->args;
+    std::cout << std::endl
-        if (this->argparser != nullptr)
+              << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
-            delete this->argparser;
+
-        if (this->robotConn != nullptr)
+    if (this->robot != nullptr) {
-            delete this->robotConn;
+        this->robot->lock();
-        if (this->robot != nullptr)
+        std::cout << "disabling motors" << std::endl;
-            delete this->robot;
+        this->robot->disableMotors();
+        std::cout << "disabled motors" << std::endl;
+        Aria::shutdown();
+    }
+    if (this->robotConn != nullptr) {
+        std::cout << "disconnecting" << std::endl;
+        this->robotConn->disconnectAll();
+        std::cout << "disconnected" << std::endl;
     }
 
-    ArRobot *getRobot()
+    if (this->args != nullptr)
-    {
+        delete this->args;
-        return this->robot;
+    if (this->argparser != nullptr)
-    }
+        delete this->argparser;
+    if (this->robotConn != nullptr)
+        delete this->robotConn;
+    if (this->robot != nullptr)
+        delete this->robot;
+}
 
-    ArGripper *getGripper()
+ArRobot *RAIIBot::getRobot() {
-    {
+    return this->robot;
-        return this->gripper;
+}
-    }
 
-    rclcpp::Clock::SharedPtr getClock()
+ArGripper *RAIIBot::getGripper() {
-    {
+    return this->gripper;
-        return this->clock;
+}
-    }
 
-    void lock()
+rclcpp::Clock::SharedPtr RAIIBot::getClock() {
-    {
+    return this->clock;
-        if (this->robot != nullptr)
+}
-            this->robot->lock();
-    }
 
-    void unlock()
+void RAIIBot::lock() {
-    {
+    if (this->robot != nullptr)
-        if (this->robot != nullptr)
+        this->robot->lock();
-            this->robot->unlock();
+}
-    }
 
-    void pokeWatchdog()
+void RAIIBot::unlock() {
-    {
+    if (this->robot != nullptr)
-        // this should probably be made thread safe, or something?
+        this->robot->unlock();
-        this->lastWatchdogPoke = this->clock->now();
+}
-    }
 
-private:
+void RAIIBot::pokeWatchdog() {
-    ArRobot *robot = nullptr;
+    // this should probably be made thread safe, or something?
-    ArRobotConnector *robotConn = nullptr;
+    this->lastWatchdogPoke = this->clock->now();
-    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();
-        return this->node->get_logger();
+}
-    }
 
-    void startWatchdog()
+void RAIIBot::startWatchdog() {
-    {
+    using namespace std::chrono_literals;
-        this->lastWatchdogPoke = clock->now();
+    this->lastWatchdogPoke = clock->now();
-        watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
+    watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
-    }
+}
 
-    void watchdog_cb()
+void RAIIBot::watchdog_cb() {
-    {
+    using namespace std::chrono_literals;
-        auto now = this->clock->now();
+
-        if (now - this->lastWatchdogPoke > 1s)
+    auto now = this->clock->now();
-        {
+    if (now - this->lastWatchdogPoke > 1s) {
-            std::lock_guard<RAIIBot> lock(*this);
+        std::lock_guard<RAIIBot> lock(*this);
-            auto r = this->getRobot();
+        auto r = this->getRobot();
-            r->setVel(0);
+        r->setVel(0);
-            r->setRotVel(0);
+        r->setRotVel(0);
-            if (r->hasLatVel())
+        if (r->hasLatVel())
-                r->setLatVel(0);
+            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,46 @@ Ros2Aria::Ros2Aria()
     : Node("ros2aria"),
       sensorCb(this, &Ros2Aria::publish)
 {
-    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); // TODO: use args for port
+    if(get_namespace() != std::string("/")){
+        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");
+
+    if(use_safety_system){
+        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));
+        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+            "scan", 10, std::bind(&Ros2Aria::scan_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));
 
-    sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
-    sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
-    pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
+    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();
 
-    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 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>("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 +61,39 @@ 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
+    ros_namespace = get_namespace() == std::string("/")? "" : get_namespace();
+    RCLCPP_INFO_STREAM(get_logger(), "NAMESPACE = " << ros_namespace);
+    RCLCPP_INFO_STREAM(get_logger(), "ID = " << 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);
+    declare_parameter("use_safety_system", true);
+    use_sonar = get_parameter("use_sonar").as_bool();
+    num_of_sonars = get_parameter("num_of_sonars").as_int();
+    use_safety_system = get_parameter("use_safety_system").as_bool();
+
+    RCLCPP_INFO_STREAM(get_logger(), "use_sonar = " << use_sonar);
+    RCLCPP_INFO_STREAM(get_logger(), "num_of_sonars = " << num_of_sonars);
+    RCLCPP_INFO_STREAM(get_logger(), "use_safety_system = " << use_safety_system);
 }
 
 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 = ros_namespace + "/front_sonar";
-    cloud.header.frame_id = this->sonar_frame_id;
 
     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 =  id;
-        robot_info_msg.robot_id.data = 6;
         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;
-
+        robot_info_msg.state.data = not user_stop;
-        // 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.clutch.data = r->areMotorsEnabled();
-        // TODO: actually look for obstacles
+        robot_info_msg.obstacle_detected.data = obstacle_too_close;
-        robot_info_msg.obstacle_detected.data = false;
 
-        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";
+    // robot_state_publisher gives namespace
-    wheels.name[1] = "Wheel_R";
+    wheels.name[0] = "left_wheel_joint";
-    wheels.position[0] = r->getLeftEncoder();
+    wheels.name[1] =  "right_wheel_joint";
-    wheels.position[1] = r->getRightEncoder();
-    wheels.velocity[0] = r->getLeftVel();
-    wheels.velocity[1] = r->getRightVel();
 
+    wheels.position[0] = encoder_to_rad(r->getLeftEncoder());
+    wheels.position[1] = encoder_to_rad(r->getRightEncoder());
+    wheels.velocity[0] = mm_per_sec_to_rad_per_sec(r->getLeftVel());
+    wheels.velocity[1] = mm_per_sec_to_rad_per_sec(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