Arc 63 Add trajectory follower wrapper (#2379)

* add trajectory follower wrapper to launch file

* add trajectory follower wrapper pkg

* add pkg to sonarqube

* fix sonarqube

* address comments and add unit test

* update unit test

* address comments2

* address comments3

* update logic and unit test

* fix unit test

* address comments

* address comments

* expose callbacks to enable unit testing

* address comments2

* update unit test

* add and update unit tests

* address pr comments

* address PR comments and added documentation

* launch fixes

.sonarqube/sonar-scanner.properties

@@ -77,7 +77,8 @@ sonar.modules= bsm_generator, \
  carma_guidance_plugins, \
  carma_cloud_client, \
  approaching_emergency_vehicle_plugin, \
- carma_cooperative_perception
+ carma_cooperative_perception, \
+ trajectory_follower_wrapper
 
 guidance.sonar.projectBaseDir = /opt/carma/src/carma-platform/guidance
 bsm_generator.sonar.projectBaseDir = /opt/carma/src/carma-platform/bsm_generator
@@ -125,6 +126,7 @@ carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/carma-platform/carm
 carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cloud_client
 approaching_emergency_vehicle_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/approaching_emergency_vehicle_plugin
 carma_cooperative_perception.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cooperative_perception
+trajectory_follower_wrapper.sonar.projectBaseDir = /opt/carma/src/carma-platform/trajectory_follower_wrapper
 
 # C++ Package differences
 # Sources
@@ -218,6 +220,8 @@ approaching_emergency_vehicle_plugin.sonar.sources = src
 approaching_emergency_vehicle_plugin.sonar.exclusions =test/**
 carma_cooperative_perception.sonar.sources = src
 carma_cooperative_perception.sonar.exclusions = test/**
+trajectory_follower_wrapper.sonar.sources = src
+trajectory_follower_wrapper.sonar.exclusions = test/**
 
 # Tests
 # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated.
@@ -267,3 +271,4 @@ carma_guidance_plugins.sonar.sources = test
 carma_cloud_client.sonar.sources = test
 approaching_emergency_vehicle_plugin.sonar.sources = test
 carma_cooperative_perception.tests = test
+trajectory_follower_wrapper.tests = test

carma/launch/plugins.launch.py

@@ -84,6 +84,9 @@ def generate_launch_description():
  stop_controlled_intersection_tactical_plugin_file_path = os.path.join(
  get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml')
 
+ trajectory_follower_wrapper_param_file = os.path.join(
+ get_package_share_directory('trajectory_follower_wrapper'), 'config/parameters.yaml')
+
  env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }')
 
  pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"]
@@ -451,8 +454,37 @@ def generate_launch_description():
  ],
  parameters=[
  vehicle_characteristics_param_file, #vehicle_response_lag
- pure_pursuit_tuning_parameters, #pure_pursuit calibration parameters
- vehicle_config_param_file
+ vehicle_config_param_file,
+ pure_pursuit_tuning_parameters
+ ]
+ ),
+ ]
+ )
+
+ carma_trajectory_follower_wrapper_container = ComposableNodeContainer(
+ package='carma_ros2_utils',
+ name='carma_trajectory_follower_wrapper_container',
+ executable='carma_component_container_mt',
+ namespace=GetCurrentNamespace(),
+ composable_node_descriptions=[
+ ComposableNode(
+ package='trajectory_follower_wrapper',
+ plugin='trajectory_follower_wrapper::TrajectoryFollowerWrapperNode',
+ name='trajectory_follower_wrapper',
+ extra_arguments=[
+ {'use_intra_process_comms': True},
+ {'--log-level' : GetLogLevel('trajectory_follower_wrapper', env_log_levels) }
+ ],
+ remappings = [
+ ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
+ ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
+ ("trajectory_follower_wrapper/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/trajectory_follower_wrapper/plan_trajectory" ] ),
+ ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+ ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
+ ],
+ parameters=[
+ vehicle_characteristics_param_file,
+ trajectory_follower_wrapper_param_file
  ]
  ),
  ]
@@ -595,6 +627,7 @@ def generate_launch_description():
  carma_yield_plugin_container,
  carma_light_controlled_intersection_plugins_container,
  carma_pure_pursuit_wrapper_container,
+ carma_trajectory_follower_wrapper_container,
  #platooning_strategic_plugin_container,
  platooning_tactical_plugin_container,
  intersection_transit_maneuvering_container

carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp

@@ -28,12 +28,12 @@ namespace carma_guidance_plugins
 {
 
  /**
- * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API. 
- * 
- * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory. 
- * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system. 
- * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency. 
- * 
+ * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API.
+ *
+ * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory.
+ * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system.
+ * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency.
+ *
  */
  class ControlPlugin : public PluginBaseNode
  {
@@ -50,11 +50,6 @@ namespace carma_guidance_plugins
  // Timers
  rclcpp::TimerBase::SharedPtr command_timer_;
 
- // These callbacks do direct assignment into their respective member variables
- void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
- void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg);
- void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
-
 
  protected:
 
@@ -68,24 +63,30 @@ namespace carma_guidance_plugins
  //! The most recent trajectory received by this plugin
  boost::optional<carma_planning_msgs::msg::TrajectoryPlan> current_trajectory_;
 
+ // These callbacks do direct assignment into their respective member variables
+ void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
+ void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg);
+ void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
+
+
 
  public:
  /**
- * \brief ControlPlugin constructor 
+ * \brief ControlPlugin constructor
  */
  explicit ControlPlugin(const rclcpp::NodeOptions &);
 
  //! Virtual destructor for safe deletion
  virtual ~ControlPlugin() = default;
 
  /**
- * \brief Extending class provided method which should generate a command message 
+ * \brief Extending class provided method which should generate a command message
  * which will be published to the required topic by the base class
- * 
+ *
  * NOTE: Implementer can determine if trajectory has changed based on current_trajectory_->trajectory_id
- * 
+ *
  * \return The command message to publish
- */ 
+ */
  virtual autoware_msgs::msg::ControlCommandStamped generate_command() = 0;
 
  ////
@@ -102,7 +103,7 @@ namespace carma_guidance_plugins
  carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final;
  carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final;
  carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final;
- carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; 
+ carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final;
  carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final;
  };
 

trajectory_follower_wrapper/CMakeLists.txt

@@ -0,0 +1,72 @@
+
+# Copyright (C) 2024 LEIDOS.
+#
+# 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.
+
+cmake_minimum_required(VERSION 3.5)
+project(trajectory_follower_wrapper)
+
+# Declare carma package and check ROS version
+find_package(carma_cmake_common REQUIRED)
+carma_check_ros_version(2)
+carma_package()
+
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Name build targets
+set(node_exec trajectory_follower_wrapper_node_exec)
+set(node_lib trajectory_follower_wrapper_node)
+
+# Includes
+include_directories(
+ include
+)
+
+# Build
+ament_auto_add_library(${node_lib} SHARED
+ src/trajectory_follower_wrapper_node.cpp
+)
+
+ament_auto_add_executable(${node_exec}
+ src/main.cpp
+)
+
+# Register component
+rclcpp_components_register_nodes(${node_lib} "trajectory_follower_wrapper::TrajectoryFollowerWrapperNode")
+
+# All locally created targets will need to be manually linked
+# ament auto will handle linking of external dependencies
+target_link_libraries(${node_exec}
+ ${node_lib}
+)
+
+# Testing
+if(BUILD_TESTING)
+
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable
+
+ ament_add_gtest(test_trajectory_follower_wrapper test/test_trajectory_follower_wrapper.cpp)
+
+ ament_target_dependencies(test_trajectory_follower_wrapper ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
+
+ target_link_libraries(test_trajectory_follower_wrapper ${node_lib})
+
+endif()
+
+# Install
+ament_auto_package(
+ INSTALL_TO_SHARE config launch
+)

trajectory_follower_wrapper/README.md

@@ -0,0 +1,3 @@
+# trajectory_follower_wrapper
+
+Design Document: https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2899312668/Detailed+Design+-+Trajectory+Follower+Wrapper

trajectory_follower_wrapper/config/parameters.yaml

@@ -0,0 +1,2 @@
+# acceptable time difference from autoware's control command to still use the command in sec
+incoming_cmd_time_threshold: 1.0

trajectory_follower_wrapper/include/trajectory_follower_wrapper/trajectory_follower_wrapper_config.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+/*
+ * Copyright (C) 2024 LEIDOS.
+ *
+ * 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.
+ */
+
+#include <iostream>
+
+namespace trajectory_follower_wrapper
+{
+
+ /**
+ * \brief Stuct containing the algorithm configuration values for trajectory_follower_wrapper
+ */
+ struct TrajectoryFollowerWrapperConfig
+ {
+ double vehicle_response_lag = 0.2; // approximation of the delay (sec) between sent vehicle commands and the vehicle beginning a meaningful acceleration to that command"
+ double incoming_cmd_time_threshold = 1.0; // acceptable time difference from autoware's contrl command to still use the command in sec
+
+
+ // Stream operator for this config
+ friend std::ostream &operator<<(std::ostream &output, const TrajectoryFollowerWrapperConfig &c)
+ {
+ output << "trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig { " << std::endl
+ << "vehicle_response_lag: " << c.vehicle_response_lag << std::endl
+ << "incoming_cmd_time_threshold: " << c.incoming_cmd_time_threshold << std::endl
+ << "}" << std::endl;
+ return output;
+ }
+ };
+
+} // trajectory_follower_wrapper