Adding launchfiles, sample rosbags and improving documentation (#19)

.gitignore

@@ -54,4 +54,8 @@ CATKIN_IGNORE
 .pytest_cache/
 
 # VSCode
-workspace.code-workspace
+workspace.code-workspace
+
+# rosbag
+*.db3-shm
+*.db3-wal

massrobotics_amr_sender_py/launch/massrobotics_amr_sender_launch.py

@@ -0,0 +1,56 @@
+# Copyright 2021 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.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch.substitutions import ThisLaunchFileDir
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'mass_config_file',
+            default_value=[
+                PathJoinSubstitution([ThisLaunchFileDir(), '..', 'sample_config.yaml'])
+            ],
+            description='massrobotics_amr_sender node configuration file'
+        ),
+        Node(
+            package='massrobotics_amr_sender',
+            namespace='massrobotics_amr_sender',
+            executable='massrobotics_amr_node',
+            name='massrobotics_amr_sender',
+            parameters=[
+                {'config_file': LaunchConfiguration('mass_config_file')}
+            ]
+        ),
+    ])

massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py

@@ -191,6 +191,8 @@ async def _async_send_report(self, mass_object):
             self.logger.info(f"Reconnecting to server: {self._uri}")
             await self._async_connect()
 
+        mass_object.update_timestamp()
+
         try:
             await self._wss_conn.send(json.dumps(mass_object.data))
         except Exception as ex:
@@ -206,6 +208,13 @@ def _read_config_file(self, config_file_path):
 
     def _get_frame_id_from_header(self, msg):
         msg_frame_id = msg.header.frame_id
+
+        # Return no frame_id if the original message had no frame_id
+        # This is validated before looking up keys in order to avoid
+        # flooding logs with warning messages below
+        if not msg_frame_id:
+            return ''
+
         frame_id = self._config.mappings['rosFrameToPlanarDatumUUID'].get(msg_frame_id)
         if not frame_id:
             self.logger.warning(f"Couldn't find mapping for frame '{msg_frame_id}': {msg}")
@@ -262,7 +271,7 @@ def _callback_twist_stamped_msg(self, param_name, msg_field, data):
 
         self.mass_status_report.data[param_name] = {
             "linear": linear_vel,
-            "angle": {
+            "angular": {
                 "x": quat[0],
                 "y": quat[1],
                 "z": quat[2],

massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py

@@ -31,6 +31,7 @@
 from pathlib import Path
 import json
 import jsonschema
+from datetime import timezone
 
 # MassRobotics AMR Interop required properties
 # for both Identity and Status report objects.
@@ -63,14 +64,14 @@ def __init__(self, **kwargs) -> None:
             raise ValueError(f"Missing mandatory IdentityReport parameter {MASS_REPORT_UUID}")
 
         self.data = {MASS_REPORT_UUID: kwargs[MASS_REPORT_UUID]}
-        self._update_timestamp()
+        self.update_timestamp()
         self.schema = self._load_schema()
 
-    def _update_timestamp(self):
+    def update_timestamp(self):
         # As per Mass example, data format is ISO8601
         # with timezone offset e.g. 2012-04-21T18:25:43-05:00
-        self.data[MASS_REPORT_TIMESTAMP] = datetime.now() \
-            .replace(microsecond=0).astimezone().isoformat()
+        self.data[MASS_REPORT_TIMESTAMP] = datetime.now(tz=timezone.utc) \
+            .replace(microsecond=0).isoformat()
 
     def update_parameter(self, name, value):
         """
@@ -90,7 +91,7 @@ def update_parameter(self, name, value):
 
         """
         self.data[name] = value
-        self._update_timestamp()
+        self.update_timestamp()
 
     def _load_schema(self):
         cwd = Path(__file__).resolve().parent

massrobotics_amr_sender_py/sample/README.md

@@ -0,0 +1,37 @@
+# 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
+$ ros2 bag info rosbag/rosbag_demo.db3
+[INFO] [1625526486.600008195] [rosbag2_storage]: Opened database 'rosbag_demo.db3' for READ_ONLY.
+
+Files:             rosbag/rosbag_demo.db3
+Bag size:          4.2 MiB
+Storage id:        sqlite3
+Duration:          111.390s
+Start:             Jul  5 2021 19:45:30.357 (1625525130.357)
+End:               Jul  5 2021 19:47:21.747 (1625525241.747)
+Messages:          12282
+Topic information: Topic: /battery | Type: sensor_msgs/msg/BatteryState | Count: 111 | Serialization Format: cdr
+                   Topic: /battery_runtime | Type: std_msgs/msg/Float32 | Count: 37 | Serialization Format: cdr
+                   Topic: /load_perc_available | Type: std_msgs/msg/Float32 | Count: 22 | Serialization Format: cdr
+                   Topic: /local_plan | Type: nav_msgs/msg/Path | Count: 1453 | Serialization Format: cdr
+                   Topic: /location | Type: geometry_msgs/msg/PoseStamped | Count: 5273 | Serialization Format: cdr
+                   Topic: /mode | Type: std_msgs/msg/String | Count: 5 | Serialization Format: cdr
+                   Topic: /plan | Type: nav_msgs/msg/Path | Count: 74 | Serialization Format: cdr
+                   Topic: /troubleshooting/errorcodes | Type: std_msgs/msg/String | Count: 37 | Serialization Format: cdr
+                   Topic: /velocity | Type: geometry_msgs/msg/TwistStamped | Count: 5270 | Serialization Format: cdr
+```
+
+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.py` 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
+ros2 launch massrobotics_amr_sender_rosbag_launch.py
+```

massrobotics_amr_sender_py/sample/config.yaml

@@ -0,0 +1,151 @@
+# ====================================================================
+# MassRobotics AMR Interoperability Standard sender configuration file
+# ====================================================================
+#
+# Parameters that are used to configure ROS2 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
+# `baseRobotEnvelope` (full spec https://github.com/MassRobotics-AMR/AMR_Interop_Standard/).
+# 
+# Translation to AMR Interop Standard messages might be direct (i.e. a string to a report message
+# field) or complex in case of ROS2 message having data that maps to many AMR report message fields.
+# For this reason, some configuration parameters below expect a particular ROS2 message type e.g.
+# fields on a ROS2 message of type `sensor_msgs/msg/BatteryState` are translated into AMR Interop
+# Status Report fields `batteryPercentage`, `remainingRunTime` and `loadPercentageStillAvailable`.
+# 
+# In addition to local values i.e. strings or objects, the `mappings` section supports a variety of
+# sources from where the parameter value can be obtained: `envVar`, `rosTopic` and `rosParameter`.
+
+config:
+  server: "ws://localhost:3000"
+  mappings:
+    # Mapping definition for Identity report messages
+
+    # UUID that all subsequent messages should reference.
+    # It is obtained from the environment variable ``MY_UUID``. If the variable
+    # is not defined or if it has no value an error will be thrown.
+    uuid:
+      valueFrom:
+        envVar: MY_UUID
+
+    # Robot manufacturer name 
+    manufacturerName: Spoonlift
+
+    # Robot model
+    robotModel: "spoony1.0"
+
+    # Unique robot identifier
+    robotSerialNumber: "2172837"
+
+    # Robot footprint based on orientation, centered on current location
+    baseRobotEnvelope:
+      x: 2
+      y: 1
+      z: 3
+
+    # Robot max speed in m/s
+    maxSpeed: 2.5
+
+    # Estimated runtime in hours
+    maxRunTime: 8
+
+    # Emergency contact - preferrably phone number
+    emergencyContactInformation: "555-5555"
+
+    # Robot charger type
+    chargerType: "24V plus"
+
+    # Vendor that supplied robot
+    supportVendorName: "We-B-Robots"
+
+    # Vendor contact information
+    supportVendorContactInformation: "[email protected]"
+
+    # Link to product documenation
+    productDocumentation: "https://spoon.lift/support/docs/spoony1.0"
+
+    # Link to thumbnail graphic stored as PNG
+    thumbnailImage: "https://spoon.lift/media/spoony1.0.png"
+
+    # Cargo description
+    cargoType: "Anything solid or liquid"
+
+    # Max volume of cargo in meters
+    cargoMaxVolume:
+      x: 2
+      y: 2
+      z: 1
+
+    # Max weight of cargo in kg
+    cargoMaxWeight: "4000"
+
+
+    # Mapping definition for Status report messages
+
+    # Current action the robot is performing
+    operationalState:
+      valueFrom:
+        rosTopic: /mode
+        msgType: std_msgs/String
+
+    # Current location of AMR
+    location:
+      valueFrom:
+        rosTopic: /location
+        msgType: geometry_msgs/msg/PoseStamped
+
+    # Current velocity of AMR
+    velocity:
+      valueFrom:
+        rosTopic: /velocity
+        msgType: geometry_msgs/msg/TwistStamped
+
+    # Percentage of battery remaining
+    # The ``msgField`` indicates a message field where the battery
+    # percentage value will be extracted
+    batteryPercentage:
+      valueFrom:
+        rosTopic: /battery
+        msgType: sensor_msgs/msg/BatteryState
+        msgField: percentage
+
+    # Estimated remaining runtime in hours
+    remainingRunTime:
+      valueFrom:
+        rosTopic: /battery_runtime
+        msgType: std_msgs/Float32
+
+    # Percentage of capacity still available
+    loadPercentageStillAvailable:
+      valueFrom:
+        rosTopic: /load_perc_available
+        msgType: std_msgs/Float32
+
+    # List of current error states
+    # Error codes are expected to be comma-separated strings published on a topic of
+    # type std_msgs/msg/String. Those errors are then transformed into an array, as
+    # required on the MassRobotics standard.
+    errorCodes:
+      valueFrom:
+        rosTopic: /troubleshooting/errorcodes
+        msgType: std_msgs/msg/String
+
+    # Target destination(s) of Automated Guided Vehicle (AGV)
+    destinations:
+      valueFrom:
+        rosTopic: /plan
+        msgType: nav_msgs/Path
+
+    # Short term path of Automated Guided Vehicle (AGV) ~10 sec
+    path:
+      valueFrom:
+        rosTopic: /local_plan
+        msgType: nav_msgs/Path
+
+    # Id of planarDatum AMR is referencing
+    rosFrameToPlanarDatumUUID:
+      # required since Mass expects frames to be referenced using uuids
+      map: "196522ad-51fa-4796-9b31-a35b0f8d0b54"
+      floor1: "096522ad-61fa-4796-9b31-e35b0f8d0b26"
+      odom: "6ec7a6d0-21a9-4f04-b680-e7c640a0687e"

massrobotics_amr_sender_py/sample/massrobotics_amr_sender_rosbag_launch.py

@@ -0,0 +1,68 @@
+# Copyright 2021 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.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch.substitutions import ThisLaunchFileDir
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'mass_config_file',
+            default_value=[
+                PathJoinSubstitution([ThisLaunchFileDir(), 'config.yaml'])
+            ],
+            description='massrobotics_amr_sender node configuration file'
+        ),
+        DeclareLaunchArgument(
+            'bag_path',
+            default_value=[
+                PathJoinSubstitution([ThisLaunchFileDir(), 'rosbag', 'rosbag_demo.db3'])
+            ],
+            description='Path for ROS 2 data bag'
+        ),
+        Node(
+            package='massrobotics_amr_sender',
+            namespace='massrobotics_amr_sender',
+            executable='massrobotics_amr_node',
+            name='massrobotics_amr_sender',
+            parameters=[
+                {'config_file': LaunchConfiguration('mass_config_file')}
+            ]
+        ),
+        ExecuteProcess(
+            cmd=['ros2', 'bag', 'play', '-l', LaunchConfiguration('bag_path')],
+            output='screen',
+            name='rosbag_demo'
+        )
+    ])

massrobotics_amr_sender_py/sample/rosbag/map/README.md

@@ -0,0 +1,3 @@
+# rosbag demo map
+
+This is the map obtained from `turtlebot3` [SLAM simulation](https://emanual.robotis.com/docs/en/platform/turtlebot3/slam_simulation/)