inorbit-ai · FlorGrosso · May 31, 2022 · May 18, 2022 · May 18, 2022 · May 19, 2022
diff --git a/.gitignore b/.gitignore
@@ -55,6 +55,7 @@ CATKIN_IGNORE
 
 # VSCode
 workspace.code-workspace
+*.vscode/
 
 # rosbag
 *.db3-shm

diff --git a/README.md b/README.md
@@ -10,15 +10,14 @@ standards, with a focus on AMRs (Autonomous Mobile Robots).
 
 The following packages are included in this repository:
 
+### Mass Robotics AMR Interop Sender for ROS1
 
-### Mass Robotics AMR Interop Sender for ROS2
-
-The [massrobotics_amr_sender_py](https://github.com/inorbit-ai/ros_amr_interop/tree/foxy-devel/massrobotics_amr_sender_py#readme)
-package provides a ROS2 node written in Python that takes input from a
-ROS2 system and publishes it to a [Mass Robotics Interop compliant
+The [massrobotics_amr_sender](https://github.com/inorbit-ai/ros_amr_interop/tree/foxy-devel/massrobotics_amr_sender_py#readme)
+package provides a ROS1 node written in Python that takes input from a
+ROS1 system and publishes it to a [Mass Robotics Interop compliant
 Receiver](https://github.com/MassRobotics-AMR/AMR_Interop_Standard/tree/main/MassRobotics-AMR-Receiver).
 
-Mapping of different data elements from the ROS2 system into Mass
+Mapping of different data elements from the ROS1 system into Mass
 Robotics Interop messages can be customized through a YAML configuration
 file.
 

diff --git a/massrobotics_amr_sender/CMakeLists.txt b/massrobotics_amr_sender/CMakeLists.txt
@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(massrobotics_amr_sender)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+
+catkin_python_setup()
+
+catkin_package(
+  INCLUDE_DIRS
+  LIBRARIES
+  CATKIN_DEPENDS
+    rospy
+  DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+#############
+## Install ##
+#############
+
+install(
+  DIRECTORY
+  	scripts/
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY
+    launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+catkin_install_python(
+  PROGRAMS
+    scripts/massrobotics_amr_sender_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
diff --git a/massrobotics_amr_sender_py/CONTRIBUTING.md → massrobotics_amr_sender/CONTRIBUTING.md b/massrobotics_amr_sender_py/CONTRIBUTING.md → massrobotics_amr_sender/CONTRIBUTING.md
diff --git a/massrobotics_amr_sender/README.md b/massrobotics_amr_sender/README.md
@@ -0,0 +1,44 @@
+# massrobotics_amr_sender
+
+Configuration-based ROS package for sending MassRobotics [AMR Interop Standard messages](https://github.com/MassRobotics-AMR/AMR_Interop_Standard) to compliant receivers.
+
+# Package installation
+
+## From binary packages
+
+Coming soon.
+
+## Building from source
+
+Make sure `ros` is installed properly. Then clone this repository inside your `src` folder on your local workspace and build the package executing the following commands:
+
+```bash
+# Create a ROS workspace and go into it - if you don't have one already
+mkdir -p ~/ros_ws/src && cd ros_ws/
+# Clone the repo inside the workspace
+git clone --branch noetic-devel https://github.com/inorbit-ai/ros_amr_interop.git ./src
+# Install dependencies
+rosdep update && rosdep install --ignore-src --from-paths src/
+# Run the build
+catkin config --install
+catkin build
+```
+# Node configuration
+
+A configuration file must be provided to define how ROS1 messages are mapped to different AMR Interop Standard messages. A [sample_config.yaml](https://github.com/inorbit-ai/ros_amr_interop/blob/foxy-devel/massrobotics_amr_sender_py/sample_config.yaml) is provided for reference.
+
+# Running the sender node
+
+The node takes the MassRobotics AMR config file path as parameter. If not provided, it is assumed the file is on the current directory.
+
+```bash
+# Remember to source the ROS2 environment from the binary installation or your workspace overlay
+source devel/setup.bash
+# Launch the node pointing to your configuration file
+roslaunch massrobotics_amr_sender massrobotics_amr_sender.launch config_file:=/path/to/config.yaml
+```
+
+
+# Tests
+
+TODO.
diff --git a/massrobotics_amr_sender/launch/massrobotics_amr_sender.launch b/massrobotics_amr_sender/launch/massrobotics_amr_sender.launch
@@ -0,0 +1,38 @@
+<!--
+  Copyright 2022 InOrbit, Inc.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+
+     * Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+     * Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+     * Neither the name of the InOrbit, Inc. nor the names of its
+       contributors may be used to endorse or promote products derived from
+       this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+-->
+
+<launch>
+	<arg name="config_file" default="$(find massrobotics_amr_sender)/params/sample_config.yaml"/>
+
+	<!-- Launch Node -->
+	<node name="massrobotics_amr_sender" pkg="massrobotics_amr_sender" type="massrobotics_amr_sender_node.py" output="screen">
+      <param name="config_file" value="$(arg config_file)" type="string" />
+
+    </node>
+</launch>
diff --git a/massrobotics_amr_sender_py/package.xml → massrobotics_amr_sender/package.xml b/massrobotics_amr_sender_py/package.xml → massrobotics_amr_sender/package.xml
@@ -1,31 +1,32 @@
 <?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">
+<package format="2">
   <name>massrobotics_amr_sender</name>
   <version>1.0.0</version>
   <description>MassRobotics AMR Interop Sender</description>
   <maintainer email="[email protected]">InOrbit</maintainer>
   <license>3-Clause BSD License</license>
 
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <build_depend>rospy</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+
   <exec_depend>python3-websockets</exec_depend>
   <!-- kdl is used for processing Twist messages -->
   <exec_depend>tf2_kdl</exec_depend>
   <exec_depend>python3-pykdl</exec_depend>
-  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rospy</exec_depend>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <!-- common_interfaces for nav_msgs -->
-  <test_depend>common_interfaces</test_depend>
   <test_depend>python3-jsonschema</test_depend>
   <test_depend>python3-mock</test_depend>
   <test_depend>python3-pep8</test_depend>
   <test_depend>python3-pytest</test_depend>
   <test_depend>python3-pytest-mock</test_depend>
   <test_depend>python3-yaml</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
 </package>
diff --git a/...s_amr_sender_py/params/sample_config.yaml → ...tics_amr_sender/params/sample_config.yaml b/...s_amr_sender_py/params/sample_config.yaml → ...tics_amr_sender/params/sample_config.yaml
@@ -18,7 +18,7 @@
 # sources from where the parameter value can be obtained: `envVar`, `rosTopic` and `rosParameter`.
 
 config:
-  server: "ws://localhost:3000"
+  server: "ws://localhost:3006"
   mappings:
     # Mapping definition for Identity report messages
 

diff --git a/...ender_py/resource/massrobotics_amr_sender → ...r_sender/resource/massrobotics_amr_sender b/...ender_py/resource/massrobotics_amr_sender → ...r_sender/resource/massrobotics_amr_sender
diff --git a/massrobotics_amr_sender/sample/README.md b/massrobotics_amr_sender/sample/README.md
@@ -0,0 +1,42 @@
+# Sample data for massrobotics_amr_sender
+
+Scripts, launch files, recordings and other tools for demoing and testing the `massrobotics_amr_sender` node.
+
+The `rosbag` folder contains a stripped rosbag based on `turtlebot3`. It was built using on the [Gazebo](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/#gazebo-simulation), [SLAM](https://emanual.robotis.com/docs/en/platform/turtlebot3/slam_simulation/) and [Navigation](https://emanual.robotis.com/docs/en/platform/turtlebot3/nav_simulation/) simulations.
+
+```bash
+$ rosbag info rosbag_demo.bag
+path:        rosbag_demo.bag
+version:     2.0
+duration:    1:51s (111s)
+start:       Jul 05 2021 19:45:30.36 (1625525130.36)
+end:         Jul 05 2021 19:47:21.75 (1625525241.75)
+size:        3.8 MB
+messages:    12282
+compression: none [4/4 chunks]
+types:       geometry_msgs/PoseStamped  [d3812c3cbc69362b77dc0b19b345f8f5]
+             geometry_msgs/TwistStamped [98d34b0043a2093cf9d9345ab6eef12e]
+             nav_msgs/Path              [6227e2b7e9cce15051f669a5e197bbf7]
+             sensor_msgs/BatteryState   [4ddae7f048e32fda22cac764685e3974]
+             std_msgs/Float32           [73fcbf46b49191e672908e50842a83d4]
+             std_msgs/String            [992ce8a1687cec8c8bd883ec73ca41d1]
+topics:      /battery                       111 msgs    : sensor_msgs/BatteryState
+             /battery_runtime                37 msgs    : std_msgs/Float32
+             /load_perc_available            22 msgs    : std_msgs/Float32
+             /local_plan                   1453 msgs    : nav_msgs/Path
+             /location                     5273 msgs    : geometry_msgs/PoseStamped
+             /mode                            5 msgs    : std_msgs/String
+             /plan                           74 msgs    : nav_msgs/Path
+             /troubleshooting/errorcodes     37 msgs    : std_msgs/String
+             /velocity                     5270 msgs    : geometry_msgs/TwistStamped
+```
+
+Messages on topics such as `/plan` and `/local_plan` were kept unchanged while messages on `/location` and `/velocity` were crafted by creating `PoseStamped` and `TwistStamped` messages using data from `Odometry` messages on topic `/odom`. The messages on the remaning topics `/battery`, `/battery_runtime`, `/load_perc_available`, `/mode` and `/troubleshooting/errorcodes` as well as all the transformation described above were generated with a small ROS2 node that is available at `synthetic/node.py`.
+
+## How to run
+
+The `massrobotics_amr_sender_rosbag_launch.launch` launch file describes a `massrobotics_amr_sender` node that uses a configuration file customized for the sample rosbag, and also plays the rosbag in loop mode so the different node callbacks are executed.
+
+```bash
+roslaunch massrobotics_amr_sender massrobotics_amr_sender_rosbag_launch.launch
+```
diff --git a/...robotics_amr_sender_py/sample/config.yaml → massrobotics_amr_sender/sample/config.yaml b/...robotics_amr_sender_py/sample/config.yaml → massrobotics_amr_sender/sample/config.yaml
@@ -2,7 +2,7 @@
 # MassRobotics AMR Interoperability Standard sender configuration file
 # ====================================================================
 #
-# Parameters that are used to configure ROS2 node for connecting to MassRobotics compatible servers.
+# Parameters that are used to configure ROS1 node for connecting to MassRobotics compatible servers.
 # The `server` section expects a string with a WebSocket server URI, while the `mappings` section
 # contains a list of paramaters for configuring the ROS2 node. As per AMR Interop Standard,
 # mandatory parameters are `uuid`, `manufacturerName`, `robotModel`, `robotSerialNumber` and
@@ -18,7 +18,7 @@
 # sources from where the parameter value can be obtained: `envVar`, `rosTopic` and `rosParameter`.
 
 config:
-  server: "ws://localhost:3000"
+  server: "ws://localdev.com:3006/receiver/eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJjb21wYW55QXBpS2V5IjoibU5jUm9pQTRTbUd3RG03MiIsImlhdCI6MTY1MjgxOTUyM30.c0NAyaTpRKVq_pz_0zaYhLo1nYAP2QU52RmV8HjE_Oo"
   mappings:
     # Mapping definition for Identity report messages
 

diff --git a/massrobotics_amr_sender/sample/massrobotics_amr_sender_rosbag_launch.launch b/massrobotics_amr_sender/sample/massrobotics_amr_sender_rosbag_launch.launch
@@ -0,0 +1,40 @@
+<!--
+  Copyright 2022 InOrbit, Inc.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+
+     * Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+     * Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+     * Neither the name of the InOrbit, Inc. nor the names of its
+       contributors may be used to endorse or promote products derived from
+       this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.
+-->
+
+<launch>
+	<arg name="mass_config_file" default="$(find massrobotics_amr_sender)/sample/config.yaml"/>
+    <arg name="bag_path" default="$(find massrobotics_amr_sender)/sample/rosbag/rosbag_demo.bag"/>
+
+	<!-- Launch Node -->
+	<node name="massrobotics_amr_sender" pkg="massrobotics_amr_sender" type="massrobotics_amr_sender_node.py" output="screen">
+      <param name="config_file" value="$(arg mass_config_file)" type="string" />
+    </node>
+
+    <node name="rosbag" pkg="rosbag" type="play" output="screen" args="--loop $(arg bag_path)" required="true"/>
+</launch>
diff --git a/...amr_sender_py/sample/rosbag/map/README.md → ...cs_amr_sender/sample/rosbag/map/README.md b/...amr_sender_py/sample/rosbag/map/README.md → ...cs_amr_sender/sample/rosbag/map/README.md
diff --git a/...s_amr_sender_py/sample/rosbag/map/map.pgm → ...tics_amr_sender/sample/rosbag/map/map.pgm b/...s_amr_sender_py/sample/rosbag/map/map.pgm → ...tics_amr_sender/sample/rosbag/map/map.pgm
diff --git a/..._amr_sender_py/sample/rosbag/map/map.yaml → ...ics_amr_sender/sample/rosbag/map/map.yaml b/..._amr_sender_py/sample/rosbag/map/map.yaml → ...ics_amr_sender/sample/rosbag/map/map.yaml
diff --git a/massrobotics_amr_sender/sample/rosbag/rosbag_demo.bag b/massrobotics_amr_sender/sample/rosbag/rosbag_demo.bag