Feature/waypoint downsample

carma_wm/src/Geometry.cpp

@@ -25,6 +25,8 @@ namespace carma_wm
 namespace geometry
 {
 
+constexpr double SPATIAL_EPSILON_M = 0.05;
+
 // https://stackoverflow.com/questions/8489792/is-it-legal-to-take-acos-of-1-0f-or-1-0f
 double safeAcos (double x)
 {
@@ -329,8 +331,16 @@ concatenate_line_strings(const lanelet::BasicLineString2d& a,
                                           const lanelet::BasicLineString2d& b)
 {
   lanelet::BasicLineString2d out;
+
+  int start_offset = 0;
+  if (!a.empty() && !b.empty()) {
+    if (compute_euclidean_distance(a.back(), b.front()) < SPATIAL_EPSILON_M) {
+      start_offset = 1;
+    }
+  }
+
   out.insert(out.end(), a.begin(), a.end());
-  out.insert(out.end(), b.begin(), b.end());
+  out.insert(out.end(), b.begin() + start_offset, b.end());
 
   return out;
 }

carma_wm/test/GeometryTest.cpp

@@ -657,6 +657,35 @@ TEST(GeometryTest, compute_tangent_orientations_straight)
   }
 }
 
+TEST(GeometryTest, concatenate_line_string_dedupe) 
+{
+  auto p1 = Eigen::Vector2d(-1, 0);
+  auto p2 = Eigen::Vector2d(-1, 1);
+  auto p3 = Eigen::Vector2d(-1, 2);
+  auto p4 = Eigen::Vector2d(-1, 2.001);
+  auto p5 = Eigen::Vector2d(-1, 3);
+  auto p6 = Eigen::Vector2d(-1, 4);
+  const lanelet::BasicLineString2d line1 = { p1, p2, p3 };
+  const lanelet::BasicLineString2d line2 = { p4, p4, p6 };
+
+  auto result = carma_wm::geometry::concatenate_line_strings(line1, line2);
+
+  ASSERT_EQ(5, result.size());
+
+  p1 = Eigen::Vector2d(-1, 0);
+  p2 = Eigen::Vector2d(-1, 1);
+  p3 = Eigen::Vector2d(-1, 2);
+  p4 = Eigen::Vector2d(-1, 2.5);
+  p5 = Eigen::Vector2d(-1, 3);
+  p6 = Eigen::Vector2d(-1, 4);
+  const lanelet::BasicLineString2d line3 = { p1, p2, p3 };
+  const lanelet::BasicLineString2d line4 = { p4, p4, p6 };
+
+  result = carma_wm::geometry::concatenate_line_strings(line3, line4);
+
+  ASSERT_EQ(6, result.size());
+}
+
 TEST(GeometryTest, compute_tangent_orientations_curved)
 {
 

waypoint_generator/config/params.yaml

@@ -21,4 +21,8 @@ longitudinal_decel_limit: 3.0
 
 # Maximum speed along the route
 # Units: m/s
-max_speed: 10.0
+max_speed: 10.0
+
+# Ratio to downsample output waypoints (keep 1/n waypoints)
+# Units: N/a
+downsample_ratio: 2

waypoint_generator/include/waypoint_generator/waypoint_generator.hpp

@@ -67,11 +67,11 @@ namespace waypoint_generator
             /**!
              * \brief Get a list of speed limits at each point of the centerline
              * for each lanelet
-             * \param lanelets The lanelets to get speed limits from
+             * \param points The centerline points to get speed limit data for
              * \return A vector where the i-th element is the speed limit for
              * the i-th centerline point amongst all input lanelets
              */
-            std::vector<double> get_speed_limits(std::vector<lanelet::ConstLanelet> lanelets) const;
+            std::vector<double> get_speed_limits(const lanelet::BasicLineString2d& points) const;
 
             /**!
              * \brief Normalize the curvature within curvature regions to an
@@ -160,6 +160,19 @@ namespace waypoint_generator
     std::vector<double> speeds, std::vector<geometry_msgs::Quaternion> orientations,
     const lanelet::BasicLineString2d& centerline) const;
 
+
+            /**!
+             * \brief Downsample the generated waypoints to a specified ratio.
+             * 
+             * \param waypoints The LaneArray message to be downsampled
+             * \param ratio The ratio to reduce the size by (e.g. 2 yields 1/2,
+             * yields 1/4 points)
+             * 
+             * \return The fully composed autoware_msgs::LaneArray object ready
+             * for publication
+             */
+            autoware_msgs::LaneArray downsample_waypoints(autoware_msgs::LaneArray waypoints, int ratio) const;
+
             /**!
              * \brief Apply a basic speed limiter to all speeds in the input list
              * 

waypoint_generator/include/waypoint_generator/waypoint_generator_config.hpp

@@ -7,4 +7,5 @@ struct WaypointGeneratorConfig {
   double _longitudinal_accel_limit = 1.5;
   double _longitudinal_decel_limit = 1.5;
   double _max_speed = 10.0;
+  int _downsample_ratio = 2.0;
 };

waypoint_generator/src/waypoint_generator/waypoint_generator.cpp

@@ -293,32 +293,37 @@ autoware_msgs::LaneArray WaypointGenerator::generate_lane_array_message(
 
   return out;
 }
-std::vector<double> WaypointGenerator::get_speed_limits(std::vector<lanelet::ConstLanelet> lanelets) const
+std::vector<double> WaypointGenerator::get_speed_limits(const lanelet::BasicLineString2d& centerline) const
 {
   std::vector<double> out;
   if (!_wm)
   {
     ROS_ERROR_STREAM("get_speed_limit: Invalid WM");
     throw std::invalid_argument("get_speed_limit: Inavlid WM");
   }
-  if (lanelets.size() == 0)
+  if (centerline.size() == 0)
   {
-    ROS_ERROR_STREAM("get_speed_limit: Invalid lanelets");
-    throw std::invalid_argument("get_speed_limit: Empty lanelets passed!");
+    ROS_ERROR_STREAM("get_speed_limit: Invalid centerline");
+    throw std::invalid_argument("get_speed_limit: Empty centerline passed!");
   }
-  for (int i = 0; i < lanelets.size(); i++)
+  for (int i = 0; i < centerline.size(); i++)
   {
-    auto slis = lanelets[i].regulatoryElementsAs<lanelet::DigitalSpeedLimit>();
-    if (slis.size() == 0)
+
+    auto nearest_lanelet = _wm->getMap()->laneletLayer.nearest(centerline[i], 1);
+    if (nearest_lanelet.size() == 0)
     {
-      std::string err_msg = "get_speed_limit: Lanalet Id:" + std::to_string(lanelets[i].id()) + " has no Digital Speed Limit Regulatory Element!";
+      std::string err_msg = "get_speed_limit: No lanelet found matching point #" + std::to_string(i);
       ROS_ERROR_STREAM(err_msg);
       throw std::invalid_argument(err_msg);
     }
-    for (int j = 0; j < lanelets[i].centerline2d().size(); j++)
+    auto slis = nearest_lanelet[0].regulatoryElementsAs<lanelet::DigitalSpeedLimit>();
+    if (slis.size() == 0)
     {
-      out.push_back(slis[0]->getSpeedLimit().value());
+      std::string err_msg = "get_speed_limit: Lanalet Id:" + std::to_string(nearest_lanelet[0].id()) + " has no Digital Speed Limit Regulatory Element!";
+      ROS_ERROR_STREAM(err_msg);
+      throw std::invalid_argument(err_msg);
     }
+    out.push_back(slis[0]->getSpeedLimit().value());
   }
 
   return out;
@@ -477,7 +482,7 @@ void WaypointGenerator::new_route_callback()
     ROS_DEBUG_STREAM(" Speed: " << p);
   }
 
-  std::vector<double> speed_limits = get_speed_limits(tmp);
+  std::vector<double> speed_limits = get_speed_limits(route_geometry);
 
   ROS_DEBUG_STREAM(" ");
   ROS_DEBUG_STREAM(" ");
@@ -516,10 +521,30 @@ void WaypointGenerator::new_route_callback()
   ROS_DEBUG("Generating final waypoint message.");
   autoware_msgs::LaneArray waypoint_msg;
   waypoint_msg = this->generate_lane_array_message(final_speeds, orientations, centerline);
+  waypoint_msg = this->downsample_waypoints(waypoint_msg, _config._downsample_ratio);
 
   ROS_DEBUG_STREAM("Finished processing route.");
 
   _waypoint_publisher(waypoint_msg);
   ROS_DEBUG_STREAM("Published waypoints list!");
 }
+
+autoware_msgs::LaneArray WaypointGenerator::downsample_waypoints(autoware_msgs::LaneArray waypoints, int ratio) const
+{
+  autoware_msgs::LaneArray downsampled{waypoints};
+
+  for (int i = 0; i < downsampled.lanes.size(); i++) {
+    int idx = 0;
+    for (auto j = downsampled.lanes[i].waypoints.begin(); j != downsampled.lanes[i].waypoints.end();) {
+      if (idx++ % ratio != 0) {
+        j = downsampled.lanes[i].waypoints.erase(j);
+      } else {
+        ++j;
+      }
+    }
+  }
+
+  return downsampled;
+}
+
 };  // namespace waypoint_generator

waypoint_generator/src/waypoint_generator/waypoint_generator_node.cpp

@@ -42,9 +42,11 @@ namespace waypoint_generator
         _pnh->param<double>("lateral_accel_limit", config._lateral_accel_limit, config._lateral_accel_limit);
         _pnh->param<double>("longitudinal_accel_limit", config._longitudinal_accel_limit, config._longitudinal_accel_limit);
         _pnh->param<double>("longitudinal_decel_limit", config._longitudinal_decel_limit, config._longitudinal_decel_limit);
+        _pnh->param<int>("downsample_ratio", config._downsample_ratio, config._downsample_ratio);
         ROS_DEBUG_STREAM("Parameters loaded!" << std::endl
             << "curvature_epsilon: " << config._curvature_epsilon << std::endl
             << "linearity_constraint" << config._linearity_constraint << std::endl
+            << "downsample_ratio" << config._downsample_ratio << std::endl
             << "lateral_accel_limit: " << config._lateral_accel_limit << std::endl
             << "longitudinal_accel_limit: " << config._longitudinal_accel_limit << std::endl
             << "longitudinal_decel_limit: " << config._longitudinal_decel_limit << std::endl);