feat(simple_planning_simulator): add control_mode server (#1061)

* add control-mode in simulator

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update readme

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update simulator

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

simulator/simple_planning_simulator/design/simple_planning_simulator-design.md

@@ -17,16 +17,24 @@ The purpose of this simulator is for the integration test of planning and contro
 
 ### input
 
-- input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle.
-- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle.
-- input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear).
 - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
+- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle
+- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual)
+- input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command.
+- input/manual_gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual)
+- input/turn_indicators_command [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command
+- input/hazard_lights_command [`autoware_auto_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command
+- input/control_mode_request [`tier4_vehicle_msgs::srv::ControlModeRequest`] : mode change for Auto/Manual driving
 
 ### output
 
 - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link)
-- /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM)
-- /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.)
+- /output/odometry [`nav_msgs/msg/Odometry`] : simulated vehicle pose and twist
+- /output/steering [`autoware_auto_vehicle_msgs/msg/SteeringReport`] : simulated steering angle
+- /output/control_mode_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual)
+- /output/gear_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated gear
+- /output/turn_indicators_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status
+- /output/hazard_lights_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status
 
 ## Inner-workings / Algorithms
 

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -42,6 +42,8 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tier4_external_api_msgs/srv/initialize_pose.hpp"
+#include "tier4_vehicle_msgs/msg/control_mode.hpp"
+#include "tier4_vehicle_msgs/srv/control_mode_request.hpp"
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -80,6 +82,8 @@ using geometry_msgs::msg::TransformStamped;
 using geometry_msgs::msg::Twist;
 using nav_msgs::msg::Odometry;
 using tier4_external_api_msgs::srv::InitializePose;
+using tier4_vehicle_msgs::msg::ControlMode;
+using tier4_vehicle_msgs::srv::ControlModeRequest;
 
 class DeltaTime
 {
@@ -130,14 +134,18 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
 
   rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
+  rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
   rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr sub_turn_indicators_cmd_;
   rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_;
   rclcpp::Subscription<VehicleControlCommand>::SharedPtr sub_vehicle_cmd_;
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_ackermann_cmd_;
+  rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_manual_ackermann_cmd_;
   rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_init_pose_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
   rclcpp::Subscription<Engage>::SharedPtr sub_engage_;
 
+  rclcpp::Service<ControlModeRequest>::SharedPtr srv_mode_req_;
+
   rclcpp::CallbackGroup::SharedPtr group_api_service_;
   tier4_api_utils::Service<InitializePose>::SharedPtr srv_set_pose_;
 
@@ -156,13 +164,15 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   VelocityReport current_velocity_;
   Odometry current_odometry_;
   SteeringReport current_steer_;
-  VehicleControlCommand::ConstSharedPtr current_vehicle_cmd_ptr_;
-  AckermannControlCommand::ConstSharedPtr current_ackermann_cmd_ptr_;
-  GearCommand::ConstSharedPtr current_gear_cmd_ptr_;
+  AckermannControlCommand current_ackermann_cmd_;
+  AckermannControlCommand current_manual_ackermann_cmd_;
+  GearCommand current_gear_cmd_;
+  GearCommand current_manual_gear_cmd_;
   TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_;
   HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_;
   Trajectory::ConstSharedPtr current_trajectory_ptr_;
-  bool current_engage_;
+  bool simulate_motion_;  //!< stop vehicle motion simulation if false
+  ControlMode current_control_mode_;
 
   /* frame_id */
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
@@ -195,20 +205,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg);
 
-  /**
-   * @brief set current_ackermann_cmd_ptr_ with received message
-   */
-  void on_ackermann_cmd(const AckermannControlCommand::ConstSharedPtr msg);
-
   /**
    * @brief set input steering, velocity, and acceleration of the vehicle model
    */
-  void set_input(const float steer, const float vel, const float accel);
-
-  /**
-   * @brief set current_vehicle_state_ with received message
-   */
-  void on_gear_cmd(const GearCommand::ConstSharedPtr msg);
+  void set_input(const AckermannControlCommand & cmd);
 
   /**
    * @brief set current_vehicle_state_ with received message
@@ -242,6 +242,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void on_engage(const Engage::ConstSharedPtr msg);
 
+  /**
+   * @brief ControlModeRequest server
+   */
+  void on_control_mode_request(
+    const ControlModeRequest::Request::SharedPtr request,
+    const ControlModeRequest::Response::SharedPtr response);
+
   /**
    * @brief get z-position from trajectory
    * @param [in] x current x-position

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp

@@ -111,7 +111,7 @@ class SimModelDelaySteerAcc : public SimModelInterface
   float64_t getVy() override;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   float64_t getAx() override;
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp

@@ -111,7 +111,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
   float64_t getVy() override;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   float64_t getAx() override;
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp

@@ -74,7 +74,7 @@ class SimModelIdealSteerAcc : public SimModelInterface
   float64_t getVy() override;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   float64_t getAx() override;
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp

@@ -75,7 +75,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface
   float64_t getVy() override;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   float64_t getAx() override;
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp

@@ -75,7 +75,7 @@ class SimModelIdealSteerVel : public SimModelInterface
   float64_t getVy() override;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   float64_t getAx() override;
 

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp

@@ -128,7 +128,7 @@ class SimModelInterface
   virtual float64_t getVy() = 0;
 
   /**
-   * @brief get vehicle longiudinal acceleration
+   * @brief get vehicle longitudinal acceleration
    */
   virtual float64_t getAx() = 0;
 
@@ -142,13 +142,18 @@ class SimModelInterface
    */
   virtual float64_t getSteer() = 0;
 
+  /**
+   * @brief get vehicle gear
+   */
+  uint8_t getGear() const;
+
   /**
    * @brief get state vector dimension
    */
   inline int getDimX() { return dim_x_; }
 
   /**
-   * @brief get input vector demension
+   * @brief get input vector dimension
    */
   inline int getDimU() { return dim_u_; }
 

simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py

@@ -54,11 +54,14 @@ def launch_setup(context, *args, **kwargs):
         ],
         remappings=[
             ("input/ackermann_control_command", "/control/command/control_cmd"),
+            ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
             ("input/gear_command", "/control/command/gear_cmd"),
+            ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
             ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
             ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
             ("input/trajectory", "/planning/scenario_planning/trajectory"),
             ("input/engage", "/vehicle/engage"),
+            ("input/control_mode_request", "/system/control_mode_request"),
             ("output/twist", "/vehicle/status/velocity_status"),
             ("output/odometry", "/localization/kinematic_state"),
             ("output/steering", "/vehicle/status/steering_status"),

simulator/simple_planning_simulator/package.xml

@@ -27,6 +27,7 @@
   <depend>tier4_api_utils</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
   <depend>vehicle_info_util</depend>
 
   <exec_depend>launch_ros</exec_depend>

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -79,7 +79,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
   simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
   origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
   add_measurement_noise_ = declare_parameter("add_measurement_noise", false);
-  current_engage_ = declare_parameter<bool>("initial_engage_state");
+  simulate_motion_ = declare_parameter<bool>("initial_engage_state");
 
   using rclcpp::QoS;
   using std::placeholders::_1;
@@ -89,9 +89,16 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1));
   sub_ackermann_cmd_ = create_subscription<AckermannControlCommand>(
     "input/ackermann_control_command", QoS{1},
-    std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1));
+    [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; });
+  sub_manual_ackermann_cmd_ = create_subscription<AckermannControlCommand>(
+    "input/manual_ackermann_control_command", QoS{1},
+    [this](const AckermannControlCommand::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; });
   sub_gear_cmd_ = create_subscription<GearCommand>(
-    "input/gear_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_gear_cmd, this, _1));
+    "input/gear_command", QoS{1},
+    [this](const GearCommand::SharedPtr msg) { current_gear_cmd_ = *msg; });
+  sub_manual_gear_cmd_ = create_subscription<GearCommand>(
+    "input/manual_gear_command", QoS{1},
+    [this](const GearCommand::SharedPtr msg) { current_manual_gear_cmd_ = *msg; });
   sub_turn_indicators_cmd_ = create_subscription<TurnIndicatorsCommand>(
     "input/turn_indicators_command", QoS{1},
     std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1));
@@ -100,6 +107,12 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1));
   sub_trajectory_ = create_subscription<Trajectory>(
     "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1));
+
+  srv_mode_req_ = create_service<tier4_vehicle_msgs::srv::ControlModeRequest>(
+    "input/control_mode_request",
+    std::bind(&SimplePlanningSimulator::on_control_mode_request, this, _1, _2));
+
+  // TODO(Horibe): should be replaced by mode_request. Keep for the backward compatibility.
   sub_engage_ = create_subscription<Engage>(
     "input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1));
 
@@ -162,6 +175,10 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     x_stddev_ = declare_parameter("x_stddev", 0.0001);
     y_stddev_ = declare_parameter("y_stddev", 0.0001);
   }
+
+  // control mode
+  current_control_mode_.data = ControlMode::AUTO;
+  current_manual_gear_cmd_.command = GearCommand::DRIVE;
 }
 
 void SimplePlanningSimulator::initialize_vehicle_model()
@@ -241,7 +258,15 @@ void SimplePlanningSimulator::on_timer()
   {
     const float64_t dt = delta_time_.get_dt(get_clock()->now());
 
-    if (current_engage_) {
+    if (current_control_mode_.data == ControlMode::AUTO) {
+      vehicle_model_ptr_->setGear(current_gear_cmd_.command);
+      set_input(current_ackermann_cmd_);
+    } else {
+      vehicle_model_ptr_->setGear(current_manual_gear_cmd_.command);
+      set_input(current_manual_ackermann_cmd_);
+    }
+
+    if (simulate_motion_) {
       vehicle_model_ptr_->update(dt);
     }
   }
@@ -299,27 +324,22 @@ void SimplePlanningSimulator::on_set_pose(
   response->status = tier4_api_utils::response_success();
 }
 
-void SimplePlanningSimulator::on_ackermann_cmd(
-  const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
+void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd)
 {
-  current_ackermann_cmd_ptr_ = msg;
-  set_input(
-    msg->lateral.steering_tire_angle, msg->longitudinal.speed, msg->longitudinal.acceleration);
-}
+  const auto steer = cmd.lateral.steering_tire_angle;
+  const auto vel = cmd.longitudinal.speed;
+  const auto accel = cmd.longitudinal.acceleration;
 
-void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel)
-{
   using autoware_auto_vehicle_msgs::msg::GearCommand;
   Eigen::VectorXd input(vehicle_model_ptr_->getDimU());
+  const auto gear = vehicle_model_ptr_->getGear();
 
   // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different
   // between .auto and proposal.iv, and will be discussed later.
   float acc = accel;
-  if (!current_gear_cmd_ptr_) {
+  if (gear == GearCommand::NONE) {
     acc = 0.0;
-  } else if (
-    current_gear_cmd_ptr_->command == GearCommand::REVERSE ||
-    current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) {
+  } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
     acc = -accel;
   }
 
@@ -339,17 +359,6 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons
   vehicle_model_ptr_->setInput(input);
 }
 
-void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg)
-{
-  current_gear_cmd_ptr_ = msg;
-
-  if (
-    vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED ||
-    vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) {
-    vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command);
-  }
-}
-
 void SimplePlanningSimulator::on_turn_indicators_cmd(
   const TurnIndicatorsCommand::ConstSharedPtr msg)
 {
@@ -368,7 +377,16 @@ void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg
 
 void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg)
 {
-  current_engage_ = msg->engage;
+  simulate_motion_ = msg->engage;
+}
+
+void SimplePlanningSimulator::on_control_mode_request(
+  const ControlModeRequest::Request::SharedPtr request,
+  const ControlModeRequest::Response::SharedPtr response)
+{
+  current_control_mode_ = request->mode;
+  response->success = true;
+  return;
 }
 
 void SimplePlanningSimulator::add_measurement_noise(
@@ -504,7 +522,7 @@ void SimplePlanningSimulator::publish_control_mode_report()
 {
   ControlModeReport msg;
   msg.stamp = get_clock()->now();
-  if (current_engage_) {
+  if (current_control_mode_.data == ControlMode::AUTO) {
     msg.mode = ControlModeReport::AUTONOMOUS;
   } else {
     msg.mode = ControlModeReport::MANUAL;
@@ -514,12 +532,9 @@ void SimplePlanningSimulator::publish_control_mode_report()
 
 void SimplePlanningSimulator::publish_gear_report()
 {
-  if (!current_gear_cmd_ptr_) {
-    return;
-  }
   GearReport msg;
   msg.stamp = get_clock()->now();
-  msg.report = current_gear_cmd_ptr_->command;
+  msg.report = vehicle_model_ptr_->getGear();
   pub_gear_report_->publish(msg);
 }
 

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp

@@ -38,3 +38,4 @@ void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }
 void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }
 void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }
 void SimModelInterface::setGear(const uint8_t gear) { gear_ = gear; }
+uint8_t SimModelInterface::getGear() const { return gear_; }