Merge pull request #1866 from usdot-fhwa-stol/release/cabin

Release/cabin

.circleci/config.yml

@@ -28,7 +28,7 @@ jobs:
     # Pull docker image from docker hub
     # XTERM used for better catkin_make output
     docker:
-      - image: usdotfhwastol/autoware.ai:carma-system-4.1.0
+      - image: usdotfhwastol/autoware.ai:carma-system-4.2.0
         user: carma
         environment:
           TERM: xterm

.sonarqube/sonar-scanner.properties

@@ -72,7 +72,9 @@ sonar.modules= bsm_generator, \
   frame_transformer, \
   object_visualizer, \
   points_map_filter, \
-  light_controlled_intersection_tactical_plugin
+  light_controlled_intersection_tactical_plugin, \
+  platoon_control_ihp, \
+  carma_guidance_plugins
 
 guidance.sonar.projectBaseDir                  = /opt/carma/src/CARMAPlatform/guidance
 bsm_generator.sonar.projectBaseDir             = /opt/carma/src/CARMAPlatform/bsm_generator
@@ -117,7 +119,9 @@ frame_transformer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/frame_tran
 object_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/object_visualizer
 points_map_filter.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/points_map_filter
 light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/light_controlled_intersection_tactical_plugin
+platoon_control_ihp.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_control_ihp
 lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/lci_strategic_plugin
+carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_guidance_plugins
 
 # C++ Package differences
 # Sources
@@ -164,7 +168,9 @@ frame_transformer.sonar.sources = src
 object_visualizer.sonar.sources = src
 points_map_filter.sonar.sources = src
 light_controlled_intersection_tactical_plugin.sonar.sources = src
+platoon_control_ihp.sonar.sources = src
 lci_strategic_plugin.sonar.sources = src
+carma_guidance_plugins.sonar.sources = src
 
 # Tests
 # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated.
@@ -201,11 +207,17 @@ basic_autonomy.sonar.tests = test
 mobilitypath_visualizer.sonar.tests = test
 wz_strategic_plugin.sonar.tests = test
 sci_strategic_plugin.sonar.tests = test
+platoon_control_ihp.sonar.tests = test
+
+
 stop_controlled_intersection_tactical_plugin.sonar.tests = test
 system_controller.sonar.sources = test
 subsystem_controllers.sonar.sources = test
 frame_transformer.sonar.sources = test
 object_visualizer.sonar.sources = test
 points_map_filter.sonar.sources = test
 light_controlled_intersection_tactical_plugin.sonar.tests = test
+platoon_control_ihp_plugin.sonar.tests = test
 lci_strategic_plugin.sonar.sources = src
+lci_strategic_plugin.sonar.sources = test
+carma_guidance_plugins.sonar.sources = test

Dockerfile

@@ -33,7 +33,7 @@
 # Stage 1 - Acquire the CARMA source as well as any extra packages
 # /////////////////////////////////////////////////////////////////////////////
 
-FROM usdotfhwastol/autoware.ai:carma-system-4.1.0 AS base-image
+FROM usdotfhwastol/autoware.ai:carma-system-4.2.0 AS base-image
 
 FROM base-image AS source-code
 
@@ -47,6 +47,10 @@ RUN ~/src/carma-platform/docker/checkout.bash
 
 
 FROM base-image AS install
+ARG ROS1_PACKAGES=""
+ENV ROS1_PACKAGES=${ROS1_PACKAGES}
+ARG ROS2_PACKAGES=""
+ENV ROS2_PACKAGES=${ROS2_PACKAGES}
 
 # Copy the source files from the previous stage and build/install
 RUN mkdir ~/carma_ws

arbitrator/config/arbitrator_params.yaml

@@ -26,4 +26,4 @@ use_fixed_costs: true
 # Map: The priorities associated with each plugin during the planning 
 # process, values will be normalized at runtime and inverted into costs
 # Unit: N/a
-plugin_priorities: {LCIStrategicPlugin: 5.0, SCIStrategicPlugin: 4.0, RouteFollowingPlugin: 2.0, WorkZonePlugin: 4.0, PlatooningStrategicPlugin: 1.0}
+plugin_priorities: {PlatooningStrategicIHPPlugin: 4.0, LCIStrategicPlugin: 3.0, SCIStrategicPlugin: 2.0, PlatooningStrategicPlugin: 2.0, RouteFollowingPlugin: 1.0}

basic_autonomy/include/basic_autonomy/basic_autonomy.h

@@ -83,6 +83,7 @@ namespace basic_autonomy
                                                           // computed curvature and output speeds
             double back_distance = 20;                    // Number of meters behind the first maneuver that need to be included in points for curvature calculation
             double buffer_ending_downtrack = 20.0;        //The additional downtrack beyond requested end dist used to fit points along spline
+            std::string desired_controller_plugin = "default";  //The desired controller plugin for the generated trajectory
         };
 
 
@@ -145,12 +146,13 @@ namespace basic_autonomy
    * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s
    * \param yaws The orientation the vehicle should achieve at each point. Units radians
    * \param startTime The absolute start time which will be used to update the input relative times. Units s
+   * \param desired_controller_plugin The name of the controller plugin for the generated trajectory.
    * 
    * \return A list of trajectory points built from the provided inputs.
    */
         std::vector<cav_msgs::TrajectoryPlanPoint> trajectory_from_points_times_orientations(
             const std::vector<lanelet::BasicPoint2d> &points, const std::vector<double> &times,
-            const std::vector<double> &yaws, ros::Time startTime);
+            const std::vector<double> &yaws, ros::Time startTime, const std::string &desired_controller_plugin);
 
         /**
    * \brief Attaches back_distance length of points behind the future points
@@ -337,7 +339,8 @@ namespace basic_autonomy
                                                             int speed_moving_average_window_size,
                                                             int curvature_moving_average_window_size,
                                                             double back_distance,
-                                                            double buffer_ending_downtrack);
+                                                            double buffer_ending_downtrack,
+                                                            std::string desired_controller_plugin = "default");
 
     GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type,
                                                         int default_downsample_ratio,

basic_autonomy/src/basic_autonomy.cpp

@@ -75,9 +75,35 @@ namespace basic_autonomy
             std::vector<PointSpeedPair> points_and_target_speeds;
 
             cav_msgs::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver;
-            
+
             auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack, true, true);
 
+            //TODO: This fix is temporary and only applies on lane follow maneuvers from Platooning Strategic Plugin IHP. 
+            // A general fix will be implemented soon. (issue #1863)
+            bool lanelets_defined = !maneuver.lane_following_maneuver.lane_ids.empty();
+            ROS_DEBUG_STREAM("lanelets_defined: " << lanelets_defined);
+            bool isFromPlatooning = maneuver.lane_following_maneuver.parameters.planning_strategic_plugin == "PlatooningStrategicIHPPlugin";
+            ROS_DEBUG_STREAM("isFromPlatooning: " << isFromPlatooning);
+
+            if (lanelets_defined && isFromPlatooning)
+            {
+                lanelets = {};
+                int lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[0]);
+                ROS_DEBUG_STREAM("extracted id: " << lane_id);
+                lanelet::ConstLanelet new_lanelet = wm->getMap()->laneletLayer.get(lane_id);
+                lanelets.push_back(new_lanelet);
+
+                if (maneuver.lane_following_maneuver.lane_ids.size()>1)
+                {
+                    lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[1]);
+                    ROS_DEBUG_STREAM("more extracted id: " << lane_id);
+                    new_lanelet = wm->getMap()->laneletLayer.get(lane_id);
+                    lanelets.push_back(new_lanelet);
+                }
+            } 
+
+
+
             if (lanelets.empty())
             {
                 ROS_ERROR_STREAM("Detected no lanelets between starting downtrack: "<< starting_downtrack << ", and lane_following_maneuver.end_dist: "<< lane_following_maneuver.end_dist);
@@ -594,6 +620,7 @@ namespace basic_autonomy
             size_t time_boundary_exclusive_index =
                 trajectory_utils::time_boundary_index(downtracks, speeds, time_span);
 
+			ROS_DEBUG_STREAM("time_boundary_exclusive_index = " << time_boundary_exclusive_index);
             if (time_boundary_exclusive_index == 0)
             {
                 throw std::invalid_argument("No points to fit in timespan");
@@ -701,7 +728,7 @@ namespace basic_autonomy
 
         std::vector<cav_msgs::TrajectoryPlanPoint> trajectory_from_points_times_orientations(
             const std::vector<lanelet::BasicPoint2d> &points, const std::vector<double> &times, const std::vector<double> &yaws,
-            ros::Time startTime)
+            ros::Time startTime, const std::string &desired_controller_plugin)
         {
             if (points.size() != times.size() || points.size() != yaws.size())
             {
@@ -720,7 +747,7 @@ namespace basic_autonomy
                 tpp.y = points[i].y();
                 tpp.yaw = yaws[i];
 
-                tpp.controller_plugin_name = "default";
+                tpp.controller_plugin_name = desired_controller_plugin;
                 //tpp.planner_plugin_name        //Planner plugin name is filled in the tactical plugin
 
                 traj.push_back(tpp);
@@ -795,6 +822,7 @@ namespace basic_autonomy
             ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index);
 
             std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position
+			ROS_DEBUG_STREAM("Ready to call constrain_to_time_boundary: future_points size = " << future_points.size() << ", trajectory_time_length = " << detailed_config.trajectory_time_length);
             auto time_bound_points = constrain_to_time_boundary(future_points, detailed_config.trajectory_time_length);
 
             ROS_DEBUG_STREAM("Got time_bound_points with size:" << time_bound_points.size());
@@ -914,7 +942,7 @@ namespace basic_autonomy
                     std::vector<double> yaw = {state.orientation, state.orientation}; //Keep current orientation
 
                     std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
-                    trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time);
+                    trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time, detailed_config.desired_controller_plugin);
 
                     return traj_points;
 
@@ -971,7 +999,7 @@ namespace basic_autonomy
 
             // Build trajectory points
             std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
-                trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time);
+                trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin);
 
             //debug msg
             carma_debug_msgs::TrajectoryCurvatureSpeeds msg;
@@ -1002,7 +1030,8 @@ namespace basic_autonomy
                                                               int speed_moving_average_window_size,
                                                               int curvature_moving_average_window_size,
                                                               double back_distance,
-                                                              double buffer_ending_downtrack)
+                                                              double buffer_ending_downtrack,
+                                                              std::string desired_controller_plugin)
         {
             DetailedTrajConfig detailed_config;
 
@@ -1015,6 +1044,7 @@ namespace basic_autonomy
             detailed_config.curvature_moving_average_window_size = curvature_moving_average_window_size;
             detailed_config.back_distance = back_distance;
             detailed_config.buffer_ending_downtrack = buffer_ending_downtrack;
+            detailed_config.desired_controller_plugin = desired_controller_plugin;
 
             return detailed_config;
         }
@@ -1101,7 +1131,7 @@ namespace basic_autonomy
             ROS_DEBUG_STREAM("After removing extra buffer points, future_geom_points.size():"<< future_geom_points.size());
 
             std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
-                trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time);
+                trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin);
 
             return traj_points;
         }

basic_autonomy/test/test_waypoint_generation.cpp

@@ -83,7 +83,7 @@ namespace basic_autonomy
         std::vector<double> yaws = {0.2, 0.5, 0.6, 1.0};
         ros::Time startTime(1.0);
         std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
-            basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime);
+            basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime, "default");
 
         ASSERT_EQ(4, traj_points.size());
         ASSERT_NEAR(1.0, traj_points[0].target_time.toSec(), 0.0000001);

carma/launch/carma_src.launch.py

@@ -126,6 +126,8 @@ def generate_launch_description():
                 PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/environment.launch.py']),
                 launch_arguments = { 
                     'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'],
+                    'vehicle_config_param_file' : vehicle_config_param_file,
+                    'vehicle_characteristics_param_file' : vehicle_characteristics_param_file
                     }.items()
             ),
         ]
@@ -166,6 +168,7 @@ def generate_launch_description():
                 launch_arguments={
                     'route_file_folder' : route_file_folder,
                     'vehicle_characteristics_param_file' : vehicle_characteristics_param_file, 
+                    'vehicle_config_param_file' : vehicle_config_param_file,
                     'enable_guidance_plugin_validator' : enable_guidance_plugin_validator,
                     'strategic_plugins_to_validate' : strategic_plugins_to_validate,
                     'tactical_plugins_to_validate' : tactical_plugins_to_validate,

carma/launch/environment.launch

@@ -42,18 +42,4 @@
     <remap from="/lanelet_map_bin" to="semantic_map"/>
   </node>
 
-  <!-- World Model Updater -->
-  <group>
-    <remap from="outgoing_geofence_request" to="$(optenv CARMA_MSG_NS)/outgoing_geofence_request"/>
-    <include file="$(find carma_wm_ctrl)/launch/carma_wm_broadcaster.launch"/>
-  </group>
-
-  <!-- Traffic Incident Parser -->
-  <group>
-    <remap from="georeference" to="$(optenv CARMA_LOCZ_NS)/map_param_loader/georeference"/>
-    <remap from="geofence" to="$(optenv CARMA_MSG_NS)/incoming_geofence_control"/>
-    <remap from="incoming_mobility_operation" to="$(optenv CARMA_MSG_NS)/incoming_mobility_operation"/>
-    <include file="$(find traffic_incident_parser)/launch/traffic_incident_parser.launch"/>
-  </group>
-
 </launch>