Add monr relay module

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "atos/iso22133"]
 	path = atos/iso22133
-	url = https://github.com/RI-SE/iso22133.git
+	url = git@github.com:RI-SE/iso22133.git
 [submodule "atos_interfaces"]
 	path = atos_interfaces
-	url = https://github.com/RI-SE/atos_interfaces.git
+	url = git@github.com:RI-SE/atos_interfaces.git

atos/CMakeLists.txt

@@ -65,6 +65,7 @@ set(WITH_INTEGRATION_TESTING ON CACHE BOOL "Enable IntegrationTesting module")
 set(WITH_BACK_TO_START ON CACHE BOOL "Enable BackToStart module")
 set(WITH_OPEN_SCENARIO_GATEWAY ON CACHE BOOL "Enable OpenScenario Gateway module")
 set(WITH_REST_BRIDGE ON CACHE BOOL "Enable RESTBridge module")
+set(WITH_MONR_RELAY ON CACHE BOOL "Enable MonrRelay module")
 
 set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build")
 
@@ -102,6 +103,9 @@ endif()
 if(WITH_REST_BRIDGE)
         list(APPEND ENABLED_MODULES RESTBridge)
 endif()
+if(WITH_MONR_RELAY)
+        list(APPEND ENABLED_MODULES MonrRelay)
+endif()
 
 
 # Add corresponding subprojects

atos/common/roschannels/monitorchannel.hpp

@@ -5,124 +5,148 @@
  */
 #pragma once
 
-#include "roschannel.hpp"
 #include "atos_interfaces/msg/monitor.hpp"
 #include "positioning.h"
+#include "roschannel.hpp"
 #include <tf2/LinearMath/Quaternion.h>
 #if ROS_FOXY
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #elif ROS_HUMBLE
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #endif
 
-
 namespace ROSChannels {
-    namespace Monitor {
-        const std::string topicName = "object_monitor";
-        using message_type = atos_interfaces::msg::Monitor;
-        const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1));
+namespace Monitor {
+const std::string topicName	 = "object_monitor";
+using message_type			 = atos_interfaces::msg::Monitor;
+const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1));
+
+class Pub : public BasePub<message_type> {
+public:
+	const uint32_t objectId;
+	Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) :
+	  BasePub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, qos),
+	  objectId(id) {}
+};
+
+class PubAll : public BasePub<message_type> {
+public:
+	PubAll(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) :
+	  BasePub<message_type>(node, topicName, qos) {}
+};
+
+class AnchorPub : public BasePub<message_type> {
+public:
+	AnchorPub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) :
+	  BasePub<message_type>(node, "object_anchor/" + topicName, qos) {}
+};
+
+class Sub : public BaseSub<message_type> {
+public:
+	const uint32_t objectId;
+	Sub(rclcpp::Node& node,
+		const uint32_t id,
+		std::function<void(const message_type::SharedPtr)> callback,
+		const rclcpp::QoS& qos = defaultQoS) :
+	  BaseSub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos),
+	  objectId(id) {}
+};
+
+class SubAll : public BaseSub<message_type> {
+public:
+	SubAll(rclcpp::Node& node,
+		   std::function<void(const message_type::SharedPtr)> callback,
+		   const rclcpp::QoS& qos = defaultQoS) :
+	  BaseSub<message_type>(node, topicName, callback, qos) {}
+};
 
-        class Pub : public BasePub<message_type> {
-        public:
-            const uint32_t objectId;
-            Pub(rclcpp::Node& node, const uint32_t id, const rclcpp::QoS& qos = defaultQoS) : 
-                BasePub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, qos),
-                objectId(id) {}
-        };
+class AnchorSub : public BaseSub<message_type> {
+public:
+	AnchorSub(rclcpp::Node& node,
+			  std::function<void(const message_type::SharedPtr)> callback,
+			  const rclcpp::QoS& qos = defaultQoS) :
+	  BaseSub<message_type>(node, "object_anchor/" + topicName, callback, qos) {}
+};
 
-        class AnchorPub : public BasePub<message_type> {
-        public:
-            AnchorPub(rclcpp::Node& node, const rclcpp::QoS& qos = defaultQoS) :
-                BasePub<message_type>(node, "object_anchor/" + topicName, qos) {}
-        };
+inline message_type fromISOMonr(const uint32_t id,
+								const ObjectMonitorType& indata,
+								const std::vector<unsigned char>& raw_data) {
+	atos_interfaces::msg::Monitor outdata;
+	auto txid  = id;
+	auto stamp = rclcpp::Time(indata.timestamp.tv_sec, indata.timestamp.tv_usec * 1000);
 
-        class Sub : public BaseSub<message_type> {
-        public:
-            const uint32_t objectId;
-            Sub(rclcpp::Node& node, const uint32_t id, std::function<void(const message_type::SharedPtr)> callback, const rclcpp::QoS& qos = defaultQoS) : 
-                BaseSub<message_type>(node, "object_" + std::to_string(id) + "/" + topicName, callback, qos),
-                objectId(id) {}
-        };
+	// Set stamp for all subtypes
+	outdata.atos_header.header.stamp  = stamp;
+	outdata.pose.header.stamp		  = stamp;
+	outdata.velocity.header.stamp	  = stamp;
+	outdata.acceleration.header.stamp = stamp;
 
-        class AnchorSub : public BaseSub<message_type> {
-        public:
-            AnchorSub(rclcpp::Node& node, std::function<void(const message_type::SharedPtr)> callback, const rclcpp::QoS& qos = defaultQoS) : 
-                BaseSub<message_type>(node, "object_anchor/" + topicName, callback, qos) {}
-        };
+	// Set frame ids
+	outdata.atos_header.header.frame_id	 = "map"; // TODO
+	outdata.pose.header.frame_id		 = "map"; // TODO
+	outdata.velocity.header.frame_id	 = "map"; // TODO vehicle local
+	outdata.acceleration.header.frame_id = "map"; // TODO vehicle local
 
-        inline message_type fromISOMonr(const uint32_t id, const ObjectMonitorType& indata) {
-            atos_interfaces::msg::Monitor outdata;
-            auto txid = id;
-            auto stamp = rclcpp::Time(indata.timestamp.tv_sec, indata.timestamp.tv_usec*1000);
-
-            // Set stamp for all subtypes
-            outdata.atos_header.header.stamp = stamp;
-            outdata.pose.header.stamp = stamp;
-            outdata.velocity.header.stamp = stamp;
-            outdata.acceleration.header.stamp = stamp;
+	outdata.atos_header.object_id = txid;
+	outdata.object_state.state	  = indata.state;
+	if (indata.position.isPositionValid) {
+		outdata.pose.pose.position.x = indata.position.xCoord_m;
+		outdata.pose.pose.position.y = indata.position.yCoord_m;
+		outdata.pose.pose.position.z = indata.position.isZcoordValid ? indata.position.zCoord_m : 0.0;
+	}
+	if (indata.position.isHeadingValid) {
+		tf2::Quaternion orientation;
+		orientation.setRPY(0, 0, indata.position.heading_rad);
+		outdata.pose.pose.orientation = tf2::toMsg(orientation);
+	}
+	outdata.velocity.twist.linear.x	 = indata.speed.isLongitudinalValid ? indata.speed.longitudinal_m_s : 0;
+	outdata.velocity.twist.linear.y	 = indata.speed.isLateralValid ? indata.speed.lateral_m_s : 0;
+	outdata.velocity.twist.linear.z	 = 0;
+	outdata.velocity.twist.angular.x = 0;
+	outdata.velocity.twist.angular.y = 0;
+	outdata.velocity.twist.angular.z = 0;
+	outdata.acceleration.accel.linear.x =
+	  indata.acceleration.isLongitudinalValid ? indata.acceleration.longitudinal_m_s2 : 0;
+	outdata.acceleration.accel.linear.y	 = indata.acceleration.isLateralValid ? indata.acceleration.lateral_m_s2 : 0;
+	outdata.acceleration.accel.linear.z	 = 0;
+	outdata.acceleration.accel.angular.x = 0;
+	outdata.acceleration.accel.angular.y = 0;
+	outdata.acceleration.accel.angular.z = 0;
 
-            // Set frame ids
-            outdata.atos_header.header.frame_id = "map"; // TODO
-            outdata.pose.header.frame_id = "map"; // TODO
-            outdata.velocity.header.frame_id = "map"; // TODO vehicle local
-            outdata.acceleration.header.frame_id = "map"; // TODO vehicle local
+	outdata.raw_data = raw_data;
 
-            outdata.atos_header.object_id = txid;
-            outdata.object_state.state = indata.state;
-            if (indata.position.isPositionValid) {
-                outdata.pose.pose.position.x = indata.position.xCoord_m;
-                outdata.pose.pose.position.y = indata.position.yCoord_m;
-                outdata.pose.pose.position.z = indata.position.isZcoordValid ? indata.position.zCoord_m : 0.0;
-            }
-            if (indata.position.isHeadingValid) {
-                tf2::Quaternion orientation;
-                orientation.setRPY(0, 0, indata.position.heading_rad);
-                outdata.pose.pose.orientation = tf2::toMsg(orientation);
-            }
-            outdata.velocity.twist.linear.x = indata.speed.isLongitudinalValid ? indata.speed.longitudinal_m_s : 0;
-            outdata.velocity.twist.linear.y = indata.speed.isLateralValid ? indata.speed.lateral_m_s : 0;
-            outdata.velocity.twist.linear.z = 0;
-            outdata.velocity.twist.angular.x = 0;
-            outdata.velocity.twist.angular.y = 0;
-            outdata.velocity.twist.angular.z = 0;
-            outdata.acceleration.accel.linear.x = indata.acceleration.isLongitudinalValid ? indata.acceleration.longitudinal_m_s2 : 0;
-            outdata.acceleration.accel.linear.y = indata.acceleration.isLateralValid ? indata.acceleration.lateral_m_s2 : 0;
-            outdata.acceleration.accel.linear.z = 0;
-            outdata.acceleration.accel.angular.x = 0;
-            outdata.acceleration.accel.angular.y = 0;
-            outdata.acceleration.accel.angular.z = 0;
-            return outdata;
-        }
+	return outdata;
+}
 
-        inline ObjectMonitorType toISOMonr(message_type& indata) {
-            ObjectMonitorType outdata;
-            outdata.timestamp.tv_sec = indata.atos_header.header.stamp.sec;
-            outdata.timestamp.tv_usec = indata.atos_header.header.stamp.nanosec / 1000;
-            outdata.state = static_cast<ObjectStateType>(indata.object_state.state);
-            outdata.position.isPositionValid = true;
-            outdata.position.isXcoordValid = true;
-            outdata.position.isYcoordValid = true;
-            outdata.position.isZcoordValid = true;
-            outdata.position.xCoord_m = indata.pose.pose.position.x;
-            outdata.position.yCoord_m = indata.pose.pose.position.y;
-            outdata.position.zCoord_m = indata.pose.pose.position.z;
-            outdata.position.isHeadingValid = true;
-            tf2::Quaternion quat_tf;
-            geometry_msgs::msg::Quaternion quat_msg = indata.pose.pose.orientation;
-            tf2::fromMsg(quat_msg, quat_tf);
-            double r{}, p{}, y{};
-            tf2::Matrix3x3 m(quat_tf);
-            m.getRPY(r, p, y);
-            outdata.position.heading_rad = y;
-            outdata.speed.isLongitudinalValid = true;
-            outdata.speed.longitudinal_m_s = indata.velocity.twist.linear.x;
-            outdata.speed.isLateralValid = true;
-            outdata.speed.lateral_m_s = indata.velocity.twist.linear.y;
-            outdata.acceleration.isLongitudinalValid = true;
-            outdata.acceleration.longitudinal_m_s2 = indata.acceleration.accel.linear.x;
-            outdata.acceleration.isLateralValid = true;
-            outdata.acceleration.lateral_m_s2 = indata.acceleration.accel.linear.y;
-            return outdata;
-        }
-    }
-}
+inline ObjectMonitorType toISOMonr(message_type& indata) {
+	ObjectMonitorType outdata;
+	outdata.timestamp.tv_sec		 = indata.atos_header.header.stamp.sec;
+	outdata.timestamp.tv_usec		 = indata.atos_header.header.stamp.nanosec / 1000;
+	outdata.state					 = static_cast<ObjectStateType>(indata.object_state.state);
+	outdata.position.isPositionValid = true;
+	outdata.position.isXcoordValid	 = true;
+	outdata.position.isYcoordValid	 = true;
+	outdata.position.isZcoordValid	 = true;
+	outdata.position.xCoord_m		 = indata.pose.pose.position.x;
+	outdata.position.yCoord_m		 = indata.pose.pose.position.y;
+	outdata.position.zCoord_m		 = indata.pose.pose.position.z;
+	outdata.position.isHeadingValid	 = true;
+	tf2::Quaternion quat_tf;
+	geometry_msgs::msg::Quaternion quat_msg = indata.pose.pose.orientation;
+	tf2::fromMsg(quat_msg, quat_tf);
+	double r{}, p{}, y{};
+	tf2::Matrix3x3 m(quat_tf);
+	m.getRPY(r, p, y);
+	outdata.position.heading_rad			 = y;
+	outdata.speed.isLongitudinalValid		 = true;
+	outdata.speed.longitudinal_m_s			 = indata.velocity.twist.linear.x;
+	outdata.speed.isLateralValid			 = true;
+	outdata.speed.lateral_m_s				 = indata.velocity.twist.linear.y;
+	outdata.acceleration.isLongitudinalValid = true;
+	outdata.acceleration.longitudinal_m_s2	 = indata.acceleration.accel.linear.x;
+	outdata.acceleration.isLateralValid		 = true;
+	outdata.acceleration.lateral_m_s2		 = indata.acceleration.accel.linear.y;
+	return outdata;
+}
+} // namespace Monitor
+} // namespace ROSChannels

atos/launch/launch_experimental.py

@@ -1,5 +1,6 @@
-import sys
 import os
+import sys
+
 from ament_index_python.packages import get_package_prefix
 
 sys.path.insert(
@@ -8,9 +9,10 @@
         get_package_prefix("atos"), "share", "atos", "launch"
     ),
 )
-from launch_ros.actions import Node
-from launch import LaunchDescription
+
 import launch_utils.launch_base as launch_base
+from launch import LaunchDescription
+from launch_ros.actions import Node
 
 
 def get_experimental_nodes():
@@ -53,6 +55,13 @@ def get_experimental_nodes():
             name="rest_bridge",
             parameters=[files["params"]],
         ),
+        Node(
+            package="atos",
+            namespace="atos",
+            executable="monr_relay",
+            name="monr_relay",
+            parameters=[files["params"]],
+        ),
     ]
 
 

atos/modules/MonrRelay/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.8)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_C_STANDARD_REQUIRED ON)
+
+project(monr_relay)
+find_package(atos_interfaces REQUIRED)
+find_package(std_srvs REQUIRED)
+
+# Define target names
+set(MODULE_TARGET ${PROJECT_NAME})
+
+set(COREUTILS_LIBRARY ATOSCoreUtil)
+set(COMMON_LIBRARY ATOSCommon) # Common library for ATOS with e.g. Trajectory class
+set(SOCKET_LIBRARY TCPUDPSocket) # Socket library for TCP/UDP communication
+
+get_target_property(COMMON_HEADERS ${COMMON_LIBRARY} INCLUDE_DIRECTORIES)
+get_target_property(COREUTILS_HEADERS ${COREUTILS_LIBRARY} INCLUDE_DIRECTORIES)
+
+include(GNUInstallDirs)
+
+# Create project main executable target
+add_executable(${MODULE_TARGET}
+	${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/monr_relay.cpp
+)
+# Link project executable to common libraries
+set(TARGET_LIBRARIES
+	${COREUTILS_LIBRARY}
+	${SOCKET_LIBRARY}
+	${COMMON_LIBRARY}
+)
+target_link_libraries(${MODULE_TARGET} ${TARGET_LIBRARIES})
+
+# Add include directories
+target_include_directories(${MODULE_TARGET} PUBLIC SYSTEM
+	${CMAKE_CURRENT_SOURCE_DIR}/inc
+	${COMMON_HEADERS}
+	${COREUTILS_HEADERS}
+)
+
+# ROS specific rules
+ament_target_dependencies(${MODULE_TARGET}
+	rclcpp
+	std_msgs
+	std_srvs
+	atos_interfaces
+)
+
+# Installation rules
+install(CODE "MESSAGE(STATUS \"Installing target ${MODULE_TARGET}\")")
+install(TARGETS ${MODULE_TARGET}
+	RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos"
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)