Arc 257 humbler (#2476)

* added ported packages to list

* changed platooning_strategic_ihp to platoon_strategic_ihp and migrated to humble

* localization_manager humbles

* motion_computation humbles

* points_map_filter humbles

* route humbles

* route_following_plugin humbles

* renamed platoon_strategic_ihp to platooning_strategic_ihp

* applied clang format to motion_computation

.github/workflows/humble-upgraded-packages.txt

@@ -1 +1 @@
-approximate_intersection carma motion_prediction_visualizer system_controller object_visualizer mock_controller_driver object_detection_tracking gnss_to_map_convertor mobilitypath_visualizer bsm_generator trajectory_visualizer trajectory_executor lightbar_manager mobilitypath_publisher guidance port_drayage_plugin subsystem_controllers carma_wm roadway_objects basic_autonomy carma_guidance_plugins plan_delegator traffic_incident_parser platooning_tactical_plugin intersection_transit_maneuvering light_controlled_intersection_tactical_plugin stop_and_dwell_strategic_plugin cooperative_lanechange lci_strategic_plugin stop_controlled_intersection_tactical_plugin sci_strategic_plugin inlanecruising_plugin platooning_control trajectory_follower_wrapper approaching_emergency_vehicle_plugin yield_plugin arbitrator carma_cloud_client carma_cooperative_perception carma_wm_ctrl frame_transformer pure_pursuit_wrapper stop_and_wait_plugin
+approximate_intersection carma motion_prediction_visualizer system_controller object_visualizer mock_controller_driver object_detection_tracking gnss_to_map_convertor mobilitypath_visualizer bsm_generator trajectory_visualizer trajectory_executor lightbar_manager mobilitypath_publisher guidance port_drayage_plugin subsystem_controllers carma_wm roadway_objects basic_autonomy carma_guidance_plugins plan_delegator traffic_incident_parser platooning_tactical_plugin intersection_transit_maneuvering light_controlled_intersection_tactical_plugin stop_and_dwell_strategic_plugin cooperative_lanechange lci_strategic_plugin stop_controlled_intersection_tactical_plugin sci_strategic_plugin inlanecruising_plugin platooning_control trajectory_follower_wrapper approaching_emergency_vehicle_plugin yield_plugin arbitrator carma_cloud_client carma_cooperative_perception carma_wm_ctrl frame_transformer pure_pursuit_wrapper stop_and_wait_plugin localization_manager motion_computation platooning_strategic_ihp points_map_filter route route_following_plugin

arbitrator/config/arbitrator_params.yaml

@@ -33,10 +33,10 @@ plugin_priorities: '
  	  "plugin_priorities": 
       {
         "approaching_emergency_vehicle_plugin": 5.0,
-        "platoon_strategic_ihp": 4.0,
+        "platooning_strategic_ihp": 4.0,
         "lci_strategic_plugin": 3.0,
         "sci_strategic_plugin": 2.0,
-        "platoon_strategic": 2.0,
+        "platooning_strategic": 2.0,
         "route_following_plugin": 1.0
       }
   }

carma/launch/plugins.launch.py

@@ -60,8 +60,8 @@ def generate_launch_description():
     cooperative_lanechange_param_file = os.path.join(
         get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml')
 
-    platoon_strategic_ihp_param_file = os.path.join(
-        get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml')
+    platooning_strategic_ihp_param_file = os.path.join(
+        get_package_share_directory('platooning_strategic_ihp'), 'config/parameters.yaml')
 
     sci_strategic_plugin_file_path = os.path.join(
         get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml')
@@ -559,12 +559,12 @@ def generate_launch_description():
         namespace=GetCurrentNamespace(),
         composable_node_descriptions=[
             ComposableNode(
-                package='platoon_strategic_ihp',
-                plugin='platoon_strategic_ihp::Node',
-                name='platoon_strategic_ihp_node',
+                package='platooning_strategic_ihp',
+                plugin='platooning_strategic_ihp::Node',
+                name='platooning_strategic_ihp_node',
                 extra_arguments=[
                     {'use_intra_process_comms': True},
-                    {'--log-level' : GetLogLevel('platoon_strategic_ihp', env_log_levels) }
+                    {'--log-level' : GetLogLevel('platooning_strategic_ihp', env_log_levels) }
                 ],
                 remappings = [
                     ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ),
@@ -586,7 +586,7 @@ def generate_launch_description():
                     ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
                 ],
                 parameters=[
-                    platoon_strategic_ihp_param_file,
+                    platooning_strategic_ihp_param_file,
                     vehicle_config_param_file
                 ]
             ),

localization_manager/package.xml

@@ -34,6 +34,8 @@
     <depend>carma_localization_msgs</depend>
     <depend>autoware_msgs</depend>
     <depend>message_filters_humble</depend>
+    <depend>tf2_geometry_msgs</depend>
+    <depend>std_srvs</depend>
 
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_cmake_gtest</test_depend>

localization_manager/src/LocalizationManager.cpp

@@ -14,7 +14,7 @@
  * the License.
  */
 
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <algorithm>
 #include "localization_manager/LocalizationManager.hpp"
 
@@ -242,14 +242,14 @@ namespace localization_manager
             prev_ndt_stamp_ = boost::none;
 
             current_timer_id_ = nextId();
-            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6),
+            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration::from_nanoseconds(config_.auto_initialization_timeout * 1e6),
                                                         std::bind(&LocalizationManager::timerCallback, this, new_state), true, true);
 
             timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id
             break;
         case LocalizationState::DEGRADED_NO_LIDAR_FIX:
             current_timer_id_ = nextId();
-            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6),
+            current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration::from_nanoseconds(config_.gnss_only_operation_timeout * 1e6),
                                                         std::bind(&LocalizationManager::timerCallback, this, new_state), true, true);
 
             timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id
@@ -281,7 +281,7 @@ namespace localization_manager
         }
 
         // check if last gnss time stamp is older than threshold and send the corresponding signal
-        if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6))
+        if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration::from_nanoseconds(config_.gnss_data_timeout * 1e6))
         {
             transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT);
         }

motion_computation/include/motion_computation/motion_computation_node.hpp

@@ -16,7 +16,6 @@
 #define MOTION_COMPUTATION__MOTION_COMPUTATION_NODE_HPP_
 
 #include <lanelet2_extension/projection/local_frame_projector.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <functional>
 #include <vector>
@@ -29,6 +28,7 @@
 #include <carma_perception_msgs/msg/external_object_list.hpp>
 #include <carma_v2x_msgs/msg/mobility_path.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace motion_computation
 {

motion_computation/include/motion_computation/motion_computation_worker.hpp

@@ -18,7 +18,6 @@
 #include <gtest/gtest_prod.h>
 #include <lanelet2_extension/projection/local_frame_projector.h>
 #include <tf2/LinearMath/Transform.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <functional>
 #include <memory>
@@ -36,6 +35,7 @@
 #include <carma_v2x_msgs/msg/mobility_path.hpp>
 #include <carma_v2x_msgs/msg/psm.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace motion_computation
 {

motion_computation/src/bsm_to_external_object_convertor.cpp

@@ -201,7 +201,8 @@ void convert(
 
   out_msg.predictions = impl::predicted_poses_to_predicted_state(
     predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
-    rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
+    rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence,
+    out_msg.confidence);
   out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
 }
 

motion_computation/src/main.cpp

@@ -13,10 +13,12 @@
 // limitations under the License.
 
 #include <memory>
+
 #include "motion_computation/motion_computation_node.hpp"
 #include <rclcpp/rclcpp.hpp>
 
-int main(int argc, char **argv) {
+int main(int argc, char ** argv)
+{
   rclcpp::init(argc, argv);
 
   auto node = std::make_shared<motion_computation::MotionComputationNode>(rclcpp::NodeOptions());

motion_computation/src/mobility_path_to_external_object.cpp

@@ -93,7 +93,8 @@ void convert(
   double message_offset_y = 0.0;
   double message_offset_z = 0.0;
 
-  rclcpp::Duration mobility_path_point_delta_t(mobility_path_points_timestep_size * 1e9);
+  rclcpp::Duration mobility_path_point_delta_t =
+    rclcpp::Duration::from_nanoseconds(mobility_path_points_timestep_size * 1e9);
 
   // Note the usage of current vs previous in this loop can be a bit confusing
   // The intended behavior is we our always storing our prev_point but using

motion_computation/src/motion_computation_worker.cpp

@@ -143,8 +143,7 @@ void MotionComputationWorker::predictionLogic(
 void MotionComputationWorker::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg)
 {
   // Build projector from proj string
-  if (georeference_ != msg->data)
-  {
+  if (georeference_ != msg->data) {
     georeference_ = msg->data;
     map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
 
@@ -158,9 +157,9 @@ void MotionComputationWorker::georeferenceCallback(const std_msgs::msg::String::
 
     RCLCPP_DEBUG_STREAM(
       logger_->get_logger(), "Extracted NED in Map Rotation (x,y,z,w) : ( "
-                              << ned_in_map_rotation_.getX() << ", " << ned_in_map_rotation_.getY()
-                              << ", " << ned_in_map_rotation_.getZ() << ", "
-                              << ned_in_map_rotation_.getW());
+                               << ned_in_map_rotation_.getX() << ", " << ned_in_map_rotation_.getY()
+                               << ", " << ned_in_map_rotation_.getZ() << ", "
+                               << ned_in_map_rotation_.getW());
   }
 }
 

motion_computation/src/psm_to_external_object_convertor.cpp

@@ -14,7 +14,6 @@
 
 #include <lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h>
 #include <tf2/LinearMath/Transform.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <wgs84_utils/wgs84_utils.h>
 
 #include <algorithm>
@@ -30,6 +29,7 @@
 
 #include <carma_perception_msgs/msg/external_object.hpp>
 #include <carma_v2x_msgs/msg/psm.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace motion_computation
 {
@@ -227,7 +227,8 @@ void convert(
 
   out_msg.predictions = impl::predicted_poses_to_predicted_state(
     predicted_poses, out_msg.velocity.twist.linear.x, rclcpp::Time(out_msg.header.stamp),
-    rclcpp::Duration(pred_step_size * 1e9), map_frame_id, out_msg.confidence, out_msg.confidence);
+    rclcpp::Duration::from_nanoseconds(pred_step_size * 1e9), map_frame_id, out_msg.confidence,
+    out_msg.confidence);
   out_msg.presence_vector |= carma_perception_msgs::msg::ExternalObject::PREDICTION_PRESENCE_VECTOR;
 }
 

motion_computation/test/MotionComputationTest.cpp

@@ -187,7 +187,7 @@ TEST(MotionComputationWorker, ComposePredictedState)
   tf2::Vector3 prev = {4, 0, 0};
 
   auto res = conversion::impl::composePredictedState(
-    curr, prev, time_stamp, time_stamp + rclcpp::Duration(100000000), 0.0);
+    curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000), 0.0);
   auto test_result = std::get<0>(res);
 
   ASSERT_NEAR(test_result.predicted_position.position.x, 4.0, 0.0001);
@@ -202,7 +202,8 @@ TEST(MotionComputationWorker, ComposePredictedState)
   prev = {0, 0, 0};
 
   res = conversion::impl::composePredictedState(
-    curr, prev, time_stamp, time_stamp + rclcpp::Duration(100000000), std::get<1>(res));
+    curr, prev, time_stamp, time_stamp + rclcpp::Duration::from_nanoseconds(100000000),
+    std::get<1>(res));
   test_result = std::get<0>(res);
 
   ASSERT_NEAR(test_result.predicted_position.position.x, 0.0, 0.0001);

platooning_strategic_IHP/CMakeLists.txt

@@ -14,7 +14,7 @@
 # the License.
 
 cmake_minimum_required(VERSION 3.5)
-project(platoon_strategic_ihp)
+project(platooning_strategic_ihp)
 
 # Declare carma package and check ROS version
 find_package(carma_cmake_common REQUIRED)
@@ -26,8 +26,8 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 # Name build targets
-set(node_exec platoon_strategic_ihp_node_exec)
-set(node_lib platoon_strategic_ihp_node)
+set(node_exec platooning_strategic_ihp_node_exec)
+set(node_lib platooning_strategic_ihp_node)
 
 # Includes
 include_directories(
@@ -36,17 +36,17 @@ include_directories(
 
 # Build
 ament_auto_add_library(${node_lib} SHARED
-        src/platoon_manager_ihp.cpp
-        src/platoon_strategic_ihp_node.cpp
-        src/platoon_strategic_ihp.cpp
+        src/platooning_manager_ihp.cpp
+        src/platooning_strategic_ihp_node.cpp
+        src/platooning_strategic_ihp.cpp
 )
 
 ament_auto_add_executable(${node_exec} 
         src/main.cpp 
 )
 
 # Register component
-rclcpp_components_register_nodes(${node_lib} "platoon_strategic_ihp::Node")
+rclcpp_components_register_nodes(${node_lib} "platooning_strategic_ihp::Node")
 
 # All locally created targets will need to be manually linked
 # ament auto will handle linking of external dependencies
@@ -66,16 +66,16 @@ 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_platoon_strategic_ihp 
+  ament_add_gtest(test_platooning_strategic_ihp 
         test/main_test.cpp
         test/mobility_messages.cpp
-        test/test_platoon_manager_front_join.cpp
-        test/test_platoon_manager.cpp
+        test/test_platooning_manager_front_join.cpp
+        test/test_platooning_manager.cpp
   )
 
-  ament_target_dependencies(test_platoon_strategic_ihp ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
+  ament_target_dependencies(test_platooning_strategic_ihp ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
 
-  target_link_libraries(test_platoon_strategic_ihp ${node_lib})
+  target_link_libraries(test_platooning_strategic_ihp ${node_lib})
 
 endif()
 

platooning_strategic_IHP/README.md

@@ -1,4 +1,4 @@
-# platoon_strategic_ihp
+# platooning_strategic_ihp
 
 Strategic plugin for the Integrated Highway Prototype 2 (IHP2) implementation which allows platooning to include front and rear merge joins. Note despite the name, as of Aug 2022 testing for this IHP2 implementation was only up to 35mph on a closed test track per limitations of the CARMA Platform system.
 

platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_config_ihp.h → platooning_strategic_IHP/include/platooning_strategic_ihp/platooning_config_ihp.h

@@ -18,13 +18,13 @@
 
 #include <iostream>
 
-namespace platoon_strategic_ihp
+namespace platooning_strategic_ihp
 {
 
 /**
  * \brief Stuct containing the algorithm configuration values for the yield_pluginConfig
  */
-struct PlatoonPluginConfig
+struct PlatooningPluginConfig
 {
   // following parameters are for general platooning plugin
   double vehicleLength         = 5.0;     // m
@@ -61,7 +61,7 @@ struct PlatoonPluginConfig
   double maxCutinGap             = 22.0;  // m
   double maxCrosstrackError      = 2.0;   // m
 
-  // Speed adjuster to slow down platoon memebr to create gap
+  // Speed adjuster to slow down platoon member to create gap
   double slowDownAdjuster         = 0.75;    // ratio 
   double createGapAdjuster        = 0.3;     // ratio
 
@@ -91,9 +91,9 @@ struct PlatoonPluginConfig
   int join_index = -1;            // target join index for cut-in join - used for testing purposes. -1: front cutin join, target_platoon.size()-1: cut-in rear
 
 
-  friend std::ostream& operator<<(std::ostream& output, const PlatoonPluginConfig& c)
+  friend std::ostream& operator<<(std::ostream& output, const PlatooningPluginConfig& c)
   {
-    output << "PlatoonPluginConfig { " << std::endl
+    output << "PlatooningPluginConfig { " << std::endl
           << "maxPlatoonSize: " << c.maxPlatoonSize << std::endl
           << "algorithmType: " << c.algorithmType << std::endl
           << "statusMessageInterval: " << c.statusMessageInterval << std::endl
@@ -123,4 +123,4 @@ struct PlatoonPluginConfig
   }
 };
 
-} //platoon_strategic_ihp
+} //platooning_strategic_ihp

platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_manager_ihp.h → platooning_strategic_IHP/include/platooning_strategic_ihp/platooning_manager_ihp.h

@@ -35,9 +35,9 @@
 #include <boost/uuid/uuid_generators.hpp>
 #include <boost/uuid/uuid_io.hpp>
 #include <carma_ros2_utils/timers/TimerFactory.hpp>
-#include "platoon_config_ihp.h"
+#include "platooning_config_ihp.h"
 
-namespace platoon_strategic_ihp
+namespace platooning_strategic_ihp
 {
     /**
     * \brief Struct for an action plan, which describes a transient joining activity
@@ -117,7 +117,7 @@ namespace platoon_strategic_ihp
     /**
     * \brief Class containing the logic for platoon manager. It is responsible for keeping track of the platoon members and role of the host vehicle in the platoon
     */ 
-    class PlatoonManager
+    class PlatooningManager
     {
     public:
 
@@ -126,7 +126,7 @@ namespace platoon_strategic_ihp
         * 
         * \param timer_factory An interface which can be used to get access to the current time
         */
-        PlatoonManager(std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory);
+        PlatooningManager(std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory);
 
         /**
          * \brief Stores the latest info on location of the host vehicle.
@@ -414,7 +414,7 @@ namespace platoon_strategic_ihp
     private:
 
         // local copy of configuration file
-        PlatoonPluginConfig config_;
+        PlatooningPluginConfig config_;
 
         std::string OPERATION_INFO_TYPE = "INFO";
         std::string OPERATION_STATUS_TYPE = "STATUS";
@@ -495,4 +495,4 @@ namespace platoon_strategic_ihp
         */
         std::vector<double> getTimeHeadwayFromIndex(std::vector<double> timeHeadways, int start) const;
     };
-}
+}