diff --git a/.gitignore b/.gitignore index 105ef0a..d4f5c5e 100644 --- a/.gitignore +++ b/.gitignore @@ -54,4 +54,8 @@ CATKIN_IGNORE .pytest_cache/ # VSCode -workspace.code-workspace \ No newline at end of file +workspace.code-workspace + +# rosbag +*.db3-shm +*.db3-wal diff --git a/massrobotics_amr_sender_py/launch/massrobotics_amr_sender_launch.py b/massrobotics_amr_sender_py/launch/massrobotics_amr_sender_launch.py new file mode 100644 index 0000000..034fdde --- /dev/null +++ b/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')} + ] + ), + ]) diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py b/massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py index 0aed77f..e1db822 100644 --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py +++ b/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], diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py b/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py index b3b83b8..6b7d921 100644 --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py +++ b/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 diff --git a/massrobotics_amr_sender_py/sample/README.md b/massrobotics_amr_sender_py/sample/README.md new file mode 100644 index 0000000..6a0cdd8 --- /dev/null +++ b/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 +``` diff --git a/massrobotics_amr_sender_py/sample/config.yaml b/massrobotics_amr_sender_py/sample/config.yaml new file mode 100644 index 0000000..69aba69 --- /dev/null +++ b/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: "support@we-b-robots.com" + + # 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" diff --git a/massrobotics_amr_sender_py/sample/massrobotics_amr_sender_rosbag_launch.py b/massrobotics_amr_sender_py/sample/massrobotics_amr_sender_rosbag_launch.py new file mode 100644 index 0000000..fd4dd0d --- /dev/null +++ b/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' + ) + ]) diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/README.md b/massrobotics_amr_sender_py/sample/rosbag/map/README.md new file mode 100644 index 0000000..64ee4f0 --- /dev/null +++ b/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/) diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/map.pgm b/massrobotics_amr_sender_py/sample/rosbag/map/map.pgm new file mode 100644 index 0000000..4b5941c Binary files /dev/null and b/massrobotics_amr_sender_py/sample/rosbag/map/map.pgm differ diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/map.yaml b/massrobotics_amr_sender_py/sample/rosbag/map/map.yaml new file mode 100644 index 0000000..6b7e1f2 --- /dev/null +++ b/massrobotics_amr_sender_py/sample/rosbag/map/map.yaml @@ -0,0 +1,7 @@ +image: ./map.pgm +mode: trinary +resolution: 0.05 +origin: [-1.25, -2.39, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/massrobotics_amr_sender_py/sample/rosbag/metadata.yaml b/massrobotics_amr_sender_py/sample/rosbag/metadata.yaml new file mode 100644 index 0000000..1336d5b --- /dev/null +++ b/massrobotics_amr_sender_py/sample/rosbag/metadata.yaml @@ -0,0 +1,67 @@ +rosbag2_bagfile_information: + version: 4 + storage_identifier: sqlite3 + relative_file_paths: + - rosbag_demo.db3 + duration: + nanoseconds: 111390519961 + starting_time: + nanoseconds_since_epoch: 1625525130357239385 + message_count: 12282 + topics_with_message_count: + - topic_metadata: + name: /troubleshooting/errorcodes + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 37 + - topic_metadata: + name: /velocity + type: geometry_msgs/msg/TwistStamped + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 5270 + - topic_metadata: + name: /mode + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 5 + - topic_metadata: + name: /location + type: geometry_msgs/msg/PoseStamped + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 5273 + - topic_metadata: + name: /battery + type: sensor_msgs/msg/BatteryState + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 111 + - topic_metadata: + name: /battery_runtime + type: std_msgs/msg/Float32 + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 37 + - topic_metadata: + name: /plan + type: nav_msgs/msg/Path + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 74 + - topic_metadata: + name: /load_perc_available + type: std_msgs/msg/Float32 + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 22 + - topic_metadata: + name: /local_plan + type: nav_msgs/msg/Path + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 1453 + compression_format: "" + compression_mode: "" diff --git a/massrobotics_amr_sender_py/sample/rosbag/rosbag_demo.db3 b/massrobotics_amr_sender_py/sample/rosbag/rosbag_demo.db3 new file mode 100644 index 0000000..5f864a3 Binary files /dev/null and b/massrobotics_amr_sender_py/sample/rosbag/rosbag_demo.db3 differ diff --git a/massrobotics_amr_sender_py/sample/synthetic/node.py b/massrobotics_amr_sender_py/sample/synthetic/node.py new file mode 100644 index 0000000..220edbc --- /dev/null +++ b/massrobotics_amr_sender_py/sample/synthetic/node.py @@ -0,0 +1,132 @@ +# 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. + + +import rclpy +from rclpy.node import Node +import time +from std_msgs.msg import String +from std_msgs.msg import Float32 +from sensor_msgs.msg import BatteryState +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import TwistStamped +from std_msgs.msg import Header +from builtin_interfaces.msg import Time + + +class SampleDataNode(Node): + + def __init__(self): + super().__init__('SampleDataNode') + # publishers for fake data + self.errors_publisher = self.create_publisher(String, '/troubleshooting/errorcodes', 10) + self.state_publisher = self.create_publisher(String, '/mode', 10) + self.battery_publisher = self.create_publisher(BatteryState, '/battery', 10) + self.battery_runtime_publisher = self.create_publisher(Float32, '/battery_runtime', 10) + self.load_perc_available_publisher = self.create_publisher( + Float32, '/load_perc_available', 10) + self.errors = '' + self.robot_state = 'navigating' + self.battery_perc = 90 + + self.create_timer(20, self.flip_state_callback) + self.create_timer(1, self.battery_callback) + self.create_timer(7, self.set_error_callback) + self.create_timer(3, self.send_error_callback) + self.create_timer(3, self.battery_runtime_callback) + self.create_timer(5, self.load_perc_available_callback) + + # transforming data from raw rosbag + self.odom_listener = self.create_subscription( + Odometry, + '/odom', + self.odom_to_location_and_velocity_callback, + 10) + self.location_publisher = self.create_publisher(PoseStamped, '/location', 10) + self.velocity_publisher = self.create_publisher(TwistStamped, '/velocity', 10) + + def set_error_callback(self): + self.errors = 'error_194,error_1' + + def send_error_callback(self): + msg = String() + msg.data = self.errors + self.errors_publisher.publish(msg) + self.get_logger().info('Publishing errors: "%s"' % msg.data) + self.errors = '' + + def flip_state_callback(self): + self.robot_state = 'navigating' if self.robot_state == 'charging' else 'charging' + msg = String() + msg.data = self.robot_state + self.state_publisher.publish(msg) + self.get_logger().info('Publishing state: "%s"' % msg.data) + + def battery_callback(self): + self.battery_perc = self.battery_perc + 1 \ + if self.robot_state == 'charging' else self.battery_perc - 1 + msg = BatteryState() + msg.percentage = float(self.battery_perc) + self.battery_publisher.publish(msg) + self.get_logger().info('Publishing battery: "%s"' % msg) + + def battery_runtime_callback(self): + msg = Float32(data=self.battery_perc / 50) # at 90% the remaing time ~1.8 hours + self.battery_runtime_publisher.publish(msg) + self.get_logger().info('Publishing battery runtime: "%s"' % msg) + + def load_perc_available_callback(self): + msg = Float32(data=77.1) + self.load_perc_available_publisher.publish(msg) + self.get_logger().info('Publishing load percentage available: "%s"' % msg) + + def odom_to_location_and_velocity_callback(self, msg): + # msg is type Odometry + pose = msg.pose.pose + pose = PoseStamped( + header=Header(stamp=Time(sec=int(time.time()))), + pose=pose) + self.location_publisher.publish(pose) + twist = msg.twist.twist + twist = TwistStamped( + header=Header(stamp=Time(sec=int(time.time()))), + twist=twist) + self.velocity_publisher.publish(twist) + + +def main(args=None): + rclpy.init(args=args) + sample_data_node = SampleDataNode() + rclpy.spin(sample_data_node) + sample_data_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/massrobotics_amr_sender_py/sample_config.yaml b/massrobotics_amr_sender_py/sample_config.yaml index d2d5e50..281f547 100644 --- a/massrobotics_amr_sender_py/sample_config.yaml +++ b/massrobotics_amr_sender_py/sample_config.yaml @@ -20,57 +20,109 @@ config: server: "ws://localhost:3000" mappings: - # Identity + # 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: "support@we-b-robots.com" + + # 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" - # Status + + + # Mapping definition for Status report messages + + # Current action the robot is performing operationalState: valueFrom: rosTopic: /we_b_robots/mode msgType: std_msgs/String + + # Current location of AMR location: valueFrom: rosTopic: /move_base_simple/goal msgType: geometry_msgs/msg/PoseStamped + + # Current velocity of AMR velocity: valueFrom: rosTopic: /good_sensors/vel 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: /good_sensors/bat msgType: sensor_msgs/msg/BatteryState msgField: percentage + + # Estimated remaining runtime in hours remainingRunTime: valueFrom: rosTopic: /good_sensors/bat_remaining msgType: std_msgs/Float32 + + # Percentage of capacity still available loadPercentageStillAvailable: valueFrom: rosTopic: /good_sensors/load 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. @@ -78,14 +130,20 @@ config: valueFrom: rosTopic: /troubleshooting/errorcodes msgType: std_msgs/msg/String + + # Target destination(s) of Automated Guided Vehicle (AGV) destinations: valueFrom: rosTopic: /we_b_robots/destinations msgType: nav_msgs/Path + + # Short term path of Automated Guided Vehicle (AGV) ~10 sec path: valueFrom: rosTopic: /magic_nav/path 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" diff --git a/massrobotics_amr_sender_py/test/test_mass_interop.py b/massrobotics_amr_sender_py/test/test_mass_interop.py index faadcad..b7f6d4a 100644 --- a/massrobotics_amr_sender_py/test/test_mass_interop.py +++ b/massrobotics_amr_sender_py/test/test_mass_interop.py @@ -104,7 +104,7 @@ def mock_robot_id(monkeypatch): @pytest.fixture def event_loop(): # Fixture for running the async method for sending the Mass object - loop = asyncio.get_event_loop() + loop = asyncio.new_event_loop() yield loop loop.close() @@ -182,7 +182,7 @@ def test_massrobotics_amr_node_init(): 'property': 'velocity', 'value': { 'linear': 1, - 'angle': { + 'angular': { 'w': 0.9937606691655042, 'x': 0.09970865087213879, 'y': 0.04972948160146044,