From ae49c3f39e671fabf538930b2077492d540cf457 Mon Sep 17 00:00:00 2001
From: Takamasa Horibe <horibe.takamasa@gmail.com>
Date: Mon, 5 Dec 2022 14:36:42 +0900
Subject: [PATCH] feat(simple_planning_simulator): add initial twist for debug
 purpose (#2268)

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

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
---
 .../simple_planning_simulator_core.hpp            | 10 ++++++++++
 .../simple_planning_simulator_core.cpp            | 15 +++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index b9ae9d9804b5..a270be702fb4 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -42,6 +42,7 @@
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tier4_external_api_msgs/srv/initialize_pose.hpp"
 
@@ -79,6 +80,7 @@ using geometry_msgs::msg::PoseStamped;
 using geometry_msgs::msg::PoseWithCovarianceStamped;
 using geometry_msgs::msg::TransformStamped;
 using geometry_msgs::msg::Twist;
+using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::Odometry;
 using tier4_external_api_msgs::srv::InitializePose;
 
@@ -139,6 +141,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_ackermann_cmd_;
   rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_manual_ackermann_cmd_;
   rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_init_pose_;
+  rclcpp::Subscription<TwistStamped>::SharedPtr sub_init_twist_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
   rclcpp::Subscription<Engage>::SharedPtr sub_engage_;
 
@@ -159,6 +162,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_;
 
   /* received & published topics */
+  PoseWithCovarianceStamped::ConstSharedPtr initial_pose_;
+  TwistStamped initial_twist_;
   VelocityReport current_velocity_;
   Odometry current_odometry_;
   SteeringReport current_steer_;
@@ -223,6 +228,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg);
 
+  /**
+   * @brief set initial twist for simulation with received message
+   */
+  void on_initialtwist(const TwistStamped::ConstSharedPtr msg);
+
   /**
    * @brief set initial pose for simulation with received request
    */
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index 02c1924d5e89..d7d86c75364b 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -87,6 +87,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
 
   sub_init_pose_ = create_subscription<PoseWithCovarianceStamped>(
     "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1));
+  sub_init_twist_ = create_subscription<TwistStamped>(
+    "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1));
   sub_ackermann_cmd_ = create_subscription<AckermannControlCommand>(
     "input/ackermann_control_command", QoS{1},
     [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; });
@@ -312,6 +314,19 @@ void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::Co
   initial_pose.header = msg->header;
   initial_pose.pose = msg->pose.pose;
   set_initial_state_with_transform(initial_pose, initial_twist);
+
+  initial_pose_ = msg;
+}
+
+void SimplePlanningSimulator::on_initialtwist(const TwistStamped::ConstSharedPtr msg)
+{
+  if (!initial_pose_) return;
+
+  PoseStamped initial_pose;
+  initial_pose.header = initial_pose_->header;
+  initial_pose.pose = initial_pose_->pose.pose;
+  set_initial_state_with_transform(initial_pose, msg->twist);
+  initial_twist_ = *msg;
 }
 
 void SimplePlanningSimulator::on_set_pose(