Sync develop to master (#2371)

.github/workflows/dockerhub.yml

@@ -3,10 +3,11 @@ name: Docker Hub build
 on:
  push:
  branches:
- - "develop"
- - "master"
+ - develop
+ - master
  - "release/*"
-
+ tags:
+ - "carma-system-*"
 jobs:
  dockerhub:
  uses: usdot-fhwa-stol/actions/.github/workflows/dockerhub.yml@main

Dockerfile

@@ -32,14 +32,16 @@
 # /////////////////////////////////////////////////////////////////////////////
 # Stage 1 - Acquire the CARMA source as well as any extra packages
 # /////////////////////////////////////////////////////////////////////////////
-
-FROM usdotfhwastoldev/autoware.ai:develop AS base-image
+ARG DOCKER_ORG="usdotfhwastoldev"
+ARG DOCKER_TAG="develop"
+FROM ${DOCKER_ORG}/autoware.ai:${DOCKER_TAG} as base-image
+ARG GIT_BRANCH="develop" 
 
 FROM base-image AS source-code
 
 RUN mkdir ~/src
 COPY --chown=carma . /home/carma/src/carma-platform/
-RUN ~/src/carma-platform/docker/checkout.bash
+RUN ~/src/carma-platform/docker/checkout.bash -b ${GIT_BRANCH}
 
 # /////////////////////////////////////////////////////////////////////////////
 # Stage 2 - Build and install the software 

basic_autonomy/include/basic_autonomy/helper_functions.hpp

@@ -53,7 +53,7 @@ namespace waypoint_generation
  * \param points BasicLineString2d points
  * \param target_downtrack target downtrack along the route to get index near to
  * 
- * \return index of nearest point in points
+ * \return index of nearest point in points with a downtrack less than 'target_downtrack', or -1 if the received 'points' vector is empty.
  */
  int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm, double target_downtrack);
 

basic_autonomy/src/basic_autonomy.cpp

@@ -252,7 +252,7 @@ namespace basic_autonomy
  }
 
  double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
-
+ 
  dist_accumulator += delta_d;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() <<
  "current_point.y():" << current_point.y());
@@ -276,8 +276,19 @@ namespace basic_autonomy
  {
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Extending trajectory using buffer beyond end of target lanelet");
+ int j = i - 1;
+ while (delta_d < epsilon_ && j >= 0 && !delta_point)
+ {
+ RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Looking at index j: " << j << ", where i: " << i);
+ prev_point = points_and_target_speeds.at(j).point;
+ j--;
+ delta_d = lanelet::geometry::distance2d(prev_point, current_point);
+ }
 
- if (!delta_point) { // Set the step size based on last two points
+ if (j < 0 && delta_d < epsilon_) //a very rare case where only duplicate points exist in the entire trajectory, so it wasn't possible to extend
+ break;
+
+ if (!delta_point) { // Set the step size based on last two distinct points
  delta_point = (current_point - prev_point) * 0.25; // Use a smaller step size then default to help ensure enough points are generated;
  }
 
@@ -288,6 +299,7 @@ namespace basic_autonomy
  new_pair.point = new_point;
  new_pair.speed = points_and_target_speeds.back().speed;
 
+
  points_and_target_speeds.push_back(new_pair);
  }
 
@@ -1359,4 +1371,4 @@ namespace basic_autonomy
 
  } // namespace waypoint_generation
 
-} // basic_autonomy
+} // basic_autonomy

basic_autonomy/src/helper_functions.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2021-2022 LEIDOS.
+ * Copyright (C) 2021-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
@@ -14,6 +14,7 @@
  * the License.
  */
 
+#include <algorithm>
 #include <basic_autonomy/helper_functions.hpp>
 
 
@@ -64,20 +65,28 @@ namespace waypoint_generation
 
  int get_nearest_index_by_downtrack(const std::vector<lanelet::BasicPoint2d>& points, const carma_wm::WorldModelConstPtr& wm, double target_downtrack)
  {
- size_t best_index = points.size() - 1;
- for(size_t i = 0;i < points.size(); i++){
- double downtrack = wm->routeTrackPos(points[i]).downtrack;
- if(downtrack > target_downtrack){
- //If value is negative, best index should be index 0
- best_index = std::max((size_t)0, i - 1);
+ if(std::empty(points)){
+ RCLCPP_WARN_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Empty points vector received, returning -1");
+ return -1;
+ }
 
- RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "get_nearest_index_by_downtrack>> Found best_idx: " << best_index<<", points[i].x(): " << points[best_index].x() << ", points[i].y(): " << points[best_index].y() << ", downtrack: "<< downtrack);
- break;
- }
+ // Find first point with a downtrack greater than target_downtrack
+ const auto itr = std::find_if(std::cbegin(points), std::cend(points), 
+ [&wm = std::as_const(wm), target_downtrack](const auto & point) { return wm->routeTrackPos(point).downtrack > target_downtrack; });
+
+ int best_index = std::size(points) - 1;
+
+ // Set best_index to the last point with a downtrack less than target_downtrack
+ if(itr != std::cbegin(points)){
+ best_index = std::distance(std::cbegin(points), std::prev(itr));
+ }
+ else{
+ best_index = 0;
  }
- RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "get_nearest_index_by_downtrack>> Found best_idx: " << best_index<<", points[i].x(): " << points[best_index].x() << ", points[i].y(): " << points[best_index].y());
 
- return static_cast<int>(best_index);
+ RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "get_nearest_index_by_downtrack>> Found best_index: " << best_index<<", points[i].x(): " << points.at(best_index).x() << ", points[i].y(): " << points.at(best_index).y());
+
+ return best_index;
  }
 
  void split_point_speed_pairs(const std::vector<PointSpeedPair>& points,

basic_autonomy/test/test_waypoint_generation.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2022 LEIDOS.
+ * Copyright (C) 2019-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
@@ -113,7 +113,7 @@ namespace basic_autonomy
 
  }
 
-  TEST(BasicAutonomyTest, constrain_to_time_boundary)
+ TEST(BasicAutonomyTest, constrain_to_time_boundary)
  {
 
  std::vector<waypoint_generation::PointSpeedPair> points;
@@ -201,7 +201,7 @@ namespace basic_autonomy
  ASSERT_EQ(3, waypoint_generation::get_nearest_point_index(points, state));
  }
 
-TEST(BasicAutonomyTest, get_nearest_basic_point_index)
+ TEST(BasicAutonomyTest, get_nearest_basic_point_index)
  {
  std::vector<waypoint_generation::PointSpeedPair> points;
 
@@ -231,6 +231,59 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index)
  ASSERT_EQ(3, waypoint_generation::get_nearest_point_index(points, state));
  }
 
+ TEST(BasicAutonomyTest, get_nearest_index_by_downtrack_with_nonempty_points)
+ {
+ // Create CARMA World Model with custom Guidance Test Map
+ std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
+ auto map = carma_wm::test::buildGuidanceTestMap(5.0, 10.0); // Lanelet width 5.0 meters; Lanelet length 10.0 meters
+ wm->setMap(map);
+ carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm);
+
+ // Create vector of points with y-values corresponding to their respective downtracks on the set route
+ std::vector<lanelet::BasicPoint2d> points;
+
+ lanelet::BasicPoint2d point = lanelet::BasicPoint2d(2.5, 5.0);
+ points.push_back(point);
+ point = lanelet::BasicPoint2d(2.5, 10.0);
+ points.push_back(point);
+ point = lanelet::BasicPoint2d(2.5, 15.0);
+ points.push_back(point);
+ point = lanelet::BasicPoint2d(2.5, 20.0);
+ points.push_back(point);
+
+ // Test with downtrack before points index 0
+ double target_downtrack = 4.0;
+ ASSERT_EQ(0, waypoint_generation::get_nearest_index_by_downtrack(points, wm, target_downtrack));
+
+ // Test with downtrack after points index 1
+ target_downtrack = 11.0;
+ ASSERT_EQ(1, waypoint_generation::get_nearest_index_by_downtrack(points, wm, target_downtrack));
+
+ // Test with downtrack after points index 2
+ target_downtrack = 17.0;
+ ASSERT_EQ(2, waypoint_generation::get_nearest_index_by_downtrack(points, wm, target_downtrack));
+
+ // Test with downtrack after points index 3
+ target_downtrack = 22.0;
+ ASSERT_EQ(3, waypoint_generation::get_nearest_index_by_downtrack(points, wm, target_downtrack));
+ }
+
+ TEST(BasicAutonomyTest, get_nearest_index_by_downtrack_with_empty_points)
+ {
+ // Create CARMA World Model with custom Guidance Test Map
+ std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
+ auto map = carma_wm::test::buildGuidanceTestMap(5.0, 10.0); // Lanelet width 5.0 meters; Lanelet length 10.0 meters
+ wm->setMap(map);
+ carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, wm);
+
+ // Create vector of points; this will remain empty
+ const std::vector<lanelet::BasicPoint2d> points;
+
+ // Test with arbitrary target_downtrack to verify proper return value due to empty points vector
+ static constexpr auto target_downtrack{4.0};
+ ASSERT_EQ(-1, waypoint_generation::get_nearest_index_by_downtrack(points, wm, target_downtrack));
+ }
+
  TEST(BasicAutonomyTest, split_point_speed_pairs)
  {
  std::vector<waypoint_generation::PointSpeedPair> points;

carma/launch/environment.launch.py

@@ -280,6 +280,7 @@ def generate_launch_description():
  ],
  remappings=[
  ("external_objects", "external_object_predictions"),
+ ("external_objects_viz", "fused_external_objects_viz")
  ],
  parameters=[ object_visualizer_param_file, vehicle_config_param_file ]
  ),