Akwk v4.0.0

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -991,34 +991,51 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
  * curvature calculation
  */
 template <class T>
-std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+std::vector<std::pair<double, std::pair<double, double>>> calcCurvatureAndSegmentLength(
+  const T & points)
 {
-  // Note that arclength is for the segment, not the sum.
-  std::vector<std::pair<double, double>> curvature_arc_length_vec;
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
+  // segment length is pair of segment length between {p1, p2} and {p2, p3}
+  std::vector<std::pair<double, std::pair<double, double>>> curvature_and_segment_length_vec;
+  curvature_and_segment_length_vec.reserve(points.size());
+  curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0));
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1));
     const auto p2 = autoware::universe_utils::getPoint(points.at(i));
     const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1));
     const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3);
-    const double arc_length =
-      autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
-      autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1));
-    curvature_arc_length_vec.emplace_back(curvature, arc_length);
+
+    // The first point has only the next point, so put the distance to that point.
+    // In other words, assign the first segment length at the second point to the
+    // second_segment_length at the first point.
+    if (i == 1) {
+      curvature_and_segment_length_vec.at(0).second.second =
+        autoware::universe_utils::calcDistance2d(p1, p2);
+    }
+
+    // The second_segment_length of the previous point and the first segment length of the current
+    // point are equal.
+    const std::pair<double, double> arc_length{
+      curvature_and_segment_length_vec.back().second.second,
+      autoware::universe_utils::calcDistance2d(p2, p3)};
+
+    curvature_and_segment_length_vec.emplace_back(curvature, arc_length);
   }
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
 
-  return curvature_arc_length_vec;
+  // set to the last point
+  curvature_and_segment_length_vec.emplace_back(
+    0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0));
+
+  return curvature_and_segment_length_vec;
 }
 
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 /**
@@ -2487,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold = 0.0);
 
+/// @brief calculate the time_from_start fields of the given trajectory points
+/// @details this function assumes constant longitudinal velocity between points
+/// @param trajectory trajectory for which to calculate the time_from_start
+/// @param current_ego_point current ego position
+/// @param min_velocity minimum velocity used for a trajectory point
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);
+
 }  // namespace autoware::motion_utils
 
 #endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -238,14 +238,14 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //
@@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold);
 
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
+{
+  const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
+  if (nearest_segment_idx + 1 == trajectory.size()) {
+    return;
+  }
+  for (auto & p : trajectory) {
+    p.time_from_start = rclcpp::Duration::from_seconds(0);
+  }
+  // TODO(Maxime): some points can have very low velocities which introduce huge time errors
+  // Temporary solution: use a minimum velocity
+  for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
+    const auto & from = trajectory[idx - 1];
+    const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
+    if (velocity != 0.0) {
+      auto & to = trajectory[idx];
+      const auto t = universe_utils::calcDistance2d(from, to) / velocity;
+      to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
+    }
+  }
+}
 }  // namespace autoware::motion_utils

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp

@@ -102,8 +102,7 @@ private Q_SLOTS:
     turn_signals_sub_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::HazardLightsReport>::SharedPtr
     hazard_lights_sub_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>::SharedPtr
-    traffic_sub_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroup>::SharedPtr traffic_sub_;
   rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr speed_limit_sub_;
 
   std::mutex property_mutex_;
@@ -117,7 +116,7 @@ private Q_SLOTS:
     const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg);
   void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg);
+    const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg);
   void drawWidget(QImage & hud);
 };
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp

@@ -25,7 +25,6 @@
 
 #include <autoware_perception_msgs/msg/traffic_light_element.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group.hpp>
-#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 
 #include <OgreColourValue.h>
 #include <OgreMaterial.h>
@@ -40,8 +39,8 @@ class TrafficDisplay
   TrafficDisplay();
   void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg);
-  autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_;
+    const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg);
+  autoware_perception_msgs::msg::TrafficLightGroup current_traffic_;
 
 private:
   QImage traffic_light_image_;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -109,9 +109,10 @@ void SignalDisplay::onInitialize()
   speed_limit_topic_property_->initialize(rviz_ros_node);
 
   traffic_topic_property_ = std::make_unique<rviz_common::properties::RosTopicProperty>(
-    "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals",
-    "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data",
-    this, SLOT(topic_updated_traffic()));
+    "Traffic Topic",
+    "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal",
+    "autoware_perception_msgs/msgs/msg/TrafficLightGroup", "Topic for Traffic Light Data", this,
+    SLOT(topic_updated_traffic()));
   traffic_topic_property_->initialize(rviz_ros_node);
 }
 
@@ -189,7 +190,7 @@ void SignalDisplay::onDisable()
 }
 
 void SignalDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg)
+  const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(property_mutex_);
 
@@ -454,14 +455,13 @@ void SignalDisplay::topic_updated_traffic()
   // resubscribe to the topic
   traffic_sub_.reset();
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
-  traffic_sub_ =
-    rviz_ros_node->get_raw_node()
-      ->create_subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>(
-        traffic_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-        [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) {
-          updateTrafficLightData(msg);
-        });
+  traffic_sub_ = rviz_ros_node->get_raw_node()
+                   ->create_subscription<autoware_perception_msgs::msg::TrafficLightGroup>(
+                     traffic_topic_property_->getTopicStd(),
+                     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                     [this](const autoware_perception_msgs::msg::TrafficLightGroup::SharedPtr msg) {
+                       updateTrafficLightData(msg);
+                     });
 }
 
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp

@@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay()
 }
 
 void TrafficDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg)
+  const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg)
 {
   current_traffic_ = *msg;
 }
@@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
     backgroundRect.top() + circleRect.height() / 2));
   painter.drawEllipse(circleRect);
 
-  if (!current_traffic_.traffic_light_groups.empty()) {
-    switch (current_traffic_.traffic_light_groups[0].elements[0].color) {
+  if (!current_traffic_.elements.empty()) {
+    switch (current_traffic_.elements[0].color) {
       case 1:
         painter.setBrush(QBrush(tl_red_));
         painter.drawEllipse(circleRect);

common/autoware_point_types/include/autoware_point_types/types.hpp

@@ -54,6 +54,23 @@ enum ReturnType : uint8_t {
   DUAL_ONLY,
 };
 
+struct PointXYZIRC
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+
+  friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel;
+  }
+};
+
 struct PointXYZIRADRT
 {
   float x{0.0F};
@@ -75,25 +92,97 @@ struct PointXYZIRADRT
   }
 };
 
-enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp };
+struct PointXYZIRCAEDT
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+  float azimuth{0.0F};
+  float elevation{0.0F};
+  float distance{0.0F};
+  std::uint32_t time_stamp{0U};
+
+  friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel &&
+           float_eq<float>(p1.azimuth, p2.azimuth) && float_eq<float>(p1.distance, p2.distance) &&
+           p1.time_stamp == p2.time_stamp;
+  }
+};
+
+enum class PointXYZIIndex { X, Y, Z, Intensity };
+enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel };
+enum class PointXYZIRADRTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  Ring,
+  Azimuth,
+  Distance,
+  ReturnType,
+  TimeStamp
+};
+enum class PointXYZIRCAEDTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  ReturnType,
+  Channel,
+  Azimuth,
+  Elevation,
+  Distance,
+  TimeStamp
+};
 
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth);
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp);
 
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel);
+
+using PointXYZIRCGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator>;
+
 using PointXYZIRADRTGenerator = std::tuple<
   point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
   point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
   point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator,
   field_return_type_generator, field_time_stamp_generator>;
 
+using PointXYZIRCAEDTGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator, field_azimuth_generator,
+  field_elevation_generator, field_distance_generator, field_time_stamp_generator>;
+
 }  // namespace autoware_point_types
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRC,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel))
+
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   autoware_point_types::PointXYZIRADRT,
   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
     float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
     double, time_stamp, time_stamp))
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRCAEDT,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type,
+    return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
+    float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))
 #endif  // AUTOWARE_POINT_TYPES__TYPES_HPP_

common/autoware_point_types/test/test_point_types.cpp

@@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT)
   EXPECT_EQ(pt0, pt1);
 }
 
+TEST(PointEquality, PointXYZIRCAEDT)
+{
+  using autoware_point_types::PointXYZIRCAEDT;
+
+  PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  EXPECT_EQ(pt0, pt1);
+}
+
 TEST(PointEquality, FloatEq)
 {
   // test template