Merge pull request #1483 from usdot-fhwa-stol/hotfix/3.8.1

merge Hotfix/3.8.1 branch to master

basic_autonomy/include/basic_autonomy/basic_autonomy.h

@@ -213,7 +213,7 @@ namespace basic_autonomy
      */
           std::vector<PointSpeedPair> create_lanefollow_geometry(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack,
                                                                    const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
-                                                                   const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config);
+                                                                   const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set<lanelet::Id>& visited_lanelets);
 
      /**
       * \brief Adds extra centerline points beyond required message length to lane follow maneuver points so that there's always enough points to calculate trajectory

basic_autonomy/src/basic_autonomy.cpp

@@ -27,10 +27,13 @@ namespace basic_autonomy
             std::vector<PointSpeedPair> points_and_target_speeds;
 
             bool first = true;
+            std::unordered_set<lanelet::Id> visited_lanelets;
+
             ROS_DEBUG_STREAM("VehDowntrack:"<<max_starting_downtrack);
             for(const auto &maneuver : maneuvers)
             {
                 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
+
                 if(first){
                     starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
                     first = false;
@@ -39,12 +42,12 @@ namespace basic_autonomy
 
                 if(maneuver.type == cav_msgs::Maneuver::LANE_FOLLOWING){
                     ROS_DEBUG_STREAM("Creating Lane Follow Geometry");
-                    std::vector<PointSpeedPair> lane_follow_points = create_lanefollow_geometry(maneuver, max_starting_downtrack, wm, ending_state_before_buffer, general_config, detailed_config);
+                    std::vector<PointSpeedPair> lane_follow_points = create_lanefollow_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, general_config, detailed_config, visited_lanelets);
                     points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end());
                 }
                 else if(maneuver.type == cav_msgs::Maneuver::LANE_CHANGE){
                     ROS_DEBUG_STREAM("Creating Lane Change Geometry");
-                    std::vector<PointSpeedPair> lane_change_points = create_lanechange_geometry(maneuver, max_starting_downtrack, wm, ending_state_before_buffer, state, detailed_config);
+                    std::vector<PointSpeedPair> lane_change_points = create_lanechange_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, state, detailed_config);
                     points_and_target_speeds.insert(points_and_target_speeds.end(), lane_change_points.begin(), lane_change_points.end());
                 }
                 else{
@@ -63,14 +66,13 @@ namespace basic_autonomy
         }
 
         std::vector<PointSpeedPair> create_lanefollow_geometry(const cav_msgs::Maneuver &maneuver, double starting_downtrack,
-                                                                   const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
-                                                                    const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
+                                                                const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
+                                                                const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set<lanelet::Id> &visited_lanelets)
         {
             if(maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING){
                 throw std::invalid_argument("Create_lanefollow called on a maneuver type which is not LANE_FOLLOW");
             }
             std::vector<PointSpeedPair> points_and_target_speeds;
-            std::unordered_set<lanelet::Id> visited_lanelets;
 
             cav_msgs::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver;
 

basic_autonomy/test/test_waypoint_generation.cpp

@@ -671,6 +671,47 @@ namespace basic_autonomy
         ASSERT_NEAR(lc_start_point.x(), lc_geom.front().x(), 0.000001);
     } 
 
+    TEST(BasicAutonomyTest, lanefollow_geometry_visited_lanelets)
+    {
+
+        double starting_downtrack = 0;
+        cav_msgs::VehicleState ending_state;
+        std::string trajectory_type = "lane_follow";
+        waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 0, 0);
+        waypoint_generation::DetailedTrajConfig detailed_config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20);
+
+        lanelet::Id id1 = 1100;
+        std::unordered_set<lanelet::Id> visited_lanelets;
+        visited_lanelets.insert(id1);
+        std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
+        auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10);
+        wm->setMap(map);
+        carma_wm::test::setSpeedLimit(15_mph, wm);
+
+        carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm);
+
+        cav_msgs::Maneuver maneuver;
+        maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
+        maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200));
+        maneuver.lane_following_maneuver.start_dist = 5.0;
+        maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
+        maneuver.lane_following_maneuver.start_speed = 0.0;
+
+        maneuver.lane_following_maneuver.end_dist = 14.98835712;
+        maneuver.lane_following_maneuver.end_speed = 6.7056;
+        maneuver.lane_following_maneuver.end_time = ros::Time(4.4704);
+
+
+
+        std::vector<basic_autonomy::waypoint_generation::PointSpeedPair> points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, 
+                                                                                    starting_downtrack, wm, ending_state, general_config, detailed_config, visited_lanelets);
+
+        EXPECT_EQ(visited_lanelets.size(), 5);
+
+        EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set
+        EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver
+    }
+
 } //basic_autonomy namespace
 
 // Run all the tests

inlanecruising_plugin/CMakeLists.txt

@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2018-2020 LEIDOS.
+# Copyright (C) 2018-2021 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
@@ -31,6 +31,7 @@ set( CATKIN_DEPS
   trajectory_utils
   autoware_msgs
   carma_wm
+  basic_autonomy
   tf
   tf2
   tf2_geometry_msgs
@@ -72,13 +73,13 @@ add_executable( ${PROJECT_NAME}
   src/main.cpp)
 add_library(inlanecruising_plugin_library 
   src/inlanecruising_plugin.cpp
-  src/smoothing/BSpline.cpp
 )
-target_link_libraries(inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 add_dependencies(inlanecruising_plugin_library ${catkin_EXPORTED_TARGETS})
+target_link_libraries(inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 add_library(helper src/helper.cpp)
 target_link_libraries(helper
@@ -118,7 +119,6 @@ if(CATKIN_ENABLE_TESTING)
 
   catkin_add_gmock(${PROJECT_NAME}-test 
     test/test_inlanecruising_plugin_planning.cpp
-    test/test_inlanecruising_plugin.cpp
   )
   target_link_libraries(${PROJECT_NAME}-test inlanecruising_plugin_library ${catkin_LIBRARIES})
 

inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.h

@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2020 LEIDOS.
+ * Copyright (C) 2020-2021 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

inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h

@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-2021 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
@@ -24,16 +24,16 @@
 #include <carma_utils/CARMAUtils.h>
 #include <boost/geometry.hpp>
 #include <carma_wm/Geometry.h>
+#include <basic_autonomy/basic_autonomy.h>
 #include <cav_srvs/PlanTrajectory.h>
 #include <carma_wm/WMListener.h>
+#include <carma_debug_msgs/TrajectoryCurvatureSpeeds.h>
 #include <functional>
-#include <inlanecruising_plugin/smoothing/SplineI.h>
 #include "inlanecruising_config.h"
 #include <unordered_set>
 #include <autoware_msgs/Lane.h>
 #include <ros/ros.h>
 #include <carma_debug_msgs/TrajectoryCurvatureSpeeds.h>
-#include <basic_autonomy/helper_functions.h>
 
 namespace inlanecruising_plugin
 {
@@ -76,135 +76,6 @@ class InLaneCruisingPlugin
    */ 
   bool onSpin();
 
-  /**
-   * \brief Applies the provided speed limits to the provided speeds such that each element is capped at its corresponding speed limit if needed
-   * 
-   * \param speeds The speeds to limit
-   * \param speed_limits The speed limits to apply. Must have the same size as speeds
-   * 
-   * \return The capped speed limits. Has the same size as speeds
-   */ 
-  std::vector<double> apply_speed_limits(const std::vector<double> speeds, const std::vector<double> speed_limits);
-
-  /**
-   * \brief Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis
-   * 
-   * \param p1 The origin point for the frame in the parent frame
-   * \param p2 A point in the parent frame that will define the +X axis relative to p1
-   * 
-   * \return A 2D coordinate frame transform
-   */ 
-  Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d& p1, const lanelet::BasicPoint2d& p2);
-
-  /**
-   * \brief Reduces the input points to only those points that fit within the provided time boundary
-   * 
-   * \param points The input point speed pairs to reduce
-   * \param time_span The time span in seconds which the output points will fit within
-   * 
-   * \return The subset of points that fit within time_span
-   */ 
-  std::vector<PointSpeedPair> constrain_to_time_boundary(const std::vector<PointSpeedPair>& points, double time_span);
-
-  /**
-   * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning
-   * 
-   * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. 
-   *               These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. 
-   * \param state The current state of the vehicle
-   * \param state_time The abosolute time which the provided vehicle state corresponds to
-   * 
-   * \return A list of trajectory points to send to the carma planning stack
-   */ 
-  std::vector<cav_msgs::TrajectoryPlanPoint>
-  compose_trajectory_from_centerline(const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state, const ros::Time& state_time);
-
-  /**
-   * \brief Method combines input points, times, orientations, and an absolute start time to form a valid carma platform trajectory
-   * 
-   * NOTE: All input vectors must be the same size. The output vector will take this size.
-   * 
-   * \param points The points in the map frame that the trajectory will follow. Units m
-   * \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
-   * 
-   * \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);
-
-  /**
-   * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. 
-   * 
-   * \param maneuvers The list of maneuvers to convert
-   * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier.
-   *                               If the first maneuver exceeds this then it's downtrack will be shifted to this value.
-   * 
-   * \param wm Pointer to intialized world model for semantic map access
-   * 
-   * \return List of centerline points paired with speed limits
-   */ 
-  std::vector<PointSpeedPair> maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
-                                                  double max_starting_downtrack,
-                                                  const carma_wm::WorldModelConstPtr& wm);
-
-  /**
-   * \brief Computes a spline based on the provided points
-   * 
-   * \param basic_points The points to use for fitting the spline
-   * 
-   * \return A spline which has been fit to the provided points
-   */ 
-  std::unique_ptr<smoothing::SplineI> compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points);
-
-  /**
-   * \brief Returns the speeds of points closest to the lookahead distance.
-   * 
-   * \param points The points in the map frame that the trajectory will follow. Units m
-   * \param speeds Speeds assigned to points that trajectory will follow. Unit m/s
-   * \param lookahead The lookahead distance to obtain future points' speed. Unit m
-   * 
-   * \return A vector of speed values shifted by the lookahead distance.
-   */ 
-
-  std::vector<double> get_lookahead_speed(const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& speeds, const double& lookahead);
-
-  /**
-   * \brief Applies the longitudinal acceleration limit to each point's speed
-   * 
-   * \param downtracks downtrack distances corresponding to each speed
-   * \param curv_speeds vehicle velocity in m/s.
-   * \param accel_limit vehicle longitudinal acceleration in m/s^2.
-   * 
-   * \return optimized speeds for each dowtrack points that satisfies longitudinal acceleration
-   */ 
-  std::vector<double> optimize_speed(const std::vector<double>& downtracks, const std::vector<double>& curv_speeds, double accel_limit);
-
-  /**
-   * \brief Given the curvature fit, computes the curvature at the given step along the curve
-   * 
-   * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature
-   * 
-   * \param fit_curve curvature fit
-   * 
-   * \return Curvature (k = 1/r, 1/meter)
-   */ 
-  double compute_curvature_at(const inlanecruising_plugin::smoothing::SplineI& fit_curve, double step_along_the_curve) const;
-
-    /**
-   * \brief Attaches back_distance length of points in front of future points
-   * 
-   * \param points all point speed pairs
-   * \param nearest_pt_index idx of nearest point to the vehicle
-   * \param future_points future points before which to attach the points
-   * \param back_distance number of back distance in meters
-   * 
-   * \return point speed pairs with back distance length of points in front of future points
-   */ 
-  std::vector<PointSpeedPair> attach_back_points(const std::vector<PointSpeedPair>& points, const int nearest_pt_index, 
-                               std::vector<inlanecruising_plugin::PointSpeedPair> future_points, double back_distance) const;
   /**
    * \brief set the yield service
    * 
@@ -220,20 +91,11 @@ class InLaneCruisingPlugin
    * \return true or falss
    */
   bool validate_yield_plan(const cav_msgs::TrajectoryPlan& yield_plan);
+
+  cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later
 
 private:
 
-  /**
-   * \brief Returns the min, and its idx, from the vector of values, excluding given set of values
-   * 
-   * \param values vector of values
-   * 
-   * \param excluded set of excluded values
-   * 
-   * \return minimum value and its idx
-   */ 
-  std::pair<double, size_t> min_with_exclusions(const std::vector<double>& values, const std::unordered_set<size_t>& excluded) const;
-
   carma_wm::WorldModelConstPtr wm_;
   InLaneCruisingPluginConfig config_;
   PublishPluginDiscoveryCB plugin_discovery_publisher_;
@@ -242,7 +104,6 @@ class InLaneCruisingPlugin
   cav_msgs::Plugin plugin_discovery_msg_;
   DebugPublisher debug_publisher_;
   carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_;
-  cav_msgs::VehicleState ending_state_before_buffer; //state before applying extra points for curvature calculation that are removed later
 
 };
 

inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h

@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-2021 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
@@ -63,6 +63,7 @@ class InLaneCruisingPluginNode
                      config.speed_moving_average_window_size);
     pnh.param<int>("curvature_moving_average_window_size", config.curvature_moving_average_window_size,
                      config.curvature_moving_average_window_size);
+    pnh.param<double>("buffer_ending_downtrack", config.buffer_ending_downtrack, config.buffer_ending_downtrack);
     pnh.param<double>("/vehicle_acceleration_limit", config.max_accel, config.max_accel);
     pnh.param<double>("/vehicle_lateral_accel_limit", config.lateral_accel_limit, config.lateral_accel_limit);
     pnh.param<bool>("enable_object_avoidance", config.enable_object_avoidance, config.enable_object_avoidance);

inlanecruising_plugin/launch/inlanecruising_plugin.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright (C) 2018-2020 LEIDOS.
+  Copyright (C) 2018-2021 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

inlanecruising_plugin/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <!--  
- Copyright (C) 2018-2020 LEIDOS.
+ Copyright (C) 2018-2021 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
@@ -31,10 +31,10 @@
   <depend>carma_utils</depend>
   <depend>autoware_msgs</depend>
   <depend>carma_wm</depend>
+  <depend>basic_autonomy</depend>
   <depend>tf</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend> 
   <depend>trajectory_utils</depend>   
-  <depend>carma_debug_msgs</depend> 
-  <depend>basic_autonomy</depend>   
+  <depend>carma_debug_msgs</depend>  
 </package>