feat(simple_planning_simulator): add control_mode server (autowarefou…

…ndation#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>
Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

map/map_tf_generator/CMakeLists.txt

@@ -16,6 +16,14 @@ rclcpp_components_register_node(map_tf_generator_node
   EXECUTABLE map_tf_generator
 )
 
+ament_auto_add_library(vector_map_tf_generator_node SHARED
+  src/vector_map_tf_generator_node.cpp
+)
+
+rclcpp_components_register_node(vector_map_tf_generator_node
+  PLUGIN "VectorMapTFGeneratorNode"
+  EXECUTABLE vector_map_tf_generator
+)
 
 if(BUILD_TESTING)
   function(add_testcase filepath)

map/map_tf_generator/package.xml

@@ -11,6 +11,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>lanelet2_extension</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>

map/map_tf_generator/src/vector_map_tf_generator_node.cpp

@@ -0,0 +1,96 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <memory>
+#include <string>
+
+class VectorMapTFGeneratorNode : public rclcpp::Node
+{
+public:
+  explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options)
+  : Node("vector_map_tf_generator", options)
+  {
+    map_frame_ = declare_parameter("map_frame", "map");
+    viewer_frame_ = declare_parameter("viewer_frame", "viewer");
+
+    sub_ = create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+      "vector_map", rclcpp::QoS{1}.transient_local(),
+      std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1));
+
+    static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
+  }
+
+private:
+  std::string map_frame_;
+  std::string viewer_frame_;
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_;
+
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
+  std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
+
+  void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
+  {
+    lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
+    lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_);
+    std::vector<double> points_x;
+    std::vector<double> points_y;
+    std::vector<double> points_z;
+
+
+    for (const lanelet::Point3d & point : lanelet_map_ptr_->pointLayer) {
+      const double point_x = point.x();
+      const double point_y = point.y();
+      const double point_z = point.z();
+      points_x.push_back(point_x); //same as convertPointsLayerToPoints in lanelet2_map_preprocessor
+      points_y.push_back(point_y);
+      points_z.push_back(point_z);
+    }
+    const double coordinate_x = std::accumulate(points_x.begin(), points_x.end(), 0.0) / points_x.size();
+    const double coordinate_y = std::accumulate(points_y.begin(), points_y.end(), 0.0) / points_y.size();
+    const double coordinate_z = std::accumulate(points_z.begin(), points_z.end(), 0.0) / points_z.size();
+
+    geometry_msgs::msg::TransformStamped static_transformStamped;
+    static_transformStamped.header.stamp = this->now();
+    static_transformStamped.header.frame_id = map_frame_;
+    static_transformStamped.child_frame_id = viewer_frame_;
+    static_transformStamped.transform.translation.x = coordinate_x;
+    static_transformStamped.transform.translation.y = coordinate_y;
+    static_transformStamped.transform.translation.z = coordinate_z;
+    tf2::Quaternion quat;
+    quat.setRPY(0, 0, 0);
+    static_transformStamped.transform.rotation.x = quat.x();
+    static_transformStamped.transform.rotation.y = quat.y();
+    static_transformStamped.transform.rotation.z = quat.z();
+    static_transformStamped.transform.rotation.w = quat.w();
+
+    static_broadcaster_->sendTransform(static_transformStamped);
+
+    RCLCPP_INFO_STREAM(
+      get_logger(), "broadcast static tf. map_frame:"
+                      << map_frame_ << ", viewer_frame:" << viewer_frame_ << ", x:" << coordinate_x
+                      << ", y:" << coordinate_y << ", z:" << coordinate_z);
+  }
+};
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(VectorMapTFGeneratorNode)

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>