Merge branch 'main' into fix/segmenation_pointcloud_filter_add_rle_fi…

…lter

.cppcheck_suppressions

@@ -1,7 +1,6 @@
 *:*/test/*
 
 checkersReport
-functionStatic
 missingInclude
 missingIncludeSystem
 noConstructor

common/perception_utils/CMakeLists.txt

@@ -4,8 +4,6 @@ project(perception_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_package()
-
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/run_length_encoder.cpp
 )
@@ -14,3 +12,5 @@ find_package(OpenCV REQUIRED)
 target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBS}
 )
+
+ament_auto_package()

control/autoware_external_cmd_selector/README.md

@@ -32,3 +32,7 @@ The current mode is set via service, `remote` is remotely operated, `local` is t
 | `/external/selected/hazard_lights_cmd`                 | autoware_vehicle_msgs::msg::HazardLightsCommand   | Pass through hazard light with current mode.    |
 | `/external/selected/heartbeat`                         | TBD                                               | Pass through heartbeat with current mode.       |
 | `/external/selected/turn_indicators_cmd`               | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode.  |
+
+## Parameters
+
+{{json_to_markdown("control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json")}}

control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json

@@ -0,0 +1,36 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for external_cmd_selector",
+  "type": "object",
+  "definitions": {
+    "external_cmd_selector_node": {
+      "type": "object",
+      "properties": {
+        "update_rate": {
+          "type": "number",
+          "default": 10.0,
+          "description": "The rate in Hz at which the external command selector node updates."
+        },
+        "initial_selector_mode": {
+          "type": "string",
+          "enum": ["local", "remote"],
+          "default": "local",
+          "description": "The initial mode for command selection, either 'local' or 'remote'."
+        }
+      },
+      "required": ["update_rate", "initial_selector_mode"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/external_cmd_selector_node"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -361,12 +361,14 @@ template <typename T>
 T VehicleCmdGate::getContinuousTopic(
   const std::shared_ptr<T> & prev_topic, const T & current_topic, const std::string & topic_name)
 {
-  if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() > 0.0) {
+  if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) {
     return current_topic;
   } else {
-    RCLCPP_INFO(
-      get_logger(),
-      "The operation mode is changed, but the %s is not received yet:", topic_name.c_str());
+    if (topic_name != "") {
+      RCLCPP_INFO(
+        get_logger(),
+        "The operation mode is changed, but the %s is not received yet:", topic_name.c_str());
+    }
     return *prev_topic;
   }
 }

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node
 
   template <typename T>
   T getContinuousTopic(
-    const std::shared_ptr<T> & prev_topic, const T & current_topic, const std::string & topic_name);
+    const std::shared_ptr<T> & prev_topic, const T & current_topic,
+    const std::string & topic_name = "");
 
   // Algorithm
   Control prev_control_cmd_;

evaluator/kinematic_evaluator/CMakeLists.txt

@@ -38,8 +38,7 @@ if(BUILD_TESTING)
   )
 endif()
 
-ament_auto_package(
-  INSTALL_TO_SHARE
+ament_auto_package(INSTALL_TO_SHARE
     param
     launch
 )

evaluator/kinematic_evaluator/README.md

@@ -1,3 +1,7 @@
 # Kinematic Evaluator
 
 TBD
+
+## Parameters
+
+{{json_to_markdown("evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json")}}

evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json

@@ -0,0 +1,35 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for kinematic_evaluator_node",
+  "type": "object",
+  "definitions": {
+    "kinematic_evaluator_node": {
+      "type": "object",
+      "properties": {
+        "output_file": {
+          "type": "string",
+          "default": "kinematic_metrics.results",
+          "description": "Name of the file to which kinematic metrics are written. If empty, metrics are not written to a file."
+        },
+        "selected_metrics": {
+          "type": "string",
+          "default": "velocity_stats",
+          "description": "The specific metrics selected for evaluation."
+        }
+      },
+      "required": ["output_file", "selected_metrics"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/kinematic_evaluator_node"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp

@@ -66,6 +66,7 @@ class Debugger
     pointcloud_within_polygon_->clear();
   }
 
+  // cppcheck-suppress functionStatic
   void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXY>::Ptr & input)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr input_xyz = toXYZ(input);

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

@@ -32,6 +32,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <tf2/utils.h>
@@ -360,20 +361,11 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
 }
 
 bool withinRoadLanelet(
-  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const TrackedObject & object,
+  const std::vector<std::pair<double, lanelet::Lanelet>> & surrounding_lanelets_with_dist,
   const bool use_yaw_information = false)
 {
-  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
-  // nearest lanelet
-  constexpr double search_radius = 10.0;  // [m]
-  const auto surrounding_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
-
-  for (const auto & lanelet_with_dist : surrounding_lanelets) {
-    const auto & dist = lanelet_with_dist.first;
-    const auto & lanelet = lanelet_with_dist.second;
-
+  for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
     if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
       lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (
@@ -397,6 +389,20 @@ bool withinRoadLanelet(
   return false;
 }
 
+bool withinRoadLanelet(
+  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const bool use_yaw_information = false)
+{
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
+  // nearest lanelet
+  constexpr double search_radius = 10.0;  // [m]
+  const auto surrounding_lanelets_with_dist =
+    lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius);
+
+  return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information);
+}
+
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
   const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points,
@@ -1411,9 +1417,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
-  // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
-  // implementation has been wrong.
-  const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
+  const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d(
     lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
     prediction_time_horizon_.pedestrian * velocity);
   lanelet::ConstLanelets surrounding_lanelets;
@@ -1458,7 +1462,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
     // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest
     // crosswalk and generate path to the crosswalk edge
-  } else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
+  } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) {
     lanelet::ConstLanelet closest_crosswalk{};
     const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
     const auto found_closest_crosswalk =

perception/autoware_multi_object_tracker/schema/input_channels.schema.json

@@ -8,22 +8,26 @@
       "properties": {
         "topic": {
           "type": "string",
-          "description": "The ROS topic name for the input channel."
+          "description": "The ROS topic name for the input channel.",
+          "default": "/perception/object_recognition/detection/objects"
         },
         "can_spawn_new_tracker": {
           "type": "boolean",
-          "description": "Indicates if the input channel can spawn new trackers."
+          "description": "Indicates if the input channel can spawn new trackers.",
+          "default": true
         },
         "optional": {
           "type": "object",
           "properties": {
             "name": {
               "type": "string",
-              "description": "The name of the input channel."
+              "description": "The name of the input channel.",
+              "default": "detected_objects"
             },
             "short_name": {
               "type": "string",
-              "description": "The short name of the input channel."
+              "description": "The short name of the input channel.",
+              "default": "all"
             }
           }
         }

perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp

@@ -20,7 +20,6 @@
 
 #include <memory>
 #include <string>
-#include <utility>
 
 namespace autoware::traffic_light
 {
@@ -103,26 +102,18 @@ bool TrafficLightRoiVisualizerNode::createRect(
   cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi,
   const ClassificationResult & result)
 {
-  cv::Scalar color;
-  if (result.label.find("red") != std::string::npos) {
-    color = cv::Scalar{254, 149, 149};
-  } else if (result.label.find("yellow") != std::string::npos) {
-    color = cv::Scalar{254, 250, 149};
-  } else if (result.label.find("green") != std::string::npos) {
-    color = cv::Scalar{149, 254, 161};
-  } else {
-    color = cv::Scalar{250, 250, 250};
-  }
+  const auto info = extractShapeInfo(result.label);
 
   cv::rectangle(
     image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset),
     cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height),
-    color, 2);
+    info.color, 2);
 
-  std::string shape_name = extractShapeName(result.label);
+  constexpr int shape_img_size = 16;
+  const auto position = cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset);
 
   visualization::drawTrafficLightShape(
-    image, shape_name, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), color, 16, result.prob);
+    image, info.shapes, shape_img_size, position, info.color, result.prob);
 
   return true;
 }

perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp

@@ -36,7 +36,9 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <sstream>
 #include <string>
+#include <vector>
 
 namespace autoware::traffic_light
 {
@@ -46,6 +48,15 @@ struct ClassificationResult
   std::string label;
 };
 
+/**
+ * @brief A struct to represent parsed traffic light shape information.
+ */
+struct TrafficLightShapeInfo
+{
+  cv::Scalar color;                 //!< Color associated with "circle".
+  std::vector<std::string> shapes;  //!< Shape names.
+};
+
 class TrafficLightRoiVisualizerNode : public rclcpp::Node
 {
 public:
@@ -78,25 +89,57 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node
     {tier4_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW, "right"},
     {tier4_perception_msgs::msg::TrafficLightElement::UP_ARROW, "straight"},
     {tier4_perception_msgs::msg::TrafficLightElement::DOWN_ARROW, "down"},
+    {tier4_perception_msgs::msg::TrafficLightElement::UP_LEFT_ARROW, "straight_left"},
+    {tier4_perception_msgs::msg::TrafficLightElement::UP_RIGHT_ARROW, "straight_right"},
     {tier4_perception_msgs::msg::TrafficLightElement::DOWN_LEFT_ARROW, "down_left"},
     {tier4_perception_msgs::msg::TrafficLightElement::DOWN_RIGHT_ARROW, "down_right"},
     {tier4_perception_msgs::msg::TrafficLightElement::CROSS, "cross"},
     // other
     {tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN, "unknown"},
   };
 
-  std::string extractShapeName(const std::string & label)
+  /**
+   * @brief Return RGB color from color string associated with "circle".
+   * @param color Color string.
+   * @return RGB color.
+   */
+  static cv::Scalar strToColor(const std::string & color)
+  {
+    if (color == "red") {
+      return {254, 149, 149};
+    } else if (color == "yellow") {
+      return {254, 250, 149};
+    } else if (color == "green") {
+      return {149, 254, 161};
+    } else {
+      return {250, 250, 250};
+    }
+  }
+
+  /**
+   * @brief Extract color and shape names from label.
+   * @param label String formatted as `<Color0>-<Shape0>,<Color1>-<Shape1>,...,<ColorN>-<ShapeN>`.
+   * @return Extracted information includes a color associated with "circle" and shape names.
+   */
+  static TrafficLightShapeInfo extractShapeInfo(const std::string & label)
   {
-    size_t start_pos = label.find('-');
-    if (start_pos != std::string::npos) {
-      start_pos++;                                  // Start after the hyphen
-      size_t end_pos = label.find(',', start_pos);  // Find the next comma after the hyphen
-      if (end_pos == std::string::npos) {  // If no comma is found, take the rest of the string
-        end_pos = label.length();
+    cv::Scalar color{255, 255, 255};
+    std::vector<std::string> shapes;
+
+    std::stringstream ss(label);
+    std::string segment;
+    while (std::getline(ss, segment, ',')) {
+      size_t hyphen_pos = segment.find('-');
+      if (hyphen_pos != std::string::npos) {
+        auto shape = segment.substr(hyphen_pos + 1);
+        if (shape == "circle") {
+          const auto color_str = segment.substr(0, hyphen_pos);
+          color = strToColor(color_str);
+        }
+        shapes.emplace_back(shape);
       }
-      return label.substr(start_pos, end_pos - start_pos);
     }
-    return "unknown";  // Return "unknown" if no hyphen is found
+    return {color, shapes};
   }
 
   bool createRect(