feat(simple_planning_simulator): add acceleration publisher (#1214)

* feat(simple_planning_simulator): add acceleration publisher Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add cov Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Jul 5, 2022 · cfc8b67 · cfc8b67
1 parent c45e597
commit cfc8b67
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 0 deletions.
diff --git a/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -35,6 +35,7 @@
 #include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp"
 #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
@@ -75,6 +76,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
 using autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
 using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
 using geometry_msgs::msg::PoseWithCovarianceStamped;
@@ -126,6 +128,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
   rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
   rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
+  rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
   rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
   rclcpp::Publisher<GearReport>::SharedPtr pub_gear_report_;
   rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
@@ -315,6 +318,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void publish_steering(const SteeringReport & steer);
 
+  /**
+   * @brief publish acceleration
+   */
+  void publish_acceleration();
+
   /**
    * @brief publish control_mode report
    */

diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -64,6 +64,7 @@ def launch_setup(context, *args, **kwargs):
             ("input/control_mode_request", "/system/control_mode_request"),
             ("output/twist", "/vehicle/status/velocity_status"),
             ("output/odometry", "/localization/kinematic_state"),
+            ("output/acceleration", "/localization/acceleration"),
             ("output/steering", "/vehicle/status/steering_status"),
             ("output/gear_report", "/vehicle/status/gear_status"),
             ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),

diff --git a/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -127,6 +127,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
   pub_velocity_ = create_publisher<VelocityReport>("output/twist", QoS{1});
   pub_odom_ = create_publisher<Odometry>("output/odometry", QoS{1});
   pub_steer_ = create_publisher<SteeringReport>("output/steering", QoS{1});
+  pub_acc_ = create_publisher<AccelWithCovarianceStamped>("output/acceleration", QoS{1});
   pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", QoS{1});
 
   /* set param callback */
@@ -293,6 +294,7 @@ void SimplePlanningSimulator::on_timer()
   publish_odometry(current_odometry_);
   publish_velocity(current_velocity_);
   publish_steering(current_steer_);
+  publish_acceleration();
 
   publish_control_mode_report();
   publish_gear_report();
@@ -518,6 +520,23 @@ void SimplePlanningSimulator::publish_steering(const SteeringReport & steer)
   pub_steer_->publish(msg);
 }
 
+void SimplePlanningSimulator::publish_acceleration()
+{
+  AccelWithCovarianceStamped msg;
+  msg.header.frame_id = "/base_link";
+  msg.header.stamp = get_clock()->now();
+  msg.accel.accel.linear.x = vehicle_model_ptr_->getAx();
+
+  constexpr auto COV = 0.001;
+  msg.accel.covariance.at(6 * 0 + 0) = COV;  // linear x
+  msg.accel.covariance.at(6 * 1 + 1) = COV;  // linear y
+  msg.accel.covariance.at(6 * 2 + 2) = COV;  // linear z
+  msg.accel.covariance.at(6 * 3 + 3) = COV;  // angular x
+  msg.accel.covariance.at(6 * 4 + 4) = COV;  // angular y
+  msg.accel.covariance.at(6 * 5 + 5) = COV;  // angular z
+  pub_acc_->publish(msg);
+}
+
 void SimplePlanningSimulator::publish_control_mode_report()
 {
   ControlModeReport msg;