Feature/py scenario module

.pre-commit-config.yaml

@@ -5,4 +5,8 @@ repos:
     hooks:
       - id: clang-format
         args: [--style=LLVM, -i]
-      - id: cppcheck
+      - id: cppcheck
+  - repo: https://github.com/ambv/black
+    rev: 24.4.2
+    hooks:
+      - id: black

atos/CMakeLists.txt

@@ -45,9 +45,13 @@ list(APPEND CMAKE_MODULE_PATH "${UTIL_CMAKE_ROOT}")
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
 find_package(std_msgs REQUIRED)
 
+ament_python_install_package(modules)
+
 # Enable or disable modules
 set(WITH_OBJECT_CONTROL ON CACHE BOOL "Enable ObjectControl module")
 set(WITH_DIRECT_CONTROL OFF CACHE BOOL "Enable DirectControl module")
@@ -59,6 +63,7 @@ set(WITH_MQTT_BRIDGE ON CACHE BOOL "Enable MQTTBridge module")
 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(ENABLE_TESTS ON CACHE BOOL "Enable testing on build")
 
@@ -106,8 +111,6 @@ add_subdirectory(common/sockets)
 add_subdirectory(common)
 SET_COMPILER_WARNINGS_GCC_CLANG("ON")
 
-# Subdirectories with compiler warnings
-add_subdirectory(modules/ATOSBase)
 
 foreach(MODULE ${ENABLED_MODULES})
 	add_subdirectory(modules/${MODULE})
@@ -116,11 +119,24 @@ endforeach()
 if(BUILD_TESTING)
   add_subdirectory(modules/SampleModule)
   find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
 #   ament_lint_auto_find_test_dependencies() # Comment this out for now to avoid linting errors
+  set(_pytest_tests
+     modules/OpenScenarioGateway/tests/test_openscenariogateway.py
+  )
+  foreach(_test_path ${_pytest_tests})
+  get_filename_component(_test_name ${_test_path} NAME_WE)
+  ament_add_pytest_test(${_test_name} ${_test_path}
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+    TIMEOUT 60
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+  )
+  endforeach()
 endif()
 
-ament_package()
+install(PROGRAMS modules/OpenScenarioGateway/openscenariogateway.py DESTINATION lib/atos)
 
+ament_package()
 
 # Install configuration
 include(GNUInstallDirs)

atos/common/CRSTransformation.cpp

@@ -6,102 +6,126 @@
 #include "CRSTransformation.hpp"
 
 /**
- * @brief Construct a new CRSTransformation::CRSTransformation object to create a pipeline
- * 				between two known coordinate reference systems
- * 
+ * @brief Construct a new CRSTransformation::CRSTransformation object to create
+ * a pipeline between two known coordinate reference systems
+ *
  * @param fromCRS Coordinate reference system from
  * @param toCRS Coordinate reference system to
  */
 
-
-CRSTransformation::CRSTransformation(const std::string &fromCRS, const std::string &toCRS) :
- ctxt(proj_context_create(), [](PJ_CONTEXT* ctxt){ proj_context_destroy(ctxt); }),
-	projection(proj_create_crs_to_crs(ctxt.get(), fromCRS.c_str(), toCRS.c_str(), nullptr),
-		[](PJ* proj){ proj_destroy(proj); })
-{
-	if (projection == nullptr) {
-		throw std::logic_error("Failed to create CRS conversion from " + fromCRS
-			+ " to " + toCRS + ": " + proj_context_errno_string(ctxt.get(),
-				proj_context_errno(ctxt.get())));
-	}
+CRSTransformation::CRSTransformation(const std::string &fromCRS,
+                                     const std::string &toCRS)
+    : ctxt(proj_context_create(),
+           [](PJ_CONTEXT *ctxt) { proj_context_destroy(ctxt); }),
+      projection(proj_create_crs_to_crs(ctxt.get(), fromCRS.c_str(),
+                                        toCRS.c_str(), nullptr),
+                 [](PJ *proj) { proj_destroy(proj); }) {
+  if (projection == nullptr) {
+    throw std::logic_error(
+        "Failed to create CRS conversion from " + fromCRS + " to " + toCRS +
+        ": " +
+        proj_context_errno_string(ctxt.get(), proj_context_errno(ctxt.get())));
+  }
 }
 
 /**
  * @brief Apply transformation from fromCRS to toCRS
- * 
+ *
  * @param traj reference to trajectory to transform
  */
-void CRSTransformation::apply(std::vector<ATOS::Trajectory::TrajectoryPoint> &trajPoints) {
-	// Put TrajPoints into array of PJ_POINTS
-	PJ_COORD in[trajPoints.size()];
-	auto arraySize = sizeof(in) / sizeof(in[0]);
-	for (int i = 0; i < arraySize; i++){
-		in[i].xyz.x = trajPoints[i].getXCoord();
-		in[i].xyz.y = trajPoints[i].getYCoord();
-		in[i].xyz.z = trajPoints[i].getZCoord();
-	}
+void CRSTransformation::apply(
+    std::vector<ATOS::Trajectory::TrajectoryPoint> &trajPoints) {
+
+  // Put TrajPoints into array of PJ_POINTS
+  PJ_COORD in[trajPoints.size()];
+  auto arraySize = sizeof(in) / sizeof(in[0]);
+  for (int i = 0; i < arraySize; i++) {
+    in[i].xyz.x = trajPoints[i].getXCoord();
+    in[i].xyz.y = trajPoints[i].getYCoord();
+    in[i].xyz.z = trajPoints[i].getZCoord();
+  }
 
-	RCLCPP_DEBUG(logger, "Converting trajectory with %ld points", arraySize);
-	if (0 != proj_trans_array(projection.get(), PJ_FWD, arraySize, in)){
-		throw std::runtime_error("Failed to convert trajectory");
-	}
+  RCLCPP_DEBUG(logger, "Converting trajectory with %ld points", arraySize);
+  if (0 != proj_trans_array(projection.get(), PJ_FWD, arraySize, in)) {
+    throw std::runtime_error("Failed to convert trajectory");
+  }
 
-	// Put transformed PJ_POINTS back into TrajPoints
-	for (int i = 0; i < arraySize; i++){
-		trajPoints[i].setXCoord(in[i].xyz.x);
-		trajPoints[i].setYCoord(in[i].xyz.y);
-		trajPoints[i].setZCoord(in[i].xyz.z);
-	}
+  // Put transformed PJ_POINTS back into TrajPoints
+  for (int i = 0; i < arraySize; i++) {
+    if (isnan(in[i].xyz.x) || isnan(in[i].xyz.y) || isnan(in[i].xyz.z)) {
+      // Apply transformation to the point again. quick fix.
+      // TODO: Need to understand why some points are not transformed
+      auto point = geometry_msgs::msg::Point();
+      point.x = trajPoints[i].getXCoord();
+      point.y = trajPoints[i].getYCoord();
+      point.z = trajPoints[i].getZCoord();
+      apply(point, PJ_FWD);
+      in[i].xyz.x = point.x;
+      in[i].xyz.y = point.y;
+      in[i].xyz.z = point.z;
+    }
+
+    trajPoints[i].setXCoord(in[i].xyz.x);
+    trajPoints[i].setYCoord(in[i].xyz.y);
+    trajPoints[i].setZCoord(in[i].xyz.z);
+  }
 }
 
 /**
  * @brief Apply a transform on a point
- * 
+ *
  * @param point The point to transform
  * @param direction Which direction to transform, e.g. PJ_FWD or PJ_INV
  */
-void CRSTransformation::apply(geometry_msgs::msg::Point &point, PJ_DIRECTION direction) {
-	PJ_COORD in = proj_coord(point.x, point.y, point.z, 0);
-	PJ_COORD out = proj_trans(projection.get(), direction, in);
-	point.x = out.xyz.x;
-	point.y = out.xyz.y;
-	point.z = out.xyz.z;
+void CRSTransformation::apply(geometry_msgs::msg::Point &point,
+                              PJ_DIRECTION direction) {
+  PJ_COORD in = proj_coord(point.x, point.y, point.z, 0);
+  PJ_COORD out = proj_trans(projection.get(), direction, in);
+  point.x = out.xyz.x;
+  point.y = out.xyz.y;
+  point.z = out.xyz.z;
 }
 
 /**
  * @brief Get the origin lat, lon, height in the given datum from a proj string
- * 
+ *
  * @parameter: projString proj string to transform
- * @parameter: datum datum to transform to 
+ * @parameter: datum datum to transform to
  * @return std::vector<double> Vector with lat, lon, height
  */
-std::vector<double> CRSTransformation::projToLLH(const std::string &projString, const std::string &datum) {
-    auto *pjSrc = proj_create_crs_to_crs(NULL, projString.c_str(), datum.c_str(), NULL);
-    if (pjSrc == NULL) {
-        throw std::runtime_error("Failed to create projPJ object");
-    }
+std::vector<double> CRSTransformation::projToLLH(const std::string &projString,
+                                                 const std::string &datum) {
+  auto *pjSrc =
+      proj_create_crs_to_crs(NULL, projString.c_str(), datum.c_str(), NULL);
+  if (pjSrc == NULL) {
+    throw std::runtime_error("Failed to create projPJ object");
+  }
 
-    PJ_COORD src = proj_coord(0, 0, 0, 0);
-    PJ_COORD dst = proj_trans(pjSrc, PJ_FWD, src);
+  PJ_COORD src = proj_coord(0, 0, 0, 0);
+  PJ_COORD dst = proj_trans(pjSrc, PJ_FWD, src);
 
-	std::vector<double> result = {dst.xyz.x, dst.xyz.y, dst.xyz.z};
-    proj_destroy(pjSrc);
+  std::vector<double> result = {dst.xyz.x, dst.xyz.y, dst.xyz.z};
+  proj_destroy(pjSrc);
 
-    return result;
+  return result;
 }
 
 /**
- * @brief Returns a new latitude, longitude, height after offsetting x, y, z meters
- * 
+ * @brief Returns a new latitude, longitude, height after offsetting x, y, z
+ * meters
+ *
  * @param llh The latitude, longitude, height [degrees, degrees, meters]
  * @param xyzOffset Meters offset from llh [meters, meters, meters]
  */
 void CRSTransformation::llhOffsetMeters(double *llh, const double *xyzOffset) {
-	constexpr double EARTH_EQUATOR_RADIUS_M = 6378137.0;	// earth semimajor axis (WGS84) (m)
-	const auto [lat, lon, hgt] = std::make_tuple(llh[0], llh[1], llh[2]);
-	const auto [dx, dy, dz] = std::make_tuple(xyzOffset[0], xyzOffset[1], xyzOffset[2]);
+  constexpr double EARTH_EQUATOR_RADIUS_M =
+      6378137.0; // earth semimajor axis (WGS84) (m)
+  const auto [lat, lon, hgt] = std::make_tuple(llh[0], llh[1], llh[2]);
+  const auto [dx, dy, dz] =
+      std::make_tuple(xyzOffset[0], xyzOffset[1], xyzOffset[2]);
 
-	llh[0] = lat + (dy / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI);
-	llh[1] = lon + (dx / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI) / std::cos(lat * M_PI / 180.0);
-	llh[2] = hgt + dz;
+  llh[0] = lat + (dy / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI);
+  llh[1] = lon + (dx / EARTH_EQUATOR_RADIUS_M) * (180 / M_PI) /
+                     std::cos(lat * M_PI / 180.0);
+  llh[2] = hgt + dz;
 }

atos/common/module.hpp

@@ -20,6 +20,7 @@ const std::string getObjectTrajectory = "get_object_trajectory";
 const std::string getObjectTriggerStart = "get_object_trigger_start";
 const std::string getObjectControlState = "get_object_control_state";
 const std::string getObjectReturnTrajectory = "get_object_return_trajectory";
+const std::string getOpenScenarioFilePath = "get_open_scenario_file_path";
 }
 
 // TODO move somewhere else? also make generic to allow more args (variadic template)?

atos/common/roschannels/scenariochannel.hpp

@@ -0,0 +1,33 @@
+/*
+ * 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/story_board_element_state_change.hpp"
+#include "roschannel.hpp"
+
+namespace ROSChannels {
+namespace StoryBoardElementStateChange {
+const std::string topicName = "story_board_element_state_change";
+using message_type = atos_interfaces::msg::StoryBoardElementStateChange;
+
+class Pub : public BasePub<message_type> {
+public:
+  Pub(rclcpp::Node &node)
+      : BasePub<message_type>(
+            node, topicName,
+            rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {}
+};
+
+class Sub : public BaseSub<message_type> {
+public:
+  Sub(rclcpp::Node &node,
+      std::function<void(const message_type::SharedPtr)> callback)
+      : BaseSub<message_type>(
+            node, topicName, callback,
+            rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()) {}
+};
+} // namespace StoryBoardElementStateChange
+} // namespace ROSChannels

atos/launch/launch_experimental.py

@@ -1,9 +1,13 @@
 import sys
 import os
 from ament_index_python.packages import get_package_prefix
-sys.path.insert(0, os.path.join( # Need to modify the sys.path since we launch from the ros2 installed path
-    get_package_prefix('atos'),
-    'share', 'atos', 'launch'))
+
+sys.path.insert(
+    0,
+    os.path.join(  # Need to modify the sys.path since we launch from the ros2 installed path
+        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
@@ -13,43 +17,44 @@ def get_experimental_nodes():
     files = launch_base.get_files()
     return [
         Node(
-            package='atos',
-            namespace='atos',
-            executable='trajectorylet_streamer',
-            name='trajectorylet_streamer',
-            parameters=[files["params"]]
+            package="atos",
+            namespace="atos",
+            executable="trajectorylet_streamer",
+            name="trajectorylet_streamer",
+            parameters=[files["params"]],
         ),
         Node(
-            package='atos',
-            namespace='atos',
-            executable='mqtt_bridge',
-            name='mqtt_bridge',
+            package="atos",
+            namespace="atos",
+            executable="mqtt_bridge",
+            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',
-            namespace='atos',
-            executable='osi_adapter',
-            name='osi_adapter',
-            parameters=[files["params"]]
+            package="atos",
+            namespace="atos",
+            executable="osi_adapter",
+            name="osi_adapter",
+            parameters=[files["params"]],
         ),
         Node(
-            package='atos',
-            namespace='atos',
-            executable='back_to_start',
-            name='back_to_start',
-            parameters=[files["params"]]
-        )
+            package="atos",
+            namespace="atos",
+            executable="back_to_start",
+            name="back_to_start",
+            parameters=[files["params"]],
+        ),
     ]
 
+
 def generate_launch_description():
     base_nodes = launch_base.get_base_nodes()
-    
+
     experimental_nodes = get_experimental_nodes()
 
     for node in experimental_nodes:
         base_nodes.append(node)
-        
+
     return LaunchDescription(base_nodes)