Add autoware api (#1979)

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -40,7 +40,9 @@
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
 
+#include "autoware_api_utils/autoware_api_utils.hpp"
 #include "autoware_debug_msgs/msg/float32_stamped.hpp"
+#include "autoware_external_api_msgs/srv/initialize_pose.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_vehicle_msgs/msg/control_mode.hpp"
 #include "autoware_vehicle_msgs/msg/engage.hpp"
@@ -70,6 +72,10 @@ class Simulator : public rclcpp::Node
 
 private:
   /* ros system */
+  rclcpp::CallbackGroup::SharedPtr group_api_service_;
+  autoware_api_utils::Service<autoware_external_api_msgs::srv::InitializePose>::SharedPtr
+    srv_set_pose_;  //!< @brief service to set pose for simulation
+
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
     pub_pose_;  //!< @brief topic ros publisher for current pose
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr
@@ -185,12 +191,15 @@ class Simulator : public rclcpp::Node
    */
   void callbackTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
 
+
+  // TODO(Takagi, Isamu): deprecated
   /**
    * @brief set initial pose for simulation with received message
    */
   void callbackInitialPoseWithCov(
     const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
 
+  // TODO(Takagi, Isamu): deprecated
   /**
    * @brief set initial pose with received message
    */
@@ -206,6 +215,13 @@ class Simulator : public rclcpp::Node
    */
   void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg);
 
+  /**
+   * @brief set pose for simulation with request
+   */
+  void serviceSetPose(
+    const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request,
+    const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response);
+
   /**
    * @brief get transform from two frame_ids
    * @param [in] parent_frame parent frame id

simulator/simple_planning_simulator/package.xml

@@ -15,7 +15,9 @@
   <depend>geometry_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>autoware_api_utils</depend>
   <depend>autoware_debug_msgs</depend>
+  <depend>autoware_external_api_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_control_msgs</depend>
   <depend>autoware_utils</depend>

simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp

@@ -24,7 +24,7 @@
 Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options)
 : Node(node_name, options)
 {
-  using std::placeholders::_1;
+  using namespace std::placeholders;
 
   /* simple_planning_simulator parameters */
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -37,6 +37,14 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions &
   use_trajectory_for_z_position_source_ =
     declare_parameter("use_trajectory_for_z_position_source", true);
 
+  /* service */
+  autoware_api_utils::ServiceProxyNodeInterface proxy(this);
+  group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_set_pose_ = proxy.create_service<autoware_external_api_msgs::srv::InitializePose>(
+    "/api/simulator/set/pose",
+    std::bind(&Simulator::serviceSetPose, this, _1, _2),
+    rmw_qos_profile_services_default, group_api_service_);
+
   /* set pub sub topic name */
   pub_pose_ =
     create_publisher<geometry_msgs::msg::PoseStamped>("output/current_pose", rclcpp::QoS{1});
@@ -240,6 +248,21 @@ void Simulator::callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSh
   simulator_engage_ = msg->engage;
 }
 
+void Simulator::serviceSetPose(
+  const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request,
+  const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response)
+{
+  geometry_msgs::msg::Twist initial_twist;  // initialized with zero for all components
+  if (initial_twist_ptr_) {
+    initial_twist = initial_twist_ptr_->twist;
+  }
+  // save initial pose
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  initial_pose_with_cov_ptr_ = std::make_shared<PoseWithCovarianceStamped>(request->pose);
+  setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist);
+  response->status = autoware_api_utils::response_success();
+}
+
 void Simulator::timerCallbackSimulation()
 {
   if (!is_initialized_) {