Merge pull request #1599 from usdot-fhwa-stol/release/fiesta

Release/fiesta

.circleci/config.yml

@@ -28,12 +28,12 @@ jobs:
     # Pull docker image from docker hub
     # XTERM used for better catkin_make output
     docker:
-      - image: usdotfhwastol/autoware.ai:carma-system-3.10.0
+      - image: usdotfhwastol/autoware.ai:carma-system-3.11.0
         user: carma
         environment:
           TERM: xterm
           INIT_ENV: /home/carma/.base-image/init-env.sh
-    resource_class: large
+    resource_class: large      
     # Set working directory
     working_directory: "/opt/carma/"
     # Pull code and execute tests

.sonarqube/sonar-scanner.properties

@@ -66,7 +66,8 @@ sonar.modules= bsm_generator, \
   mobilitypath_visualizer, \
   wz_strategic_plugin, \
   sci_strategic_plugin, \
-  stop_controlled_intersection_tactical_plugin
+  stop_controlled_intersection_tactical_plugin, \
+  light_controlled_intersection_tactical_plugin
 
 guidance.sonar.projectBaseDir                  = /opt/carma/src/CARMAPlatform/guidance
 bsm_generator.sonar.projectBaseDir             = /opt/carma/src/CARMAPlatform/bsm_generator
@@ -106,6 +107,7 @@ mobilitypath_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mobi
 wz_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/wz_strategic_plugin
 sci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/sci_strategic_plugin
 stop_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/stop_controlled_intersection_tactical_plugin
+light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/light_controlled_intersection_tactical_plugin
 
 # C++ Package differences
 # Sources
@@ -147,6 +149,7 @@ mobilitypath_visualizer.sonar.sources = src
 wz_strategic_plugin.sonar.sources = src
 sci_strategic_plugin.sonar.sources = src
 stop_controlled_intersection_tactical_plugin.sonar.sources = src
+light_controlled_intersection_tactical_plugin.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.
@@ -184,3 +187,4 @@ mobilitypath_visualizer.sonar.tests = test
 wz_strategic_plugin.sonar.tests = test
 sci_strategic_plugin.sonar.tests = test
 stop_controlled_intersection_tactical_plugin.sonar.tests = test
+light_controlled_intersection_tactical_plugin.sonar.tests = test

Dockerfile

@@ -33,7 +33,7 @@
 # Stage 1 - Acquire the CARMA source as well as any extra packages
 # /////////////////////////////////////////////////////////////////////////////
 
-FROM usdotfhwastol/autoware.ai:carma-system-3.10.0 AS base-image
+FROM usdotfhwastol/autoware.ai:carma-system-3.11.0 AS base-image
 
 FROM base-image AS source-code
 

README.md

@@ -71,17 +71,22 @@ For administrative information on CARMA2, including vehicle and developer PC con
 ## Other CARMA Packages
 CARMA Platform<sup>SM</sup> is a downloadable, open source software (OSS) platform architected to be extensible and reusable for a wide variety of research purposes to advance innovation for cooperative driving automation. It enables communication between vehicles, road users such as pedestrians, bicyclists, and scooters, and infrastructure devices capable of communication. It promotes collaboration between a community of engineers and researchers to accelerate the development, testing, and evaluation of cooperative driving automation while advancing the safety, security, data, and use of artificial intelligence in automated driving technology.  
 
-The CARMA Platform is distributed as a set of multiple independent packages hosted in separate Github repositories. These packages facilitate operation of the CARMA Platform with different hardware configurations or allow it to support different modes of operation. To include one of these packages in your build of the CARMA Platform system please clone the Github repository into the same Catkin workspace `src/` folder as this repository. The Catkin build system will verify that dependencies are resolved appropriately and build the newly included package when you next run `catkin_make`. An incomplete listing of available packages for CARMA2 includes:
+The CARMA Platform is distributed as a set of multiple independent packages hosted in separate Github repositories. These packages facilitate operation of the CARMA Platform with different hardware configurations or allow it to support different modes of operation. To include one of these packages in your build of the CARMA Platform system please clone the Github repository into the same Catkin workspace `src/` folder as this repository. The Catkin build system will verify that dependencies are resolved appropriately and build the newly included package when you next run `catkin_make`. An incomplete listing of available packages for CARMA3 includes:
 
 ### Vehicle Controller Interface Drivers
 * [carma-ssc-interface-wrapper](https://github.com/usdot-fhwa-stol/carma-ssc-interface-wrapper)
+* [dataspeed_controller_driver](https://github.com/VT-ASIM-LAB/dataspeed_controller_driver) (Externally managed)
+* [dataspeed_can_driver](https://github.com/VT-ASIM-LAB/dataspeed_can_driver) (Externally managed)
 
 ### Sensor Drivers
 * [carma-cohda-dsrc-driver](https://github.com/usdot-fhwa-stol/carma-cohda-dsrc-driver)
 * [carma-velodyne-lidar-driver](https://github.com/usdot-fhwa-stol/carma-velodyne-lidar-driver)
 * [avt_vimba_camera](https://github.com/usdot-fhwa-stol/avt_vimba_camera)
 * [carma-delphi-srr2-driver](https://github.com/usdot-fhwa-stol/carma-delphi-srr2-driver)
 * [novatel_gps_driver](https://github.com/usdot-fhwa-stol/novatel_gps_driver)
+* [gstreamer_camera_driver](https://github.com/VT-ASIM-LAB/gstreamer_camera_driver) (Externally managed)
+* [ouster_lidar_driver](https://github.com/VT-ASIM-LAB/ouster_lidar_driver) (Externally managed)
+* [inertiallabs_gnss_driver](https://github.com/VT-ASIM-LAB/inertiallabs_gnss_driver) (Externally managed)
 
 ### General System Utilites
 * [carma-web-ui](https://github.com/usdot-fhwa-stol/carma-web-ui)

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: {RouteFollowingPlugin: 4.0, WorkZonePlugin: 8.0, PlatooningStrategicPlugin: 1.0}
+plugin_priorities: {SCIStrategicPlugin: 4.0, RouteFollowingPlugin: 2.0, WorkZonePlugin: 4.0, PlatooningStrategicPlugin: 1.0}

arbitrator/include/capabilities_interface.tpp

@@ -33,6 +33,7 @@ namespace arbitrator
         for (auto i = topics.begin(); i != topics.end(); i++) 
         {
             ros::ServiceClient sc = nh_->serviceClient<cav_srvs::PlanManeuvers>(*i);
+            ROS_DEBUG_STREAM("found client: " << *i);
             if (sc.call(msg)) {
                 responses.emplace(*i, msg);
             }

arbitrator/src/capabilities_interface.cpp

@@ -43,6 +43,11 @@ namespace arbitrator
             stream << std::endl;
             ROS_INFO(stream.str().c_str());
         }
+        else
+        {
+            ROS_DEBUG_STREAM("servicd call failed...");
+            ROS_DEBUG_STREAM("client: " << sc_s);
+        }
 
         return topics;
 

basic_autonomy/include/basic_autonomy/basic_autonomy.h

@@ -244,8 +244,8 @@ namespace basic_autonomy
                                                       const cav_msgs::VehicleState &ending_state_before_buffer, carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg,
                                                       const DetailedTrajConfig &detailed_config);
 
-        //Functions specific to lane change
-   /**
+     //Functions specific to lane change
+     /**
       * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. 
       * 
       * \param maneuvers The list of maneuvers to convert
@@ -260,9 +260,36 @@ namespace basic_autonomy
       * 
       * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver
       */
-     std::vector<PointSpeedPair> create_lanechange_geometry(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack,
+     std::vector<PointSpeedPair> get_lanechange_points_from_maneuver(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack,
                                                                    const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
-                                                                   const cav_msgs::VehicleState &state, const DetailedTrajConfig &detailed_config);
+                                                                   const cav_msgs::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config);
+
+     /**
+      * \brief Creates a vector of lane change points using parameters defined. 
+      * 
+      * \param starting_lane_id lanelet id for where lane change plan should start
+      * \param ending_lane_id lanelet id for where lane change plan should end
+      * \param starting_downtrack The downtrack distance from which the lane change maneuver starts
+      * \param ending_downtrack The downtrack distance at which the lane change maneuver end
+      * \param wm Pointer to intialized world model for semantic map access
+      * \param state The vehicle state at the time the function is called
+      * 
+      * \return A vector of geometry points as lanelet::basicpoint2d
+      */
+     std::vector<lanelet::BasicPoint2d> create_lanechange_geometry(lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double starting_downtrack, double ending_downtrack,
+                                                            const carma_wm::WorldModelConstPtr &wm,const cav_msgs::VehicleState &state, int downsample_ratio);
+
+
+     /**
+      * \brief Resamples a pair of basicpoint2d lines to get lines of same number of points. 
+      * 
+      * \param line_1 a vector of points to be resampled
+      * \param line_2 a vector of points to be resampled
+      * 
+      * \return A 2d vector with input lines resampled at same rate. The first iteration is the resampled line_1 and the resampled line_2 is the second iteration
+      * Assumption here is for lane change to happen between two adjacent lanelets, they must share a lane boundary (linestring)
+      */
+     std::vector<std::vector<lanelet::BasicPoint2d>> resample_linestring_pair_to_same_size(std::vector<lanelet::BasicPoint2d>& line_1, std::vector<lanelet::BasicPoint2d>& line_2);
 
         /**
    * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning for a Lane following maneuver.

basic_autonomy/include/basic_autonomy/helper_functions.h

@@ -95,8 +95,7 @@ namespace waypoint_generation
      * \return index of nearest point in points
      */
     int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm,
-                                      const cav_msgs::VehicleState& state);                                                                                                                               
-
+                                      const cav_msgs::VehicleState& state);    
 
 }
 }