From cfc8b67cc63e9ab5c9f49656fc7e9aca0a27e6d0 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 5 Jul 2022 17:56:11 +0900 Subject: [PATCH] feat(simple_planning_simulator): add acceleration publisher (#1214) * feat(simple_planning_simulator): add acceleration publisher Signed-off-by: Takamasa Horibe * add cov Signed-off-by: Takamasa Horibe --- .../simple_planning_simulator_core.hpp | 8 ++++++++ .../simple_planning_simulator.launch.py | 1 + .../simple_planning_simulator_core.cpp | 19 +++++++++++++++++++ 3 files changed, 28 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 3c84312bc00a5..f61359e59c3af 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 @@ -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::SharedPtr pub_velocity_; rclcpp::Publisher::SharedPtr pub_odom_; rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_acc_; rclcpp::Publisher::SharedPtr pub_control_mode_report_; rclcpp::Publisher::SharedPtr pub_gear_report_; rclcpp::Publisher::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 index aca22359b91cb..e9f705cc69db6 100644 --- 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/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 13f6e849b5be7..351bb53087dd0 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 @@ -127,6 +127,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_velocity_ = create_publisher("output/twist", QoS{1}); pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); + pub_acc_ = create_publisher("output/acceleration", QoS{1}); pub_tf_ = create_publisher("/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;