Merge pull request #527 from tier4/sync-upstream

chore: sync upstream

common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp

@@ -21,8 +21,6 @@
 #include <qt5/QtWidgets/QWidget>
 #include <rviz_common/display_context.hpp>
 
-#include <rclcpp/version.h>
-
 namespace rviz_plugins
 {
 BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent)
@@ -69,16 +67,9 @@ void BagTimeManagerPanel::onInitialize()
 {
   raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 
-// APIs taking rclcpp::QoS objects are only available in ROS 2 Iron and higher
-#if RCLCPP_VERSION_MAJOR >= 18
-  const auto qos_default = rclcpp::ServicesQoS();
-#else
-  const auto qos_default = rmw_qos_profile_services_default;
-#endif
-
-  client_pause_ = raw_node_->create_client<Pause>("/rosbag2_player/pause", qos_default);
-  client_resume_ = raw_node_->create_client<Resume>("/rosbag2_player/resume", qos_default);
-  client_set_rate_ = raw_node_->create_client<SetRate>("/rosbag2_player/set_rate", qos_default);
+  client_pause_ = raw_node_->create_client<Pause>("/rosbag2_player/pause");
+  client_resume_ = raw_node_->create_client<Resume>("/rosbag2_player/resume");
+  client_set_rate_ = raw_node_->create_client<SetRate>("/rosbag2_player/set_rate");
 }
 
 void BagTimeManagerPanel::onPauseClicked()

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -255,14 +255,14 @@ void RTCManagerPanel::onInitialize()
 {
   raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 
-  client_rtc_commands_ = raw_node_->create_client<CooperateCommands>(
-    "/api/external/set/rtc_commands", rmw_qos_profile_services_default);
+  client_rtc_commands_ =
+    raw_node_->create_client<CooperateCommands>("/api/external/set/rtc_commands");
 
   for (size_t i = 0; i < auto_modes_.size(); i++) {
     auto & a = auto_modes_.at(i);
     // auto mode
-    a->enable_auto_mode_cli = raw_node_->create_client<AutoMode>(
-      enable_auto_mode_namespace_ + "/" + a->module_name, rmw_qos_profile_services_default);
+    a->enable_auto_mode_cli =
+      raw_node_->create_client<AutoMode>(enable_auto_mode_namespace_ + "/" + a->module_name);
   }
 
   sub_rtc_status_ = raw_node_->create_subscription<CooperateStatusArray>(

common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp

@@ -61,22 +61,20 @@ void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node)
     "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1));
 
-  cli_change_to_autonomous_ = node->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default);
+  cli_change_to_autonomous_ =
+    node->create_client<ChangeOperationMode>("/api/operation_mode/change_to_autonomous");
 
-  cli_change_to_stop_ = node->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default);
+  cli_change_to_stop_ =
+    node->create_client<ChangeOperationMode>("/api/operation_mode/change_to_stop");
 
   // Planning
   sub_route_ = node->create_subscription<RouteState>(
     "/api/routing/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1));
 
-  cli_clear_route_ =
-    node->create_client<ClearRoute>("/api/routing/clear_route", rmw_qos_profile_services_default);
+  cli_clear_route_ = node->create_client<ClearRoute>("/api/routing/clear_route");
 
-  cli_set_route_ = node->create_client<SetRoutePoints>(
-    "/api/routing/set_route_points", rmw_qos_profile_services_default);
+  cli_set_route_ = node->create_client<SetRoutePoints>("/api/routing/set_route_points");
 }
 
 // Sub

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

@@ -176,8 +176,7 @@ void ManualController::onInitialize()
   sub_gear_ = raw_node_->create_subscription<GearReport>(
     "/vehicle/status/gear_status", 10, std::bind(&ManualController::onGear, this, _1));
 
-  client_engage_ = raw_node_->create_client<EngageSrv>(
-    "/api/autoware/set/engage", rmw_qos_profile_services_default);
+  client_engage_ = raw_node_->create_client<EngageSrv>("/api/autoware/set/engage");
 
   pub_gate_mode_ = raw_node_->create_publisher<GateMode>("/control/gate_mode_cmd", rclcpp::QoS(1));
 

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -219,31 +219,30 @@ void AutowareStatePanel::onInitialize()
     "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onOperationMode, this, _1));
 
-  client_change_to_autonomous_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default);
+  client_change_to_autonomous_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_autonomous");
 
-  client_change_to_stop_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default);
+  client_change_to_stop_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_stop");
 
-  client_change_to_local_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_local", rmw_qos_profile_services_default);
+  client_change_to_local_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_local");
 
-  client_change_to_remote_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/change_to_remote", rmw_qos_profile_services_default);
+  client_change_to_remote_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_remote");
 
-  client_enable_autoware_control_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/enable_autoware_control", rmw_qos_profile_services_default);
+  client_enable_autoware_control_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/enable_autoware_control");
 
-  client_enable_direct_control_ = raw_node_->create_client<ChangeOperationMode>(
-    "/api/operation_mode/disable_autoware_control", rmw_qos_profile_services_default);
+  client_enable_direct_control_ =
+    raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/disable_autoware_control");
 
   // Routing
   sub_route_ = raw_node_->create_subscription<RouteState>(
     "/api/routing/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onRoute, this, _1));
 
-  client_clear_route_ = raw_node_->create_client<ClearRoute>(
-    "/api/routing/clear_route", rmw_qos_profile_services_default);
+  client_clear_route_ = raw_node_->create_client<ClearRoute>("/api/routing/clear_route");
 
   // Localization
   sub_localization_ = raw_node_->create_subscription<LocalizationInitializationState>(
@@ -255,8 +254,7 @@ void AutowareStatePanel::onInitialize()
     "/api/motion/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onMotion, this, _1));
 
-  client_accept_start_ = raw_node_->create_client<AcceptStart>(
-    "/api/motion/accept_start", rmw_qos_profile_services_default);
+  client_accept_start_ = raw_node_->create_client<AcceptStart>("/api/motion/accept_start");
 
   // FailSafe
   sub_mrm_ = raw_node_->create_subscription<MRMState>(
@@ -271,7 +269,7 @@ void AutowareStatePanel::onInitialize()
     "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1));
 
   client_emergency_stop_ = raw_node_->create_client<tier4_external_api_msgs::srv::SetEmergency>(
-    "/api/autoware/set/emergency", rmw_qos_profile_services_default);
+    "/api/autoware/set/emergency");
 
   pub_velocity_limit_ = raw_node_->create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
     "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local());

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -23,6 +23,7 @@
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -60,6 +61,7 @@ using vehicle_info_util::VehicleInfo;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 using Path = std::vector<geometry_msgs::msg::Pose>;
+using Vector3 = geometry_msgs::msg::Vector3;
 
 struct ObjectData
 {
@@ -155,7 +157,7 @@ class AEB : public rclcpp::Node
 
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
-  Imu::ConstSharedPtr imu_ptr_{nullptr};
+  Vector3::SharedPtr angular_velocity_ptr_{nullptr};
   Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr};
   AutowareState::ConstSharedPtr autoware_state_{nullptr};
 

control/autonomous_emergency_braking/src/node.cpp

@@ -154,7 +154,21 @@ void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg)
 
 void AEB::onImu(const Imu::ConstSharedPtr input_msg)
 {
-  imu_ptr_ = input_msg;
+  // transform imu
+  geometry_msgs::msg::TransformStamped transform_stamped{};
+  try {
+    transform_stamped = tf_buffer_.lookupTransform(
+      "base_link", input_msg->header.frame_id, input_msg->header.stamp,
+      rclcpp::Duration::from_seconds(0.5));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(
+      get_logger(),
+      "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id);
+    return;
+  }
+
+  angular_velocity_ptr_ = std::make_shared<Vector3>();
+  tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped);
 }
 
 void AEB::onPredictedTrajectory(
@@ -225,7 +239,7 @@ bool AEB::isDataReady()
     return missing("object pointcloud");
   }
 
-  if (use_imu_path_ && !imu_ptr_) {
+  if (use_imu_path_ && !angular_velocity_ptr_) {
     return missing("imu");
   }
 
@@ -286,7 +300,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   if (use_imu_path_) {
     Path ego_path;
     std::vector<Polygon2d> ego_polys;
-    const double current_w = imu_ptr_->angular_velocity.z;
+    const double current_w = angular_velocity_ptr_->z;
     constexpr double color_r = 0.0 / 256.0;
     constexpr double color_g = 148.0 / 256.0;
     constexpr double color_b = 205.0 / 256.0;

control/joy_controller/src/joy_controller/joy_controller_node.cpp

@@ -511,8 +511,8 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
     RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection...");
   }
 
-  client_autoware_engage_ = this->create_client<tier4_external_api_msgs::srv::Engage>(
-    "service/autoware_engage", rmw_qos_profile_services_default);
+  client_autoware_engage_ =
+    this->create_client<tier4_external_api_msgs::srv::Engage>("service/autoware_engage");
 
   // Timer
   initTimer(1.0 / update_rate_);

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
             ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
             ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"),
+            ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
         ],
         parameters=[
             common_param,

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -67,8 +67,8 @@ MapUpdateModule::MapUpdateModule(
       &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_);
 
-  pcd_loader_client_ = node->create_client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>(
-    "pcd_loader_service", rmw_qos_profile_services_default);
+  pcd_loader_client_ =
+    node->create_client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>("pcd_loader_service");
   while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
     RCLCPP_INFO(
       logger_,

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -61,6 +61,7 @@
 #include <tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
+#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
 #include <map>
@@ -90,6 +91,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 using tier4_planning_msgs::msg::LateralOffset;
 using tier4_planning_msgs::msg::Scenario;
+using tier4_planning_msgs::msg::StopReasonArray;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -114,6 +116,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
   rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
+  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -23,6 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 
 #include <algorithm>
 #include <limits>
@@ -37,6 +38,7 @@ namespace behavior_path_planner
 
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using tier4_autoware_utils::StopWatch;
+using tier4_planning_msgs::msg::StopReasonArray;
 using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
 
@@ -150,6 +152,34 @@ class PlannerManager
     return ret;
   }
 
+  /**
+   * @brief aggregate launched module's stop reasons.
+   * @return stop reason array
+   */
+  StopReasonArray getStopReasons() const
+  {
+    StopReasonArray stop_reason_array;
+    stop_reason_array.header.frame_id = "map";
+    stop_reason_array.header.stamp = clock_.now();
+
+    std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+      const auto reason = m->getStopReason();
+      if (reason.reason != "") {
+        stop_reason_array.stop_reasons.push_back(m->getStopReason());
+      }
+    });
+
+    std::for_each(
+      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [&](const auto & m) {
+        const auto reason = m->getStopReason();
+        if (reason.reason != "") {
+          stop_reason_array.stop_reasons.push_back(m->getStopReason());
+        }
+      });
+
+    return stop_reason_array;
+  }
+
   /**
    * @brief reset root lanelet. if there are approved modules, don't reset root lanelet.
    * @param planner data.
@@ -362,9 +392,9 @@ class PlannerManager
 
   std::vector<SceneModulePtr> candidate_module_ptrs_;
 
-  rclcpp::Logger logger_;
+  mutable rclcpp::Logger logger_;
 
-  rclcpp::Clock clock_;
+  mutable rclcpp::Clock clock_;
 
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;