Merge branch 'main' into feat/downsapmple_perception_input_pointclouds

Author: YoshiRi

.github/PULL_REQUEST_TEMPLATE/small-change.md

@@ -15,6 +15,10 @@ Not applicable.
 
 Not applicable.
 
+## Interface changes
+
+<!-- Describe any changed interfaces, such as topics, services, or parameters, including debugging interfaces -->
+
 ## Pre-review checklist for the PR author
 
 The PR author **must** check the checkboxes below when creating the PR.

.github/PULL_REQUEST_TEMPLATE/standard-change.md

@@ -18,6 +18,19 @@
 
 <!-- Describe any changed interfaces, such as topics, services, or parameters. -->
 
+### ROS Topic Changes
+
+<!-- | Topic Name       | Type                | Direction | Update Description                                            | -->
+<!-- | ---------------- | ------------------- | --------- | ------------------------------------------------------------- | -->
+<!-- | `/example_topic` | `std_msgs/String`   | Subscribe | Description of what the topic is used for in the system       | -->
+<!-- | `/another_topic` | `sensor_msgs/Image` | Publish   | Also explain if it is added / modified / deleted with the PR | -->
+
+### ROS Parameter Changes
+
+<!-- | Parameter Name       | Default Value | Update Description                                  | -->
+<!-- | -------------------- | ------------- | --------------------------------------------------- | -->
+<!-- | `example_parameters` | `1.0`         | Describe the parameter and also explain the updates | -->
+
 ## Effects on system behavior
 
 <!-- Describe how this PR affects the system behavior. -->

build_depends.repos

@@ -41,3 +41,7 @@ repositories:
     type: git
     url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
     version: main
+  universe/external/glog:  # TODO: to use isGoogleInitialized() API in v0.6.0. Remove when the rosdep glog version is updated to v0.6.0 (already updated in Ubuntu 24.04)
+    type: git
+    url: https://github.com/tier4/glog.git
+    version: v0.6.0_t4-ros

common/autoware_auto_common/include/autoware_auto_common/common/types.hpp

@@ -37,7 +37,9 @@ namespace types
 // We don't currently require code to comply to MISRA, but we should try to where it is
 // easily possible.
 using bool8_t = bool;
+#if __cplusplus < 201811L || !__cpp_char8_t
 using char8_t = char;
+#endif
 using uchar8_t = unsigned char;
 // If we ever compile on a platform where this is not true, float32_t and float64_t definitions
 // need to be adjusted.

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -57,6 +57,9 @@ struct ObjectPropertyValues
   float alpha{0.999F};
 };
 
+// Control object marker visualization
+enum class ObjectFillType { Skeleton, Fill };
+
 // Map defining colors according to value of label field in ObjectClassification msg
 const std::map<
   autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues>
@@ -87,7 +90,8 @@ get_shape_marker_ptr(
   const autoware_auto_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
-  const bool & is_orientation_available = true);
+  const bool & is_orientation_available = true,
+  const ObjectFillType fill_type = ObjectFillType::Skeleton);
 
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_2d_shape_marker_ptr(

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

@@ -112,6 +112,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_confidence_interval_property->addOption("95%", 2);
     m_confidence_interval_property->addOption("99%", 3);
 
+    m_object_fill_type_property = new rviz_common::properties::EnumProperty(
+      "Object Fill Type", "skeleton", "Change object fill type in visualization", this);
+    m_object_fill_type_property->addOption(
+      "skeleton", static_cast<int>(detail::ObjectFillType::Skeleton));
+    m_object_fill_type_property->addOption("Fill", static_cast<int>(detail::ObjectFillType::Fill));
+
     // iterate over default values to create and initialize the properties.
     for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
       const auto & class_property_values = map_property_it.second;
@@ -189,9 +195,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     const bool & is_orientation_available) const
   {
     const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
+    const auto fill_type =
+      static_cast<detail::ObjectFillType>(m_object_fill_type_property->getOptionInt());
+
     if (m_display_type_property->getOptionInt() == 0) {
       return detail::get_shape_marker_ptr(
-        shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
+        shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available,
+        fill_type);
     } else if (m_display_type_property->getOptionInt() == 1) {
       return detail::get_2d_shape_marker_ptr(
         shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
@@ -526,6 +536,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   rviz_common::properties::EnumProperty * m_simple_visualize_mode_property;
   // Property to set confidence interval of state estimations
   rviz_common::properties::EnumProperty * m_confidence_interval_property;
+  // Property to set visualization type
+  rviz_common::properties::EnumProperty * m_object_fill_type_property;
   // Property to enable/disable label visualization
   rviz_common::properties::BoolProperty m_display_label_property;
   // Property to enable/disable uuid visualization

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -495,23 +495,37 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   const autoware_auto_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
-  const bool & is_orientation_available)
+  const bool & is_orientation_available, const ObjectFillType fill_type)
 {
   auto marker_ptr = std::make_shared<Marker>();
   marker_ptr->ns = std::string("shape");
+  marker_ptr->color = color_rgba;
+  marker_ptr->scale.x = line_width;
 
   using autoware_auto_perception_msgs::msg::Shape;
   if (shape_msg.type == Shape::BOUNDING_BOX) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_bounding_box_line_list(shape_msg, marker_ptr->points);
+    if (fill_type == ObjectFillType::Skeleton) {
+      marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+      calc_bounding_box_line_list(shape_msg, marker_ptr->points);
+    } else if (fill_type == ObjectFillType::Fill) {
+      marker_ptr->type = visualization_msgs::msg::Marker::CUBE;
+      marker_ptr->scale = shape_msg.dimensions;
+      marker_ptr->color.a = 0.75f;
+    }
     if (is_orientation_available) {
       calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points);
     } else {
       calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points);
     }
   } else if (shape_msg.type == Shape::CYLINDER) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_cylinder_line_list(shape_msg, marker_ptr->points);
+    if (fill_type == ObjectFillType::Skeleton) {
+      marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+      calc_cylinder_line_list(shape_msg, marker_ptr->points);
+    } else if (fill_type == ObjectFillType::Fill) {
+      marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER;
+      marker_ptr->scale = shape_msg.dimensions;
+      marker_ptr->color.a = 0.75f;
+    }
   } else if (shape_msg.type == Shape::POLYGON) {
     marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
     calc_polygon_line_list(shape_msg, marker_ptr->points);
@@ -523,8 +537,6 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
   marker_ptr->pose = to_pose(centroid, orientation);
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
-  marker_ptr->scale.x = line_width;
-  marker_ptr->color = color_rgba;
 
   return marker_ptr;
 }

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp

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

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp

@@ -24,7 +24,6 @@
 #include <rviz_common/ros_topic_display.hpp>
 
 #include <autoware_perception_msgs/msg/traffic_signal.hpp>
-#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_perception_msgs/msg/traffic_signal_element.hpp>
 
 #include <OgreColourValue.h>
@@ -40,8 +39,8 @@ class TrafficDisplay
   TrafficDisplay();
   void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg);
-  autoware_perception_msgs::msg::TrafficSignalArray current_traffic_;
+    const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg);
+  autoware_perception_msgs::msg::TrafficSignal current_traffic_;
 
 private:
   QImage traffic_light_image_;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -111,8 +111,9 @@ 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/msg/TrafficSignalArray", "Topic for Traffic Light Data", this,
+    "Traffic Topic",
+    "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal",
+    "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this,
     SLOT(topic_updated_traffic()));
   traffic_topic_property_->initialize(rviz_ros_node);
 }
@@ -159,10 +160,10 @@ void SignalDisplay::setupRosSubscriptions()
         updateHazardLightsData(msg);
       });
 
-  traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignalArray>(
+  traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
     traffic_topic_property_->getTopicStd(),
     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) {
+    [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
       updateTrafficLightData(msg);
     });
 
@@ -237,7 +238,7 @@ void SignalDisplay::onDisable()
 }
 
 void SignalDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
+  const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(property_mutex_);
 
@@ -506,14 +507,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::TrafficSignalArray>(
-        traffic_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-        [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) {
-          updateTrafficLightData(msg);
-        });
+  traffic_sub_ = rviz_ros_node->get_raw_node()
+                   ->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
+                     traffic_topic_property_->getTopicStd(),
+                     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                     [this](const autoware_perception_msgs::msg::TrafficSignal::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::TrafficSignalArray::ConstSharedPtr & msg)
+  const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg)
 {
   current_traffic_ = *msg;
 }
@@ -70,8 +70,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
     backgroundRect.top() + circleRect.height() + 30));
   painter.drawEllipse(circleRect);
 
-  if (!current_traffic_.signals.empty()) {
-    switch (current_traffic_.signals[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/glog_component/CMakeLists.txt

@@ -8,7 +8,7 @@ autoware_package()
 ament_auto_add_library(glog_component SHARED
   src/glog_component.cpp
 )
-target_link_libraries(glog_component glog)
+target_link_libraries(glog_component glog::glog)
 
 rclcpp_components_register_node(glog_component
   PLUGIN "GlogComponent"

common/glog_component/package.xml

@@ -13,7 +13,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>libgoogle-glog-dev</depend>
+  <depend>glog</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 

common/glog_component/src/glog_component.cpp

@@ -17,8 +17,10 @@
 GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options)
 : Node("glog_component", node_options)
 {
-  google::InitGoogleLogging("glog_component");
-  google::InstallFailureSignalHandler();
+  if (!google::IsGoogleLoggingInitialized()) {
+    google::InitGoogleLogging("glog_component");
+    google::InstallFailureSignalHandler();
+  }
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -33,6 +33,8 @@ Planning:
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: behavior_path_planner.path_shifter
 
   behavior_path_planner_avoidance:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
@@ -82,6 +84,10 @@ Planning:
     - node_name: /planning/scenario_planning/motion_velocity_smoother
       logger_name: tier4_autoware_utils
 
+  route_handler:
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: route_handler
+
 # ============================================================
 # control
 # ============================================================

common/tier4_simulated_clock_rviz_plugin/README.md

@@ -10,18 +10,37 @@ This plugin allows publishing and controlling the simulated ROS time.
 | -------- | --------------------------- | -------------------------- |
 | `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time |
 
-## HowToUse
+## How to use the plugin
+
+1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`.
+
+   ```shell
+   ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true
+   ```
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses.
+
+2. Start rviz and select panels/Add new panel.
 
-1. Start rviz and select panels/Add new panel.
    ![select_panel](./images/select_panels.png)
-2. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
+3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
    ![select_clock_plugin](./images/select_clock_plugin.png)
-3. Use the added panel to control how the simulated clock is published.
+
+4. Use the added panel to control how the simulated clock is published.
+
    ![use_clock_plugin](./images/use_clock_plugin.png)
 
-   - Pause button: pause/resume the clock.
-   - Speed: speed of the clock relative to the system clock.
-   - Rate: publishing rate of the clock.
-   - Step button: advance the clock by the specified time step.
-   - Time step: value used to advance the clock when pressing the step button d).
-   - Time unit: time unit associated with the value from e).
+   <ol type="a">
+     <li>Pause button: pause/resume the clock.</li>
+     <li>Speed: speed of the clock relative to the system clock.</li>
+     <li>Rate: publishing rate of the clock.</li>
+     <li>Step button: advance the clock by the specified time step.</li>
+     <li>Time step: value used to advance the clock when pressing the step button d).</li>
+     <li>Time unit: time unit associated with the value from e).</li>
+   </ol>
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you set the time step too large, your simulation will go haywire.

common/tvm_utility/example/yolo_v2_tiny/main.cpp

@@ -200,6 +200,8 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessor<std:
             }
           }
 
+          if (max_ind == -1) continue;
+
           // Decode and copy class probabilities
           std::vector<float> class_probabilities{};
           float p_total = 0;