Merge pull request #650 from RI-SE/feature/extend_osc_support

Feature/extend osc support

atos/CMakeLists.txt

@@ -64,6 +64,7 @@ set(WITH_POINTCLOUD_PUBLISHER ON CACHE BOOL "Enable PointcloudPublisher module")
 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(ENABLE_TESTS ON CACHE BOOL "Enable testing on build")
 
@@ -98,6 +99,9 @@ endif()
 if(WITH_BACK_TO_START)
         list(APPEND ENABLED_MODULES BackToStart)
 endif()
+if(WITH_REST_BRIDGE)
+        list(APPEND ENABLED_MODULES RESTBridge)
+endif()
 
 
 # Add corresponding subprojects
@@ -123,6 +127,7 @@ if(BUILD_TESTING)
 #   ament_lint_auto_find_test_dependencies() # Comment this out for now to avoid linting errors
   set(_pytest_tests
      modules/OpenScenarioGateway/tests/test_openscenariogateway.py
+     modules/OpenScenarioGateway/tests/test_storyboard_handler.py
   )
   foreach(_test_path ${_pytest_tests})
   get_filename_component(_test_name ${_test_path} NAME_WE)

atos/common/roschannels/customcommandaction.hpp

@@ -0,0 +1,31 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "atos_interfaces/msg/custom_command_action.hpp"
+#include "roschannel.hpp"
+
+namespace ROSChannels {
+namespace CustomCommandAction {
+const std::string topicName = "custom_command_action";
+using message_type = atos_interfaces::msg::CustomCommandAction;
+const rclcpp::QoS defaultQoS = rclcpp::QoS(rclcpp::KeepLast(1));
+
+class Pub : public BasePub<message_type> {
+public:
+  Pub(rclcpp::Node &node, const rclcpp::QoS &qos = defaultQoS)
+      : BasePub<message_type>(node, topicName, qos) {}
+};
+
+class Sub : public BaseSub<message_type> {
+public:
+  Sub(rclcpp::Node &node,
+      std::function<void(const message_type::SharedPtr)> callback,
+      const rclcpp::QoS &qos = defaultQoS)
+      : BaseSub<message_type>(node, topicName, callback, qos) {}
+};
+} // namespace CustomCommandAction
+} // namespace ROSChannels

atos/launch/launch_experimental.py

@@ -30,7 +30,7 @@ def get_experimental_nodes():
             name="mqtt_bridge",
             # prefix=['gdbserver localhost:3000'], ## To use with VSC debugger
             parameters=[files["params"]],
-            arguments=["--ros-args", "--log-level", "debug"],  # To get RCL_DEBUG prints
+            # arguments=["--ros-args", "--log-level", "debug"],  # To get RCL_DEBUG prints
         ),
         Node(
             package="atos",
@@ -46,6 +46,13 @@ def get_experimental_nodes():
             name="back_to_start",
             parameters=[files["params"]],
         ),
+        Node(
+            package="atos",
+            namespace="atos",
+            executable="rest_bridge",
+            name="rest_bridge",
+            parameters=[files["params"]],
+        ),
     ]
 
 

atos/modules/EsminiAdapter/inc/esminiadapter.hpp

@@ -5,51 +5,49 @@
  */
 #pragma once
 
+#include "CRSTransformation.hpp"
+#include "esmini/esminiLib.hpp"
+#include "esmini/esminiRMLib.hpp"
 #include "module.hpp"
-#include "roschannels/v2xchannel.hpp"
 #include "roschannels/commandchannels.hpp"
-#include "roschannels/pathchannel.hpp"
-#include "roschannels/monitorchannel.hpp"
 #include "roschannels/gnsspathchannel.hpp"
-#include "roschannels/statechange.hpp"
+#include "roschannels/monitorchannel.hpp"
+#include "roschannels/pathchannel.hpp"
 #include "roschannels/scenariochannel.hpp"
-#include <unordered_map>
+#include "roschannels/statechange.hpp"
 #include <filesystem>
-#include "esmini/esminiLib.hpp"
-#include "esmini/esminiRMLib.hpp"
-#include "CRSTransformation.hpp"
+#include <unordered_map>
 
-#include "trajectory.hpp"
-#include "atos_interfaces/srv/get_test_origin.hpp"
+#include "atos_interfaces/srv/get_object_ids.hpp"
 #include "atos_interfaces/srv/get_object_trajectory.hpp"
 #include "atos_interfaces/srv/get_object_trigger_start.hpp"
 #include "atos_interfaces/srv/get_open_scenario_file_path.hpp"
-#include "atos_interfaces/srv/get_object_ids.hpp"
+#include "atos_interfaces/srv/get_test_origin.hpp"
+#include "trajectory.hpp"
 
 /*!
- * \brief The EsminiAdapter class is a singleton class that 
+ * \brief The EsminiAdapter class is a singleton class that
  * 	handles the communication between the esmini simulator and ATOS
  */
 class EsminiAdapter : public Module {
 public:
 	static inline std::string const moduleName = "esmini_adapter";
 	static inline std::filesystem::path oscFilePath;
 	static int initializeModule();
-	EsminiAdapter(EsminiAdapter const&) = delete;
-    EsminiAdapter& operator=(EsminiAdapter const&) = delete;
+	EsminiAdapter(EsminiAdapter const &) = delete;
+	EsminiAdapter &operator=(EsminiAdapter const &) = delete;
 	static std::shared_ptr<EsminiAdapter> instance();
 
 private:
 	EsminiAdapter();
 
-	ROSChannels::StartObject::Pub startObjectPub;
-	ROSChannels::V2X::Pub v2xPub;
-	ROSChannels::StoryBoardElementStateChange::Pub storyBoardElementStateChangePub;
+	ROSChannels::StoryBoardElementStateChange::Pub
+		storyBoardElementStateChangePub;
 	ROSChannels::ConnectedObjectIds::Sub connectedObjectIdsSub;
 	ROSChannels::Exit::Sub exitSub;
 	ROSChannels::StateChange::Sub stateChangeSub;
-	std::unordered_map<uint32_t,ROSChannels::Path::Pub> pathPublishers;
-	std::unordered_map<uint32_t,ROSChannels::GNSSPath::Pub> gnssPathPublishers;
+	std::unordered_map<uint32_t, ROSChannels::Path::Pub> pathPublishers;
+	std::unordered_map<uint32_t, ROSChannels::GNSSPath::Pub> gnssPathPublishers;
 
 	static std::unordered_map<uint32_t,std::shared_ptr<ROSChannels::Monitor::Sub>> monrSubscribers;
 	static std::shared_ptr<rclcpp::Service<atos_interfaces::srv::GetObjectTrajectory>> objectTrajectoryService;
@@ -61,7 +59,6 @@ class EsminiAdapter : public Module {
 	rclcpp::Client<atos_interfaces::srv::GetOpenScenarioFilePath>::SharedPtr oscFilePathClient_;	//!< Client to request the current open scenario file path
 	rclcpp::Client<atos_interfaces::srv::GetObjectIds>::SharedPtr objectIdsClient_;	//!< Client to request the ATOS object id for each openx entity name
 
-
 	void onMonitorMessage(const ROSChannels::Monitor::message_type::SharedPtr monr, uint32_t id);
 	// Below is a quickfix, fix properly later
 	static void fetchOSCFilePath();
@@ -80,16 +77,15 @@ class EsminiAdapter : public Module {
 	static ATOS::Trajectory getTrajectoryFromObjectState(uint32_t,std::vector<SE_ScenarioObjectState>& states);
 	static std::string projStrFromGeoReference(RM_GeoReference& geoRef);
 	static std::map<uint32_t, ATOS::Trajectory> extractTrajectories(double timeStep);
-	static bool isSendDenmAction(const std::string& action);
-	static ROSChannels::V2X::message_type denmFromTestOrigin(double *llh);
 
 	static void onRequestObjectTrajectory(
-		const std::shared_ptr<atos_interfaces::srv::GetObjectTrajectory::Request> req,
+		const std::shared_ptr<atos_interfaces::srv::GetObjectTrajectory::Request>
+			req,
 		std::shared_ptr<atos_interfaces::srv::GetObjectTrajectory::Response> res);
-
 
-	static void onRequestTestOrigin(const std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Request>,
-							std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Response>);
+	static void onRequestTestOrigin(
+		const std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Request>,
+		std::shared_ptr<atos_interfaces::srv::GetTestOrigin::Response>);
 
 	static std::unordered_map<int, std::string> atosIDToObjectName;
 	static std::unordered_map<std::string, int> objectNameToAtosId;

atos/modules/EsminiAdapter/src/esminiadapter.cpp

@@ -42,8 +42,6 @@ std::shared_ptr<rclcpp::Service<TestOriginSrv>> EsminiAdapter::testOriginService
 geographic_msgs::msg::GeoPose EsminiAdapter::testOrigin = geographic_msgs::msg::GeoPose();
 
 EsminiAdapter::EsminiAdapter() : Module(moduleName),
-	startObjectPub(*this),
-	v2xPub(*this),
 	storyBoardElementStateChangePub(*this),
 	connectedObjectIdsSub(*this, &EsminiAdapter::onConnectedObjectIdsMessage),
 	exitSub(*this, &EsminiAdapter::onStaticExitMessage),
@@ -103,8 +101,6 @@ std::shared_ptr<EsminiAdapter> EsminiAdapter::instance() {
 		me->connectedObjectIdsSub = ROSChannels::ConnectedObjectIds::Sub(*me,&EsminiAdapter::onConnectedObjectIdsMessage);
 		me->exitSub = ROSChannels::Exit::Sub(*me,&EsminiAdapter::onStaticExitMessage);
 		me->stateChangeSub = ROSChannels::StateChange::Sub(*me,&EsminiAdapter::onStaticStateChangeMessage);
-		// Start V2X publisher
-		me->v2xPub = ROSChannels::V2X::Pub(*me);
 
 	}
 	return me;
@@ -189,16 +185,6 @@ void EsminiAdapter::handleStartCommand()
 	RCLCPP_INFO(me->get_logger(), "Esmini ScenarioEngine started");
 }
 
-/*!
- * \brief Check if action is a DENM action.
- * \param action Action name
- * \return true if action is a DENM action, false otherwise
-*/
-bool EsminiAdapter::isSendDenmAction(const std::string& action)
-{
-	return std::regex_search(action, std::regex("denm", std::regex_constants::icase));
-}
-
 /*!
  * \brief Callback to be executed by esmini when story board state changes.
  * 		If story board element is an action, and the action is supported, the action is run.
@@ -223,20 +209,6 @@ void EsminiAdapter::handleStoryBoardElementChange(
 	me->storyBoardElementStateChangePub.publish(msg);
 }
 
-ROSChannels::V2X::message_type EsminiAdapter::denmFromTestOrigin(double *llh) {
-	ROSChannels::V2X::message_type denm;
-	denm.message_type = "DENM";
-	denm.event_id = "ATOSEvent1";
-	denm.cause_code = 12;
-	denm.latitude = static_cast<int32_t>(llh[0]*10000000); // Microdegrees
-	denm.longitude = static_cast<int32_t>(llh[1]*10000000);
-	denm.altitude = static_cast<int32_t>(llh[2]*100);		// Centimeters
-	denm.detection_time = std::chrono::duration_cast<std::chrono::seconds>( // Time since epoch in seconds
-		std::chrono::system_clock::now().time_since_epoch()
-	).count();
-	return denm;
-}
-
 /*!
  * \brief Utility function to convert a ROS MONR message to Esmini representation
  *			and report the object position to Esmini

atos/modules/MQTTBridge/inc/mqttbridge.hpp

@@ -10,8 +10,8 @@
 #include "atos_interfaces/srv/new_mqtt2_ros_bridge.hpp"
 #include "atos_interfaces/srv/new_ros2_mqtt_bridge.hpp"
 #include "module.hpp"
+#include "roschannels/customcommandaction.hpp"
 #include "roschannels/statechange.hpp"
-#include "roschannels/v2xchannel.hpp"
 #include <chrono>
 #include <fmt/format.h>
 #include <mqtt/async_client.h>
@@ -152,23 +152,24 @@ class MqttBridge : public Module,
   std::string password;
   std::string topic_prefix;
 
-  ROSChannels::V2X::Sub v2xMsgSub; //!< Subscriber to v2x messages requests
+  ROSChannels::CustomCommandAction::Sub
+      customCommandActionMsgSub; //!< Subscriber to v2x messages requests
   ROSChannels::StateChange::Sub
       obcStateChangeSub; //!< Subscriber to object state change requests
 
   void setupClient();
   void setupMqtt2RosBridge();
   void setupRos2MqttBridge();
-  void onV2xMsg(const ROSChannels::V2X::message_type::SharedPtr);
+  void onCustomCommandActionMsg(
+      const ROSChannels::CustomCommandAction::message_type::SharedPtr);
   void
   onObcStateChangeMsg(const ROSChannels::StateChange::message_type::SharedPtr);
 
   template <typename T>
   void onMessage(T msg, std::string mqtt_topic,
                  std::function<json(T)> convertFunc);
 
-  static json
-  v2xToJson(const ROSChannels::V2X::message_type::SharedPtr v2x_msg);
+  static json v2xToJson(const std::string v2x_msg);
   static json obcStateChangeToJson(
       const ROSChannels::StateChange::message_type::SharedPtr obc_msg);
 };

atos/modules/MQTTBridge/src/mqttbridge.cpp

@@ -12,7 +12,8 @@ using std::placeholders::_1;
 
 MqttBridge::MqttBridge()
     : Module(MqttBridge::moduleName),
-      v2xMsgSub(*this, std::bind(&MqttBridge::onV2xMsg, this, _1)),
+      customCommandActionMsgSub(
+          *this, std::bind(&MqttBridge::onCustomCommandActionMsg, this, _1)),
       obcStateChangeSub(*this,
                         std::bind(&MqttBridge::onObcStateChangeMsg, this, _1)) {
   this->loadParameters();
@@ -380,8 +381,15 @@ void MqttBridge::newRos2MqttBridge(
   response->success = true;
 }
 
-void MqttBridge::onV2xMsg(const V2X::message_type::SharedPtr v2x_msg) {
-  this->onMessage<V2X::message_type::SharedPtr>(v2x_msg, "v2x", v2xToJson);
+void MqttBridge::onCustomCommandActionMsg(
+    const CustomCommandAction::message_type::SharedPtr
+        custom_command_action_msg) {
+  if (custom_command_action_msg->type == "v2x") {
+    RCLCPP_INFO(this->get_logger(), "Received V2X message on %s topic",
+                CustomCommandAction::topicName.c_str());
+    this->onMessage<std::string>(custom_command_action_msg->content, "v2x",
+                                 v2xToJson);
+  }
 }
 
 void MqttBridge::onObcStateChangeMsg(
@@ -415,15 +423,26 @@ void MqttBridge::onMessage(T msg, std::string mqtt_topic,
   }
 }
 
-json MqttBridge::v2xToJson(const V2X::message_type::SharedPtr v2x_msg) {
+json MqttBridge::v2xToJson(std::string msg_content) {
+  std::replace(msg_content.begin(), msg_content.end(), '\'',
+               '"'); // Replace ROS single quotes string with double quotes to
+                     // make it a valid JSON string
   json j;
-  j["message_type"] = v2x_msg->message_type;
-  j["event_id"] = v2x_msg->event_id;
-  j["cause_code"] = v2x_msg->cause_code;
-  j["detection_time"] = v2x_msg->detection_time;
-  j["altitude"] = v2x_msg->altitude;
-  j["latitude"] = v2x_msg->latitude;
-  j["longitude"] = v2x_msg->longitude;
+  // Parse the string to a json object
+  try {
+    j = json::parse(msg_content);
+  } catch (json::parse_error &e) {
+    std::cerr << "Failed to parse JSON object: " << e.what() << std::endl;
+  }
+
+  if (j.find("message_type") == j.end()) {
+    std::cerr << "No message_type field in JSON object" << std::endl;
+    return j;
+  }
+  if (j["message_type"] == "DENM") {
+    j["detection_time"] =
+        std::chrono::system_clock::now().time_since_epoch().count();
+  }
   return j;
 }