diff --git a/.github/workflows/code-quality.yaml b/.github/workflows/code-quality.yaml index f3af8d5..209d88d 100644 --- a/.github/workflows/code-quality.yaml +++ b/.github/workflows/code-quality.yaml @@ -3,7 +3,7 @@ name: Code quality on: pull_request: push: - branches: [foxy-devel] + branches: [foxy-devel, noetic-devel] jobs: pre-commit: diff --git a/.gitignore b/.gitignore index d4f5c5e..09bda50 100644 --- 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 index 04d4667..0aea609 100644 --- 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/noetic-devel/massrobotics_amr_sender#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. @@ -43,7 +42,6 @@ The following is an incomplete and growing list of such related topics: We expect to keep curating the set of relevant topics with the contribution of the community. - ## Development Install [pre-commit](https://pre-commit.com/) in your computer and then set it up by running `pre-commit install` at the root of the cloned project. diff --git a/massrobotics_amr_sender/CMakeLists.txt b/massrobotics_amr_sender/CMakeLists.txt new file mode 100644 index 0000000..0fdf03f --- /dev/null +++ b/massrobotics_amr_sender/CMakeLists.txt @@ -0,0 +1,53 @@ +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 b/massrobotics_amr_sender/CONTRIBUTING.md similarity index 100% rename from massrobotics_amr_sender_py/CONTRIBUTING.md rename to massrobotics_amr_sender/CONTRIBUTING.md diff --git a/massrobotics_amr_sender/README.md b/massrobotics_amr_sender/README.md new file mode 100644 index 0000000..eabc023 --- /dev/null +++ b/massrobotics_amr_sender/README.md @@ -0,0 +1,43 @@ +# 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/noetic-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 ROS 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 new file mode 100644 index 0000000..c5841b5 --- /dev/null +++ b/massrobotics_amr_sender/launch/massrobotics_amr_sender.launch @@ -0,0 +1,37 @@ + + + + + + + + + + diff --git a/massrobotics_amr_sender_py/package.xml b/massrobotics_amr_sender/package.xml similarity index 60% rename from massrobotics_amr_sender_py/package.xml rename to massrobotics_amr_sender/package.xml index e82ff41..c9f7a29 100644 --- a/massrobotics_amr_sender_py/package.xml +++ b/massrobotics_amr_sender/package.xml @@ -1,31 +1,32 @@ - - + massrobotics_amr_sender 1.0.0 MassRobotics AMR Interop Sender InOrbit 3-Clause BSD License + catkin + + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + + rospy + + rospy + python3-websockets tf2_kdl python3-pykdl - rclpy + rospy - ament_copyright - ament_flake8 - ament_pep257 - - common_interfaces python3-jsonschema python3-mock python3-pep8 python3-pytest python3-pytest-mock python3-yaml - - - ament_python - diff --git a/massrobotics_amr_sender_py/params/sample_config.yaml b/massrobotics_amr_sender/params/sample_config.yaml similarity index 89% rename from massrobotics_amr_sender_py/params/sample_config.yaml rename to massrobotics_amr_sender/params/sample_config.yaml index 83b5b32..966f73a 100644 --- a/massrobotics_amr_sender_py/params/sample_config.yaml +++ b/massrobotics_amr_sender/params/sample_config.yaml @@ -2,16 +2,16 @@ # 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 a 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, +# contains a list of parameters for configuring the ROS1 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 +# field) or complex in case of ROS1 message having data that maps to many AMR report message fields. +# For this reason, some configuration parameters below expect a particular ROS1 message type e.g. +# fields on a ROS1 message of type `sensor_msgs/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 @@ -93,13 +93,13 @@ config: location: valueFrom: rosTopic: /move_base_simple/goal - msgType: geometry_msgs/msg/PoseStamped + msgType: geometry_msgs/PoseStamped # Current velocity of AMR velocity: valueFrom: rosTopic: /good_sensors/vel - msgType: geometry_msgs/msg/TwistStamped + msgType: geometry_msgs/TwistStamped # Percentage of battery remaining # The ``msgField`` indicates a message field where the battery @@ -107,7 +107,7 @@ config: batteryPercentage: valueFrom: rosTopic: /good_sensors/bat - msgType: sensor_msgs/msg/BatteryState + msgType: sensor_msgs/BatteryState msgField: percentage # Estimated remaining runtime in hours @@ -129,7 +129,7 @@ config: errorCodes: valueFrom: rosTopic: /troubleshooting/errorcodes - msgType: std_msgs/msg/String + msgType: std_msgs/String # Target destination(s) of Automated Guided Vehicle (AGV) destinations: diff --git a/massrobotics_amr_sender_py/resource/massrobotics_amr_sender b/massrobotics_amr_sender/resource/massrobotics_amr_sender similarity index 100% rename from massrobotics_amr_sender_py/resource/massrobotics_amr_sender rename to massrobotics_amr_sender/resource/massrobotics_amr_sender diff --git a/massrobotics_amr_sender/sample/README.md b/massrobotics_amr_sender/sample/README.md new file mode 100644 index 0000000..0326962 --- /dev/null +++ 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 remaining topics `/battery`, `/battery_runtime`, `/load_perc_available`, `/mode` and `/troubleshooting/errorcodes` as well as all the transformation described above were generated with a small ROS1 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/massrobotics_amr_sender_py/sample/config.yaml b/massrobotics_amr_sender/sample/config.yaml similarity index 89% rename from massrobotics_amr_sender_py/sample/config.yaml rename to massrobotics_amr_sender/sample/config.yaml index a432f32..abca5e8 100644 --- a/massrobotics_amr_sender_py/sample/config.yaml +++ b/massrobotics_amr_sender/sample/config.yaml @@ -2,16 +2,16 @@ # 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, +# contains a list of parameters for configuring the ROS1 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 +# field) or complex in case of ROS1 message having data that maps to many AMR report message fields. +# For this reason, some configuration parameters below expect a particular ROS1 message type e.g. +# fields on a ROS1 message of type `sensor_msgs/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 @@ -93,13 +93,13 @@ config: location: valueFrom: rosTopic: /location - msgType: geometry_msgs/msg/PoseStamped + msgType: geometry_msgs/PoseStamped # Current velocity of AMR velocity: valueFrom: rosTopic: /velocity - msgType: geometry_msgs/msg/TwistStamped + msgType: geometry_msgs/TwistStamped # Percentage of battery remaining # The ``msgField`` indicates a message field where the battery @@ -107,7 +107,7 @@ config: batteryPercentage: valueFrom: rosTopic: /battery - msgType: sensor_msgs/msg/BatteryState + msgType: sensor_msgs/BatteryState msgField: percentage # Estimated remaining runtime in hours @@ -129,7 +129,7 @@ config: errorCodes: valueFrom: rosTopic: /troubleshooting/errorcodes - msgType: std_msgs/msg/String + msgType: std_msgs/String # Target destination(s) of Automated Guided Vehicle (AGV) destinations: diff --git a/massrobotics_amr_sender/sample/massrobotics_amr_sender_rosbag_launch.launch b/massrobotics_amr_sender/sample/massrobotics_amr_sender_rosbag_launch.launch new file mode 100755 index 0000000..3bb9fb2 --- /dev/null +++ b/massrobotics_amr_sender/sample/massrobotics_amr_sender_rosbag_launch.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/README.md b/massrobotics_amr_sender/sample/rosbag/map/README.md similarity index 100% rename from massrobotics_amr_sender_py/sample/rosbag/map/README.md rename to massrobotics_amr_sender/sample/rosbag/map/README.md diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/map.pgm b/massrobotics_amr_sender/sample/rosbag/map/map.pgm similarity index 100% rename from massrobotics_amr_sender_py/sample/rosbag/map/map.pgm rename to massrobotics_amr_sender/sample/rosbag/map/map.pgm diff --git a/massrobotics_amr_sender_py/sample/rosbag/map/map.yaml b/massrobotics_amr_sender/sample/rosbag/map/map.yaml similarity index 100% rename from massrobotics_amr_sender_py/sample/rosbag/map/map.yaml rename to massrobotics_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 new file mode 100644 index 0000000..d7fbb1f Binary files /dev/null and b/massrobotics_amr_sender/sample/rosbag/rosbag_demo.bag differ diff --git a/massrobotics_amr_sender_py/sample/synthetic/node.py b/massrobotics_amr_sender/sample/synthetic/node.py similarity index 78% rename from massrobotics_amr_sender_py/sample/synthetic/node.py rename to massrobotics_amr_sender/sample/synthetic/node.py index 5503eb6..c0cebc3 100644 --- a/massrobotics_amr_sender_py/sample/synthetic/node.py +++ b/massrobotics_amr_sender/sample/synthetic/node.py @@ -1,4 +1,4 @@ -# Copyright 2021 InOrbit, Inc. +# 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: @@ -27,8 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE. -import rclpy -from rclpy.node import Node +import rospy import time from std_msgs.msg import String from std_msgs.msg import Float32 @@ -40,38 +39,42 @@ from builtin_interfaces.msg import Time -class SampleDataNode(Node): +class SampleDataNode: def __init__(self): - super().__init__("SampleDataNode") + rospy.init_node("SampleDataNode") # publishers for fake data - self.errors_publisher = self.create_publisher( + self.errors_publisher = rospy.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( + self.state_publisher = rospy.Publisher(String, "/mode", 10) + self.battery_publisher = rospy.Publisher(BatteryState, "/battery", 10) + self.battery_runtime_publisher = rospy.Publisher( Float32, "/battery_runtime", 10 ) - self.load_perc_available_publisher = self.create_publisher( + self.load_perc_available_publisher = rospy.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) + rospy.Timer(rospy.Duration(20), self.flip_state_callback) + rospy.Timer(rospy.Duration(1), self.battery_callback) + rospy.Timer(rospy.Duration(7), self.set_error_callback) + rospy.Timer(rospy.Duration(3), self.send_error_callback) + rospy.Timer(rospy.Duration(3), self.battery_runtime_callback) + rospy.Timer(rospy.Duration(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) + self.location_publisher = rospy.Publisher(PoseStamped, "/location", 10) + self.velocity_publisher = rospy.Publisher(TwistStamped, "/velocity", 10) + + def start(self): + rospy.loginfo("Sample node started") + rospy.spin() def set_error_callback(self): self.errors = "error_194,error_1" @@ -125,13 +128,6 @@ def odom_to_location_and_velocity_callback(self, msg): 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() + sample_data_node = SampleDataNode() + sample_data_node.start() diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/massrobotics_amr_node.py b/massrobotics_amr_sender/scripts/massrobotics_amr_sender_node.py old mode 100644 new mode 100755 similarity index 91% rename from massrobotics_amr_sender_py/massrobotics_amr_sender/massrobotics_amr_node.py rename to massrobotics_amr_sender/scripts/massrobotics_amr_sender_node.py index f5cdf3a..ff64c26 --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/massrobotics_amr_node.py +++ b/massrobotics_amr_sender/scripts/massrobotics_amr_sender_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# Copyright 2021 InOrbit, Inc. +# 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: @@ -28,18 +28,14 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - -import rclpy -from . import MassRoboticsAMRInteropNode +from massrobotics_amr_sender import MassRoboticsAMRInteropNode # Interesting example https://github.com/clalancette/mtexec_example def main(args=None): - rclpy.init(args=args) node = MassRoboticsAMRInteropNode() - rclpy.spin(node) - rclpy.shutdown() + node.start() if __name__ == "__main__": diff --git a/massrobotics_amr_sender/setup.py b/massrobotics_amr_sender/setup.py new file mode 100755 index 0000000..6958913 --- /dev/null +++ b/massrobotics_amr_sender/setup.py @@ -0,0 +1,9 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + packages=["massrobotics_amr_sender"], + package_dir={"": "src"}, +) + +setup(**setup_args) diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py b/massrobotics_amr_sender/src/massrobotics_amr_sender/__init__.py old mode 100644 new mode 100755 similarity index 85% rename from massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py rename to massrobotics_amr_sender/src/massrobotics_amr_sender/__init__.py index daf6ac1..9c211ec --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/__init__.py +++ b/massrobotics_amr_sender/src/massrobotics_amr_sender/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2021 InOrbit, Inc. +# 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: @@ -28,9 +28,11 @@ from jsonschema import exceptions as jsonschema_exc -import websockets -import json import asyncio +import json +import rospy +import websockets + from time import sleep from datetime import datetime from datetime import timezone @@ -44,7 +46,6 @@ from sensor_msgs import msg as ros_sensor_msgs from nav_msgs import msg as ros_nav_msgs -from rclpy.node import Node from .config import CFG_PARAMETER_LOCAL from .config import CFG_PARAMETER_ENVVAR from .config import CFG_PARAMETER_ROS_TOPIC @@ -66,7 +67,7 @@ def timestamp_to_isoformat(timestamp): ) -class MassRoboticsAMRInteropNode(Node): +class MassRoboticsAMRInteropNode: """ ROS node implementing WebSocket communication to MassRobotics AMR Receiver. @@ -86,17 +87,17 @@ class MassRoboticsAMRInteropNode(Node): """ - def __init__(self, **kwargs) -> None: - super().__init__(node_name=self.__class__.__name__, **kwargs) - # Get Node logger instance - self.logger = self.get_logger() + def __init__(self) -> None: + rospy.init_node("massrobotics_amr_sender") # Declare Node configuration parameter. Defaults to './config.yaml' if no # ``config_file`` parameter is provided. Provide the parameter when running # the node by using ``--ros-args -p config_file:=/path/to/config.yaml`` - self.declare_parameter("config_file", "params/config.yaml") - config_file_param = self.get_parameter(name="config_file") - config_file_path = config_file_param.get_parameter_value().string_value + if rospy.has_param("~config_file"): + config_file_path = rospy.get_param("~config_file") + rospy.loginfo("Using config from config file: {}".format(config_file_path)) + else: + config_file_path = "src/massrobotics_amr_sender/params/sample_config.yaml" self._config = self._read_config_file(config_file_path=config_file_path) # Websocket connection @@ -117,6 +118,14 @@ def __init__(self, **kwargs) -> None: self.loop.run_in_executor(self._ex, self._status_publisher_thread) self.loop.run_until_complete(self._run()) + """ + Starts the republisher node. + """ + + def start(self): + rospy.loginfo("MASS robotics AMR sender started") + rospy.spin() + async def _run(self): await self._async_connect() await self._async_send_report(self.mass_identity_report) @@ -126,14 +135,14 @@ def _status_publisher_thread(self): # callbacks. However, it's not possible to start it because it # blocks the Node thread and the ROS callbacks are never executed. loop = asyncio.new_event_loop() - self.logger.debug("Starting status publisher thread") + rospy.logdebug("Starting status publisher thread") def send_status(): while True: loop.run_until_complete( self._async_send_report(self.mass_status_report) ) - self.logger.debug(f"Status report sent. Waiting ...") + rospy.logdebug("Status report sent. Waiting ...") sleep(STATUS_REPORT_INTERVAL) loop.create_task(send_status()) @@ -165,9 +174,9 @@ def _process_config(self): self.register_mass_adapter(param_name, topic_name) async def _async_connect(self): - self.logger.debug(f"Connecting to server '{self._uri}'") + rospy.logdebug(f"Connecting to server '{self._uri}'") self._wss_conn = await websockets.connect(self._uri) - self.logger.debug(f"Connected to Mass server '{self._uri}'") + rospy.logdebug(f"Connected to Mass server '{self._uri}'") return self._wss_conn async def _async_send_report(self, mass_object): @@ -182,17 +191,17 @@ async def _async_send_report(self, mass_object): mass_object (:obj:`MassObject`): Identity or Status report """ - self.logger.debug(f"Validating schema MassRobotics object schema") + rospy.logdebug("Validating schema MassRobotics object schema") try: mass_object.validate_schema() except jsonschema_exc.ValidationError as ex: - self.logger.error( + rospy.logerr( f"Invalid schema for '{type(mass_object)}' message. " f"The error reported is: '{ex.message}'. Ignoring message." ) return - self.logger.debug(f"Sending object ({type(mass_object)}): {mass_object.data}") + rospy.logdebug(f"Sending object ({type(mass_object)}): {mass_object.data}") try: await self._wss_conn.ensure_open() except ( @@ -200,7 +209,7 @@ async def _async_send_report(self, mass_object): websockets.exceptions.ConnectionClosed, websockets.exceptions.ConnectionClosedError, ): - self.logger.info(f"Reconnecting to server: {self._uri}") + rospy.loginfo(f"Reconnecting to server: {self._uri}") await self._async_connect() mass_object.update_timestamp() @@ -208,37 +217,38 @@ async def _async_send_report(self, mass_object): try: await self._wss_conn.send(json.dumps(mass_object.data)) except Exception as ex: - self.logger.error(f"Error while sending status report: {ex}") + rospy.logerr(f"Error while sending status report: {ex}") def _read_config_file(self, config_file_path): config_file_path = Path(config_file_path).resolve() if not config_file_path.is_file(): raise ValueError(f"Configuration file '{config_file_path}' doesn't exist!") - self.logger.info(f"Using configuration file '{config_file_path}'") + rospy.loginfo(f"Using configuration file '{config_file_path}'") return MassRoboticsAMRInteropConfig(str(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 + # Return a default 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 + # NOTE(FlorGrosso): returning anything different to what the uuid + # pattern specifies would break the validation. if not msg_frame_id: - return "" + # No logs to avoid spam + return "00000000-0000-0000-0000-000000000000" 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}" - ) + rospy.logwarn(f"Couldn't find mapping for frame '{msg_frame_id}': {msg}") frame_id = "00000000-0000-0000-0000-000000000000" return frame_id def _callback_pose_stamped_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") if msg_field: - self.logger.warning( + rospy.logwarn( f"Parameter {param_name} doesn't support `msgField`. Ignoring." ) @@ -260,24 +270,22 @@ def _callback_pose_stamped_msg(self, param_name, msg_field, data): } def _callback_battery_state_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") try: self.mass_status_report.data[param_name] = getattr(data, msg_field) except AttributeError: - self.logger.error( + rospy.logerr( f"Message field '{msg_field}' on message of " f"type '{type(data)}' doesn't exist" ) def _callback_twist_stamped_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") if msg_field: - self.logger.warning( + rospy.logwarn( f"Parameter {param_name} doesn't support `msgField`. Ignoring." ) - frame_id = self._get_frame_id_from_header(data) - twist = data.twist linear_vel = PyKDL.Vector( @@ -298,29 +306,29 @@ def _callback_twist_stamped_msg(self, param_name, msg_field, data): } def _callback_string_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") if msg_field: - self.logger.warning( + rospy.logwarn( f"Parameter {param_name} doesn't support `msgField`. Ignoring." ) self.mass_status_report.data[param_name] = data.data def _callback_path_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") if msg_field: - self.logger.warning( + rospy.logwarn( f"Parameter {param_name} doesn't support `msgField`. Ignoring." ) - # list of ROS2 Poses translated into Mass predictedLocation + # list of ROS Poses translated into Mass predictedLocation mass_predicted_locations = [] for pose in data.poses: pose_position = pose.pose.position pose_orientation = pose.pose.orientation mass_predicted_locations.append( { - "timestamp": timestamp_to_isoformat(pose.header.stamp.sec), + "timestamp": timestamp_to_isoformat(pose.header.stamp.to_sec()), "x": pose_position.x, "y": pose_position.y, "z": pose_position.z, @@ -335,7 +343,7 @@ def _callback_path_msg(self, param_name, msg_field, data): ) if len(mass_predicted_locations) > 10: - self.logger.warning( + rospy.logwarn( f"Max locations for '{param_name}' are 10 (got " f"{len(mass_predicted_locations)}). Keeping the " "first 10 locations and discarding the rest." @@ -345,9 +353,9 @@ def _callback_path_msg(self, param_name, msg_field, data): self.mass_status_report.data[param_name] = mass_predicted_locations def _callback_error_codes_msg(self, param_name, msg_field, data): - self.logger.debug(f"Processing '{type(data)}' message: {data}") + rospy.logdebug(f"Processing '{type(data)}' message: {data}") if msg_field: - self.logger.warning( + rospy.logwarn( f"Parameter {param_name} doesn't support `msgField`. Ignoring." ) @@ -374,10 +382,10 @@ def register_mass_adapter(self, param_name, topic_name): Returns ------- - boolean: wheter callback registration was successful or not + boolean: whether callback registration was successful or not """ - self.logger.debug(f"Registering callback to topic '{topic_name}'") + rospy.logdebug(f"Registering callback to topic '{topic_name}'") # Topic/message type is expected to contain package name e.g. # ``geometry_msgs/msg/Twist``. @@ -412,13 +420,13 @@ def register_mass_adapter(self, param_name, topic_name): topic_type_t = getattr(msgs_types[topic_type_package], topic_type_name) except (AttributeError, KeyError): # If the message type is not supported do not register any callback - self.logger.error( + rospy.logerr( f"Undefined topic type '{topic_type}' on " f"parameter '{param_name}'. Ignoring..." ) return False - self.logger.debug(f"Binding parameter '{param_name}' with topic '{topic_name}'") + rospy.logdebug(f"Binding parameter '{param_name}' with topic '{topic_name}'") callback = None if param_name == "velocity": @@ -444,17 +452,15 @@ def register_mass_adapter(self, param_name, topic_name): callback = partial(self._callback_string_msg, param_name, msg_field) if not callback: - self.logger.error( + rospy.logerr( f"Callback for parameter '{param_name}' " f"({topic_type}) was not found." ) return False - self.create_subscription( - msg_type=topic_type_t, topic=topic_name, callback=callback, qos_profile=10 - ) + rospy.Subscriber(topic_name, topic_type_t, callback) - self.logger.info( + rospy.loginfo( f"Registered callback for parameter '{param_name}' ({topic_type_name})" ) diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/config.py b/massrobotics_amr_sender/src/massrobotics_amr_sender/config.py old mode 100644 new mode 100755 similarity index 98% rename from massrobotics_amr_sender_py/massrobotics_amr_sender/config.py rename to massrobotics_amr_sender/src/massrobotics_amr_sender/config.py index b2241e4..17439a0 --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/config.py +++ b/massrobotics_amr_sender/src/massrobotics_amr_sender/config.py @@ -1,4 +1,4 @@ -# Copyright 2021 InOrbit, Inc. +# 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: @@ -55,7 +55,7 @@ class MassRoboticsAMRInteropConfig: Parses yaml configuration file and deals with parameters with values that are not local i.e. parameter values that - are obtained from environment variables or ROS2 topics. + are obtained from environment variables or ROS topics. Attributes ---------- diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py b/massrobotics_amr_sender/src/massrobotics_amr_sender/messages/__init__.py similarity index 99% rename from massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py rename to massrobotics_amr_sender/src/massrobotics_amr_sender/messages/__init__.py index 9626904..06f9a6c 100644 --- a/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/__init__.py +++ b/massrobotics_amr_sender/src/massrobotics_amr_sender/messages/__init__.py @@ -1,4 +1,4 @@ -# Copyright 2021 InOrbit, Inc. +# 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: diff --git a/massrobotics_amr_sender_py/massrobotics_amr_sender/messages/schema.json b/massrobotics_amr_sender/src/massrobotics_amr_sender/messages/schema.json similarity index 100% rename from massrobotics_amr_sender_py/massrobotics_amr_sender/messages/schema.json rename to massrobotics_amr_sender/src/massrobotics_amr_sender/messages/schema.json diff --git a/massrobotics_amr_sender_py/.vscode/settings.json b/massrobotics_amr_sender_py/.vscode/settings.json deleted file mode 100644 index 792488d..0000000 --- a/massrobotics_amr_sender_py/.vscode/settings.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "python.pythonPath": "/usr/bin/python3", - "python.testing.pytestArgs": [ - "test" - ], - "python.testing.unittestEnabled": false, - "python.testing.nosetestsEnabled": false, - "python.testing.pytestEnabled": true, - "restructuredtext.confPath": "" -} diff --git a/massrobotics_amr_sender_py/CHANGELOG.rst b/massrobotics_amr_sender_py/CHANGELOG.rst deleted file mode 100644 index 044348d..0000000 --- a/massrobotics_amr_sender_py/CHANGELOG.rst +++ /dev/null @@ -1,28 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package massrobotics_amr_sender -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.0 (2021-06-25) ------------------- -* Adding bits for first release (`#7 `_) - -0.0.2 (2021-06-24) -------------------- -* Changed package name to ``massrobotics_amr_sender`` -* Changed repository folder organization - -0.0.1 (2021-06-23) -------------------- -* Added support for Identity and Status reports -* Added support for several ROS2 messages - - * ``geometry_msgs/TwistStamped`` - * ``sensor_msgs/BatteryState`` - * ``geometry_msgs/PoseStamped`` - * ``nav_msgs/Path`` - * ``std_msgs/String`` - * ``std_msgs/Float32`` - * ``std_msgs/Float64`` - -* Added unit tests for all message callbacks -* Added ``3-Clause BSD License`` diff --git a/massrobotics_amr_sender_py/README.md b/massrobotics_amr_sender_py/README.md deleted file mode 100644 index ce46c48..0000000 --- a/massrobotics_amr_sender_py/README.md +++ /dev/null @@ -1,54 +0,0 @@ -# massrobotics_amr_sender - -Configuration-based ROS2 package for sending MassRobotics [AMR Interop Standard messages](https://github.com/MassRobotics-AMR/AMR_Interop_Standard) to compliant receivers. - -# Package installation - -## From binary packages - -The node is available as a released package and can be added manually to your ROS2 build installation running the following command: - -```bash -$ sudo apt-get install ros-foxy-massrobotics-amr-sender -``` - -Alternatively, you can add the package as a rosdep dependency and then install it running `rosdep update` - -## Building from source - -Make sure `ros2` 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 ROS2 workspace and go into it - if you don't have one already -mkdir -p ~/ros2_ws/src && cd ros2_ws/ -# Clone the repo inside the workspace -git clone https://github.com/inorbit-ai/ros_amr_interop.git ./src -# Install dependencies -rosdep update && rosdep install --ignore-src --from-paths src/ -# Run the build -colcon build --packages-select massrobotics_amr_sender -``` -# Node configuration - -A configuration file must be provided to define how ROS2 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 install/setup.bash -# Launch the node pointing to your configuration file -ros2 launch massrobotics_amr_sender massrobotics_amr_sender.launch.py config_file:=/path/to/config.yaml -``` - - -# Running tests - -On you local workspace: - -```bash -colcon test --packages-select massrobotics_amr_sender -colcon test-result --verbose -``` diff --git a/massrobotics_amr_sender_py/launch/massrobotics_amr_sender.launch.py b/massrobotics_amr_sender_py/launch/massrobotics_amr_sender.launch.py deleted file mode 100644 index ffef722..0000000 --- a/massrobotics_amr_sender_py/launch/massrobotics_amr_sender.launch.py +++ /dev/null @@ -1,59 +0,0 @@ -# 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_ros.actions import Node - -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - my_package_share_dir = get_package_share_directory("massrobotics_amr_sender") - - return LaunchDescription( - [ - DeclareLaunchArgument( - "config_file", - default_value=PathJoinSubstitution( - [my_package_share_dir, "params", "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("config_file")}], - ), - ] - ) diff --git a/massrobotics_amr_sender_py/sample/README.md b/massrobotics_amr_sender_py/sample/README.md deleted file mode 100644 index 6a0cdd8..0000000 --- a/massrobotics_amr_sender_py/sample/README.md +++ /dev/null @@ -1,37 +0,0 @@ -# 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/massrobotics_amr_sender_rosbag_launch.py b/massrobotics_amr_sender_py/sample/massrobotics_amr_sender_rosbag_launch.py deleted file mode 100644 index ccff5b1..0000000 --- a/massrobotics_amr_sender_py/sample/massrobotics_amr_sender_rosbag_launch.py +++ /dev/null @@ -1,70 +0,0 @@ -# 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/metadata.yaml b/massrobotics_amr_sender_py/sample/rosbag/metadata.yaml deleted file mode 100644 index 1336d5b..0000000 --- a/massrobotics_amr_sender_py/sample/rosbag/metadata.yaml +++ /dev/null @@ -1,67 +0,0 @@ -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 deleted file mode 100644 index 5f864a3..0000000 Binary files a/massrobotics_amr_sender_py/sample/rosbag/rosbag_demo.db3 and /dev/null differ diff --git a/massrobotics_amr_sender_py/setup.cfg b/massrobotics_amr_sender_py/setup.cfg deleted file mode 100644 index 805cc3b..0000000 --- a/massrobotics_amr_sender_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script-dir=$base/lib/massrobotics_amr_sender -[install] -install-scripts=$base/lib/massrobotics_amr_sender diff --git a/massrobotics_amr_sender_py/setup.py b/massrobotics_amr_sender_py/setup.py deleted file mode 100644 index df7f8a5..0000000 --- a/massrobotics_amr_sender_py/setup.py +++ /dev/null @@ -1,42 +0,0 @@ -from setuptools import setup, find_packages -import xml.etree.ElementTree as ET -import os -from glob import glob - -# Read version from ``package.xml`` file -package_xml = ET.parse("package.xml").getroot() - -package_name = "massrobotics_amr_sender" - -share_dir = os.path.join("share", package_name) - -setup( - name=package_name, - packages=find_packages(), - package_data={"": ["schema.json"]}, - include_package_data=True, - version=package_xml.find("version").text, - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - (share_dir, ["package.xml"]), - # Include launch files - (share_dir, glob("launch/*.launch.py")), - # Sample config files - ( - os.path.join(share_dir, "params"), - [os.path.join("params", "sample_config.yaml")], - ), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="InOrbit", - maintainer_email="support@inorbit.ai", - description="ROS2 node implementing a MassRobotics AMR Interoperability Sender", - license="3-Clause BSD License", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "massrobotics_amr_node = massrobotics_amr_sender.massrobotics_amr_node:main" - ], - }, -) diff --git a/massrobotics_amr_sender_py/test/README.md b/massrobotics_amr_sender_py/test/README.md deleted file mode 100644 index a22069a..0000000 --- a/massrobotics_amr_sender_py/test/README.md +++ /dev/null @@ -1,272 +0,0 @@ -# Test samples - -Misc commands for various tests - -## Publishing ROS2 messages manually - -### Path - -```bash -ros2 topic pub --once /we_b_robots/destinations nav_msgs/msg/Path \ -'{ - "header": { - "frame_id": "floor1" - }, - "poses": [ - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 2.0, - "y": 0.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 2.0, - "y": 1.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 3.0, - "y": 1.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 2.0, - "y": 4.0, - "z": 0.0 - }, - "orientation": { - "x": 1.0, - "y": 1.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 2.0, - "y": 6.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 2.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 6.2, - "y": 2.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 2.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 11.0, - "y": 0.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 4.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 9.0, - "y": 5.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 1.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 6.0, - "y": 3.1, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 2.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 1.0, - "y": 1.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 1.8, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 5.3, - "y": 3.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 1.8, - "w": 1 - } - } - } - ] -}' -``` - -### TwistStamped - -```bash -ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "twist": { - "linear": { - "x": 1, - "y": 2, - "z": 3 - }, - "angular": { - "x": 1, - "y": 1, - "z": 1 - } - } -}' -``` - -### BatteryState - -```bash -ros2 topic pub --once /good_sensors/bat sensor_msgs/msg/BatteryState ' -{ - percentage: 91.3 -}' -``` - -### PoseStamped - -```bash -ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 2, - "y": 0, - "z": 0 - }, - "orientation": { - "x": 0, - "y": 0, - "z": 1.8, - "w": 1 - } - } -}' -``` diff --git a/massrobotics_amr_sender_py/test/scripts/demo_emulation.sh b/massrobotics_amr_sender_py/test/scripts/demo_emulation.sh deleted file mode 100755 index 5fc65de..0000000 --- a/massrobotics_amr_sender_py/test/scripts/demo_emulation.sh +++ /dev/null @@ -1,255 +0,0 @@ -#!/bin/bash - -publish_initial_pose() { - ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 10, - "y": 10, - "z": 0 - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0, - "w": 1 - } - } -}' - -} -publish_path() { - ros2 topic pub --once /magic_nav/path nav_msgs/msg/Path \ -'{ - "header": { - "frame_id": "floor1" - }, - "poses": [ - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 10, - "y": 10, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 19.0, - "y": 11.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 20.0, - "y": 15.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "w": 1 - } - } - }, - { - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 30.0, - "y": 15.0, - "z": 0.0 - }, - "orientation": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "w": 1 - } - } - }, - - ] -}' -} - -publish_pose_1() { - ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 19.0, - "y": 11.0, - "z": 0 - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0, - "w": 1 - } - } -}' - -ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "twist": { - "linear": { - "x": 1, - "y": 2, - "z": 3 - }, - "angular": { - "x": 1, - "y": 1, - "z": 1 - } - } -}' - -} - -publish_pose_2() { - ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 20.0, - "y": 15.0, - "z": 0 - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0, - "w": 1 - } - } -}' - -ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "twist": { - "linear": { - "x": 5, - "y": 2, - "z": 3 - }, - "angular": { - "x": 3, - "y": 3, - "z": 3 - } - } -}' - -} - -publish_pose_3() { - ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "pose": { - "position": { - "x": 30.0, - "y": 15.0, - "z": 0 - }, - "orientation": { - "x": 0, - "y": 0, - "z": 0, - "w": 1 - } - } -}' - -ros2 topic pub --once /good_sensors/vel geometry_msgs/msg/TwistStamped ' -{ - "header": { - "frame_id": "floor1" - }, - "twist": { - "linear": { - "x": 1, - "y": 0, - "z": 0 - }, - "angular": { - "x": 0, - "y": 1, - "z": 1 - } - } -}' - -} - -clear_path() { - ros2 topic pub --once /magic_nav/path nav_msgs/msg/Path -} - -clear_path -sleep 5 - -for i in {1..3} -do - publish_initial_pose - publish_path - sleep 3 - publish_pose_1 - sleep 3 - publish_pose_2 - sleep 3 - publish_pose_3 - sleep 3 - clear_path - sleep 5 -done diff --git a/massrobotics_amr_sender_py/test/test_copyright.py b/massrobotics_amr_sender_py/test/test_copyright.py deleted file mode 100644 index f46f861..0000000 --- a/massrobotics_amr_sender_py/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/massrobotics_amr_sender_py/test/test_flake8.py b/massrobotics_amr_sender_py/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/massrobotics_amr_sender_py/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/massrobotics_amr_sender_py/test/test_mass_interop.py b/massrobotics_amr_sender_py/test/test_mass_interop.py deleted file mode 100644 index b2d4030..0000000 --- a/massrobotics_amr_sender_py/test/test_mass_interop.py +++ /dev/null @@ -1,489 +0,0 @@ -# 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 pytest -import websockets -import asyncio -import rclpy -from rclpy import Parameter -from pathlib import Path -from unittest.mock import AsyncMock -from massrobotics_amr_sender import MassRoboticsAMRInteropNode - -from std_msgs import msg as ros_std_msgs -from geometry_msgs import msg as ros_geometry_msgs -from sensor_msgs import msg as ros_sensor_msgs -from nav_msgs import msg as ros_nav_msgs -from builtin_interfaces import msg as ros_builtin_msgs - -cwd = Path(__file__).resolve().parent -config_file_test = Path(cwd).parent / "params" / "sample_config.yaml" - -FAKE_ROBOT_ID = "d6f7c89c-6b11-45b4-b763-86cec88cc2eb" - -# Mass Identity Report built after parsing -# the ``sample_config.yaml`` file -MASS_IDENTITY_REPORT = { - "uuid": FAKE_ROBOT_ID, - "manufacturerName": "Spoonlift", - "robotModel": "spoony1.0", - "robotSerialNumber": "2172837", - "baseRobotEnvelope": {"x": 2, "y": 1, "z": 3}, - "maxSpeed": 2.5, - "maxRunTime": 8, - "emergencyContactInformation": "555-5555", - "chargerType": "24V plus", - "supportVendorName": "We-B-Robots", - "supportVendorContactInformation": "support@we-b-robots.com", - "productDocumentation": "https://spoon.lift/support/docs/spoony1.0", - "thumbnailImage": "https://spoon.lift/media/spoony1.0.png", - "cargoType": "Anything solid or liquid", - "cargoMaxVolume": {"x": 2, "y": 2, "z": 1}, - "cargoMaxWeight": "4000", -} - - -@pytest.fixture(autouse=True) -def mock_ws_conn(mocker): - # mock websockets connect method - websockets_mock = AsyncMock() - # websockets.connect returns an instance of WebsocketClientProtocol - # that is also mocked. - websocket_client_protocol = AsyncMock() - websocket_client_protocol.ensure_open = AsyncMock() - websocket_client_protocol.send = AsyncMock() - - # return mocked WebsocketClientProtocol - websockets_mock.return_value = websocket_client_protocol - - mocker.patch("websockets.connect", side_effect=websockets_mock) - - # On init, the Node creates a task on a separate thread for publishing - # Mass status reports on a fixed time interval. - # To avoid blocking (i.e. a method using an infinite loop), patch - # the method so it does nothing. FIXME: this can be improved. - def _fake_status_publisher_thread(): - pass - - mocker.patch( - "massrobotics_amr_sender.MassRoboticsAMRInteropNode._status_publisher_thread", - side_effect=_fake_status_publisher_thread, - ) - - -@pytest.fixture(autouse=True) -def mock_robot_id(monkeypatch): - # Environment variable used on config file - monkeypatch.setenv("MY_UUID", FAKE_ROBOT_ID) - - -@pytest.fixture -def event_loop(): - # Fixture for running the async method for sending the Mass object - loop = asyncio.new_event_loop() - yield loop - loop.close() - - -def test_mass_config_load_fails_on_missing_config_file(monkeypatch): - monkeypatch.delenv("MY_UUID") - rclpy.init() - with pytest.raises(ValueError): - MassRoboticsAMRInteropNode() - rclpy.shutdown() - - -def test_massrobotics_amr_node_init(): - rclpy.init() - node = MassRoboticsAMRInteropNode( - parameter_overrides=[Parameter("config_file", value=str(config_file_test))] - ) - rclpy.spin_once(node, timeout_sec=0.1) - rclpy.shutdown() - - mass_identity_report = node.mass_identity_report.data - - # check Node parses configuration file properly - # and populates Mass Identity report object - for prop, value in MASS_IDENTITY_REPORT.items(): - assert mass_identity_report[prop] == value - - # assert connect method has been called once - assert websockets.connect.call_count == 1 - # assert mocked status published thread has been called once - assert node._status_publisher_thread.call_count == 1 - # Mass identity report is sent once on Node init - assert node._wss_conn.send.call_count == 1 - - -# List of parameters for publishers that are used to -# invoke callbacks registered on Node init -# Parameters are -# - msg_type: callback message type -# - topic: topic where the message will be published -# - msg: the message that will be published on the topic -# - property: Mass Status report property that will be updated -# - value: the value that should be written on the Mass Status report -STATUS_REPORT_TESTS = [ - { - "msg_type": ros_std_msgs.String, - "topic": "/we_b_robots/mode", - "msg": ros_std_msgs.String(data="charging"), - "property": "operationalState", - "value": "charging", - }, - { - "msg_type": ros_geometry_msgs.PoseStamped, - "topic": "/move_base_simple/goal", - "msg": ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header(frame_id="floor1"), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=42.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion(x=-1.0, y=9.0, z=-3.0, w=0.1), - ), - ), - "property": "location", - "value": { - "x": 42, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "096522ad-61fa-4796-9b31-e35b0f8d0b26", - }, - }, - { - "msg_type": ros_geometry_msgs.TwistStamped, - "topic": "/good_sensors/vel", - "msg": ros_geometry_msgs.TwistStamped( - header=ros_std_msgs.Header(frame_id="floor2"), - twist=ros_geometry_msgs.Twist( - linear=ros_geometry_msgs.Vector3(x=1.0, y=0.0, z=0.0), - angular=ros_geometry_msgs.Vector3(x=0.2, y=0.1, z=0.0), - ), - ), - "property": "velocity", - "value": { - "linear": 1, - "angular": { - "w": 0.9937606691655042, - "x": 0.09970865087213879, - "y": 0.04972948160146044, - "z": -0.0049895912294619805, - }, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - }, - { - "msg_type": ros_sensor_msgs.BatteryState, - "topic": "/good_sensors/bat", - "msg": ros_sensor_msgs.BatteryState(percentage=12.34), - "property": "batteryPercentage", - "value": pytest.approx(12.34), - }, - { - "msg_type": ros_std_msgs.Float32, - "topic": "/good_sensors/bat_remaining", - "msg": ros_std_msgs.Float32(data=123456.789), - "property": "remainingRunTime", - "value": pytest.approx(123456.789), - }, - { - "msg_type": ros_std_msgs.Float32, - "topic": "/good_sensors/load", - "msg": ros_std_msgs.Float32(data=49.99), - "property": "loadPercentageStillAvailable", - "value": pytest.approx(49.99), - }, - { - "msg_type": ros_nav_msgs.Path, - "topic": "/we_b_robots/destinations", - "msg": ros_nav_msgs.Path( - header=ros_std_msgs.Header(frame_id="floor2"), - poses=[ - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624401648) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=42.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.1 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624402598) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=4.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=1.0, z=-3.0, w=0.1 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624403168) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=12.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.4 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor1", stamp=ros_builtin_msgs.Time(sec=1624404998) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=0.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.1 - ), - ), - ), - ], - ), - "property": "destinations", - "value": [ - { - "timestamp": "2021-06-22T22:40:48+00:00", - "x": 42, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T22:56:38+00:00", - "x": 4, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 1.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T23:06:08+00:00", - "x": 12, - "y": 4, - "z": 2, - "angle": {"w": 0.4, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T23:36:38+00:00", - "x": 0, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "096522ad-61fa-4796-9b31-e35b0f8d0b26", - }, - ], - }, - { - "msg_type": ros_nav_msgs.Path, - "topic": "/magic_nav/path", - "msg": ros_nav_msgs.Path( - header=ros_std_msgs.Header(frame_id="floor2"), - poses=[ - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624401648) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=42.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.1 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624402598) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=4.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=1.0, z=-3.0, w=0.1 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor2", stamp=ros_builtin_msgs.Time(sec=1624403168) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=12.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.4 - ), - ), - ), - ros_geometry_msgs.PoseStamped( - header=ros_std_msgs.Header( - frame_id="floor1", stamp=ros_builtin_msgs.Time(sec=1624404998) - ), - pose=ros_geometry_msgs.Pose( - position=ros_geometry_msgs.Point(x=0.0, y=4.0, z=2.0), - orientation=ros_geometry_msgs.Quaternion( - x=-1.0, y=9.0, z=-3.0, w=0.1 - ), - ), - ), - ], - ), - "property": "path", - "value": [ - { - "timestamp": "2021-06-22T22:40:48+00:00", - "x": 42, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T22:56:38+00:00", - "x": 4, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 1.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T23:06:08+00:00", - "x": 12, - "y": 4, - "z": 2, - "angle": {"w": 0.4, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "6ec7a6d0-21a9-4f04-b680-e7c640a0687e", - }, - { - "timestamp": "2021-06-22T23:36:38+00:00", - "x": 0, - "y": 4, - "z": 2, - "angle": {"w": 0.1, "x": -1.0, "y": 9.0, "z": -3.0}, - "planarDatum": "096522ad-61fa-4796-9b31-e35b0f8d0b26", - }, - ], - }, - { - "msg_type": ros_std_msgs.String, - "topic": "/troubleshooting/errorcodes", - "msg": ros_std_msgs.String(data="error1,error2,error3"), - "property": "errorCodes", - "value": ["error1", "error2", "error3"], - }, - { - "msg_type": ros_std_msgs.String, - "topic": "/troubleshooting/errorcodes", - "msg": ros_std_msgs.String(data="error1"), - "property": "errorCodes", - "value": ["error1"], - }, - { - "msg_type": ros_std_msgs.String, - "topic": "/troubleshooting/errorcodes", - "msg": ros_std_msgs.String(), - "property": "errorCodes", - "value": [], - }, -] - - -def test_massrobotics_amr_node_status_report_callbacks(event_loop): - rclpy.init() - # create the node we want to test - node = MassRoboticsAMRInteropNode( - parameter_overrides=[Parameter("config_file", value=str(config_file_test))] - ) - # also create an additional node to publish messages - helper_node = rclpy.create_node("test_helper_node") - - for test_data in STATUS_REPORT_TESTS: - - publisher = helper_node.create_publisher( - msg_type=test_data["msg_type"], topic=test_data["topic"], qos_profile=10 - ) - publisher.publish(test_data["msg"]) - - rclpy.spin_once(helper_node, timeout_sec=0.1) - rclpy.spin_once(node, timeout_sec=0.1) - - publisher.destroy() - - result = node.mass_status_report.data[test_data["property"]] - expected = test_data["value"] - - if result != expected: - pytest.fail( - f"The obtained result '{result}' doesn't match with the " - f"expected output '{expected}'. Test data: {test_data}" - ) - - event_loop.run_until_complete(node._async_send_report(node.mass_status_report)) - rclpy.shutdown() - - # assert connect method has been called once - assert websockets.connect.call_count == 1 - # assert mocked status published thread has been called once - assert node._status_publisher_thread.call_count == 1 - # Mass identity report is sent once on Node init - # and after processing all messages on ``STATUS_REPORT_TESTS`` - assert node._wss_conn.send.call_count == 2 - - -def test_massrobotics_amr_node_status_report_not_sent_on_invalid_schema(event_loop): - rclpy.init() - # create the node we want to test - node = MassRoboticsAMRInteropNode( - parameter_overrides=[Parameter("config_file", value=str(config_file_test))] - ) - - node.mass_status_report.data["operationalState"] = "foobar" - - rclpy.spin_once(node, timeout_sec=0.1) - - # Try to send a status report with an invalid schema i.e. ``foobar`` operational - # state is not an allowed value. - event_loop.run_until_complete(node._async_send_report(node.mass_status_report)) - - rclpy.shutdown() - - # assert connect method has been called once - assert websockets.connect.call_count == 1 - # assert mocked status published thread has been called once - assert node._status_publisher_thread.call_count == 1 - # Mass identity report is sent once on Node init - # That should be the only successful call given that the status - # report sent above is invalid and the node should not send it. - assert node._wss_conn.send.call_count == 1 diff --git a/massrobotics_amr_sender_py/test/test_mass_interop_config.py b/massrobotics_amr_sender_py/test/test_mass_interop_config.py deleted file mode 100644 index 145a821..0000000 --- a/massrobotics_amr_sender_py/test/test_mass_interop_config.py +++ /dev/null @@ -1,91 +0,0 @@ -# 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 pytest -from pathlib import Path -from massrobotics_amr_sender.config import MassRoboticsAMRInteropConfig -from massrobotics_amr_sender.config import CFG_PARAMETER_LOCAL -from massrobotics_amr_sender.config import CFG_PARAMETER_ROS_TOPIC -from massrobotics_amr_sender.config import CFG_PARAMETER_ENVVAR - -cwd = Path(__file__).resolve().parent - - -def test_mass_config_load(): - cfg_file_path = Path(cwd).parent / "params" / "sample_config.yaml" - assert MassRoboticsAMRInteropConfig(str(cfg_file_path)).mappings != {} - - -@pytest.mark.parametrize( - "param_name, param_type", - [ - ("uuid", CFG_PARAMETER_ENVVAR), - ("robotModel", CFG_PARAMETER_LOCAL), - ("operationalState", CFG_PARAMETER_ROS_TOPIC), - ("baseRobotEnvelope", CFG_PARAMETER_LOCAL), - ("maxSpeed", CFG_PARAMETER_LOCAL), - ], -) -def test_mass_config_get_parameter_type(param_name, param_type): - cfg_file_path = Path(cwd).parent / "params" / "sample_config.yaml" - mass_config = MassRoboticsAMRInteropConfig(str(cfg_file_path)) - assert mass_config.get_parameter_source(param_name) == param_type - - -@pytest.mark.parametrize( - "param_name, value", - [ - ("uuid", "foo"), - ("robotModel", "spoony1.0"), - ("operationalState", "/we_b_robots/mode"), - ("baseRobotEnvelope", {"x": 2, "y": 1, "z": 3}), - ], -) -def test_mass_config_get_parameter_value(monkeypatch, param_name, value): - monkeypatch.setenv("MY_UUID", "foo") # Environment variable used on config file - cfg_file_path = Path(cwd).parent / "params" / "sample_config.yaml" - mass_config = MassRoboticsAMRInteropConfig(str(cfg_file_path)) - assert mass_config.get_parameter_value(param_name) == value - - -@pytest.mark.parametrize( - "param_name, source", - [ - ("uuid", CFG_PARAMETER_ENVVAR), - ("robotModel", CFG_PARAMETER_LOCAL), - ("operationalState", CFG_PARAMETER_ROS_TOPIC), - ("baseRobotEnvelope", CFG_PARAMETER_LOCAL), - ("maxSpeed", CFG_PARAMETER_LOCAL), - ], -) -def test_mass_config_get_parameters_by_source(monkeypatch, param_name, source): - monkeypatch.setenv("MY_UUID", "foo") # Environment variable used on config file - cfg_file_path = Path(cwd).parent / "params" / "sample_config.yaml" - mass_config = MassRoboticsAMRInteropConfig(str(cfg_file_path)) - assert param_name in mass_config.parameters_by_source[source] diff --git a/massrobotics_amr_sender_py/test/test_pep257.py b/massrobotics_amr_sender_py/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/massrobotics_amr_sender_py/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings"