Merge pull request #203 from tier4/sync-upstream

chore: sync upstream

common/motion_utils/src/resample/resample.cpp

@@ -260,7 +260,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
 
   autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path;
   resampled_path.header = input_path.header;
-  resampled_path.drivable_area = input_path.drivable_area;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = input_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;
@@ -402,7 +403,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
 
   autoware_auto_planning_msgs::msg::Path resampled_path;
   resampled_path.header = input_path.header;
-  resampled_path.drivable_area = input_path.drivable_area;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = resampled_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;

common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp

@@ -308,9 +308,8 @@ bool validateFloats(const nav_msgs::msg::OccupancyGrid & msg)
 }
 
 void AutowareDrivableAreaDisplay::processMessage(
-  autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg)
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg)
 {
-  current_map_ = msg->drivable_area;
   loaded_ = true;
   // updated via signal in case ros spinner is in a different thread
   Q_EMIT mapUpdated();

common/tier4_state_rviz_plugin/README.md

@@ -2,31 +2,47 @@
 
 ## Purpose
 
-This plugin displays the current status of autoware.  
+This plugin displays the current status of autoware.
 This plugin also can engage from the panel.
 
 ## Inputs / Outputs
 
 ### Input
 
-| Name                          | Type                                            | Description                                        |
-| ----------------------------- | ----------------------------------------------- | -------------------------------------------------- |
-| `/control/current_gate_mode`  | `tier4_control_msgs::msg::GateMode`             | The topic represents the state of AUTO or EXTERNAL |
-| `/autoware/state`             | `autoware_auto_system_msgs::msg::AutowareState` | The topic represents the state of Autoware         |
-| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport`   | The topic represents the state of Gear             |
-| `/api/external/get/engage`    | `tier4_external_api_msgs::msg::EngageStatus`    | The topic represents the state of Engage           |
+| Name                                     | Type                                                           | Description                                                   |
+| ---------------------------------------- | -------------------------------------------------------------- | ------------------------------------------------------------- |
+| `/api/operation_mode/state`              | `autoware_adapi_v1_msgs::msg::OperationModeState`              | The topic represents the state of operation mode              |
+| `/api/routing/state`                     | `autoware_adapi_v1_msgs::msg::RouteState`                      | The topic represents the state of route                       |
+| `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization |
+| `/api/motion/state`                      | `autoware_adapi_v1_msgs::msg::MotionState`                     | The topic represents the state of motion                      |
+| `/api/autoware/get/emergency`            | `tier4_external_api_msgs::msg::Emergency`                      | The topic represents the state of external emergency          |
+| `/vehicle/status/gear_status`            | `autoware_auto_vehicle_msgs::msg::GearReport`                  | The topic represents the state of gear                        |
 
 ### Output
 
-| Name                       | Type                                   | Description                    |
-| -------------------------- | -------------------------------------- | ------------------------------ |
-| `/api/external/set/engage` | `tier4_external_api_msgs::srv::Engage` | The service inputs engage true |
+| Name                                               | Type                                               | Description                                        |
+| -------------------------------------------------- | -------------------------------------------------- | -------------------------------------------------- |
+| `/api/operation_mode/change_to_autonomous`         | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous |
+| `/api/operation_mode/change_to_stop`               | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop       |
+| `/api/operation_mode/change_to_local`              | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to local      |
+| `/api/operation_mode/change_to_remote`             | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to remote     |
+| `/api/operation_mode/enable_autoware_control`      | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to enable vehicle control by Autoware  |
+| `/api/operation_mode/disable_autoware_control`     | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to disable vehicle control by Autoware |
+| `/api/routing/clear_route`                         | `autoware_adapi_v1_msgs::srv::ClearRoute`          | The service to clear route state                   |
+| `/api/motion/accept_start`                         | `autoware_adapi_v1_msgs::srv::AcceptStart`         | The service to accept the vehicle to start         |
+| `/api/autoware/set/emergency`                      | `tier4_external_api_msgs::srv::SetEmergency`       | The service to set external emergency              |
+| `/planning/scenario_planning/max_velocity_default` | `tier4_planning_msgs::msg::VelocityLimit`          | The topic to set maximum speed of the vehicle      |
 
 ## HowToUse
 
 1. Start rviz and select panels/Add new panel.
+
    ![select_panel](./images/select_panels.png)
+
 2. Select tier4_state_rviz_plugin/AutowareStatePanel and press OK.
+
    ![select_state_plugin](./images/select_state_plugin.png)
-3. If the AutowareState is WaitingForEngage, can engage by clicking the Engage button.
-   ![select_engage](./images/select_engage.png)
+
+3. If the auto button is activated, can engage by clicking it.
+
+   ![select_auto](./images/select_auto.png)

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -234,6 +234,7 @@ bool MpcLateralController::isSteerConverged(
   // wait for a while to propagate the trajectory shape to the output command when the trajectory
   // shape is changed.
   if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
+    RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed");
     return false;
   }
 

control/trajectory_follower/src/mpc_utils.cpp

@@ -230,7 +230,7 @@ bool convertToMPCTrajectory(
   for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) {
     const double x = p.pose.position.x;
     const double y = p.pose.position.y;
-    const double z = 0.0;
+    const double z = p.pose.position.z;
     const double yaw = tf2::getYaw(p.pose.orientation);
     const double vx = p.longitudinal_velocity_mps;
     const double k = 0.0;

control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp

@@ -83,6 +83,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
   enum class LateralControllerMode {
     INVALID = 0,
@@ -106,6 +107,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
   LongitudinalControllerMode getLongitudinalControllerMode(
     const std::string & algorithm_name) const;
+  void publishDebugMarker() const;
 };
 }  // namespace trajectory_follower_nodes
 }  // namespace control

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -77,6 +77,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
+  debug_marker_pub_ =
+    create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", rclcpp::QoS{1});
 
   // Timer
   {
@@ -168,6 +170,34 @@ void Controller::callbackTimerControl()
   out.lateral = lateral_output_->control_cmd;
   out.longitudinal = longitudinal_output_->control_cmd;
   control_cmd_pub_->publish(out);
+
+  publishDebugMarker();
+}
+
+void Controller::publishDebugMarker() const
+{
+  visualization_msgs::msg::MarkerArray debug_marker_array{};
+
+  // steer converged marker
+  {
+    auto marker = tier4_autoware_utils::createDefaultMarker(
+      "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0),
+      tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99));
+    marker.pose = input_data_.current_odometry_ptr->pose.pose;
+
+    std::stringstream ss;
+    const double current = input_data_.current_steering_ptr->steering_tire_angle;
+    const double cmd = lateral_output_->control_cmd.steering_tire_angle;
+    const double diff = current - cmd;
+    ss << "current:" << current << " cmd:" << cmd << " diff:" << diff
+       << (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)");
+    marker.text = ss.str();
+
+    debug_marker_array.markers.push_back(marker);
+  }
+
+  debug_marker_pub_->publish(debug_marker_array);
 }
 
 }  // namespace trajectory_follower_nodes

control/vehicle_cmd_gate/README.md

@@ -21,7 +21,6 @@
 | `~/input/external/gear_cmd`                 | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command from external                                           |
 | `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat`                  | heartbeat                                                            |
 | `~/input/gate_mode`                         | `tier4_control_msgs::msg::GateMode`                        | gate mode (AUTO or EXTERNAL)                                         |
-| `~/input/emergency_state`                   | `autoware_auto_system_msgs::msg::EmergencyState`           | used to detect the emergency situation of the vehicle                |
 | `~/input/emergency/control_cmd`             | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler |
 | `~/input/emergency/hazard_lights_cmd`       | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | hazard lights command from emergency handler                         |
 | `~/input/emergency/gear_cmd`                | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command from emergency handler                                  |

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -22,7 +22,6 @@
     <remap from="input/external_emergency_stop_heartbeat" to="/external/selected/heartbeat"/>
     <remap from="input/gate_mode" to="/control/gate_mode_cmd"/>
 
-    <remap from="input/emergency_state" to="/system/emergency/emergency_state"/>
     <remap from="input/emergency/control_cmd" to="/system/emergency/control_cmd"/>
     <remap from="input/emergency/hazard_lights_cmd" to="/system/emergency/hazard_lights_cmd"/>
     <remap from="input/emergency/gear_cmd" to="/system/emergency/gear_cmd"/>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -25,7 +25,6 @@
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
 #include <autoware_auto_vehicle_msgs/msg/engage.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>

launch/tier4_control_launch/launch/control.launch.py

@@ -171,7 +171,6 @@ def launch_setup(context, *args, **kwargs):
         plugin="vehicle_cmd_gate::VehicleCmdGate",
         name="vehicle_cmd_gate",
         remappings=[
-            ("input/emergency_state", "/system/emergency/emergency_state"),
             ("input/steering", "/vehicle/status/steering_status"),
             ("input/operation_mode", "/system/operation_mode/state"),
             ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"),

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -50,6 +50,9 @@
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
       max_avoidance_acceleration: 0.5  # [m/ss]
 
+      # bound clipping for objects
+      enable_bound_clipping: false
+
       # for debug
       publish_debug_marker: false
       print_debug_info: false

launch/tier4_system_launch/launch/system.launch.xml

@@ -13,14 +13,15 @@
     <!-- System Monitor -->
     <group if="$(var launch_system_monitor)">
       <push-ros-namespace namespace="system_monitor"/>
-      <include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch.py">
+      <include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch.xml">
         <arg name="cpu_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/cpu_monitor.param.yaml"/>
         <arg name="hdd_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/hdd_monitor.param.yaml"/>
         <arg name="mem_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/mem_monitor.param.yaml"/>
         <arg name="net_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/net_monitor.param.yaml"/>
         <arg name="ntp_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/ntp_monitor.param.yaml"/>
         <arg name="process_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/process_monitor.param.yaml"/>
         <arg name="gpu_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/gpu_monitor.param.yaml"/>
+        <arg name="voltage_monitor_config_file" value="$(var tier4_system_launch_param_path)/system_monitor/voltage_monitor.param.yaml"/>
       </include>
     </group>
 

localization/gyro_odometer/CMakeLists.txt

@@ -9,6 +9,8 @@ ament_auto_add_executable(${PROJECT_NAME}
   src/gyro_odometer_core.cpp
 )
 
+target_link_libraries(${PROJECT_NAME} fmt)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
 )

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

@@ -15,6 +15,9 @@
 #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
 #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
 
+#include "tier4_autoware_utils/ros/transform_listener.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/twist_stamped.hpp>
@@ -27,27 +30,28 @@
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 
+#include <deque>
+#include <memory>
 #include <string>
 
 class GyroOdometer : public rclcpp::Node
 {
+private:
+  using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
 public:
   GyroOdometer();
   ~GyroOdometer();
 
 private:
-  void callbackTwistWithCovariance(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr);
+  void callbackVehicleTwist(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
   void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
-  bool getTransform(
-    const std::string & target_frame, const std::string & source_frame,
-    const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr);
+  void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
 
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
-    vehicle_twist_with_cov_sub_;
+    vehicle_twist_sub_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
 
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_raw_pub_;
@@ -58,14 +62,15 @@ class GyroOdometer : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
     twist_with_covariance_pub_;
 
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
 
   std::string output_frame_;
   double message_timeout_sec_;
 
-  geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_;
-  sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr_;
+  bool vehicle_twist_arrived_;
+  bool imu_arrived_;
+  std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
+  std::deque<sensor_msgs::msg::Imu> gyro_queue_;
 };
 
 #endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/launch/gyro_odometer.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input_vehicle_twist_with_covariance_topic" default="/vehicle/status/twist_with_covariance" description="input twist with covariance topic name from vehicle"/>
+  <arg name="input_vehicle_twist_with_covariance_topic" default="/sensing/vehicle_velocity_converter/twist_with_covariance" description="input twist with covariance topic name from vehicle"/>
 
   <arg name="input_imu_topic" default="/sensing/imu/imu_data" description="input imu topic name"/>
 

localization/gyro_odometer/package.xml

@@ -12,12 +12,14 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>