From a6f6917bc542d5b533150f6abba086121e800974 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sun, 4 Dec 2022 23:59:16 +0900 Subject: [PATCH] feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 --- .../mpc_lateral_controller.hpp | 8 +++-- .../src/mpc_lateral_controller.cpp | 36 ++++++++++++++----- 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index d38312505fed7..94447bee91121 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -80,6 +80,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController rclcpp::Publisher::SharedPtr m_pub_predicted_traj; //!< @brief topic publisher for control debug values rclcpp::Publisher::SharedPtr m_pub_debug_values; + rclcpp::Publisher::SharedPtr m_pub_debug_marker; //!< @brief subscription for transform messages rclcpp::Subscription::SharedPtr m_tf_sub; rclcpp::Subscription::SharedPtr m_tf_static_sub; @@ -133,6 +134,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController double m_ego_nearest_dist_threshold; double m_ego_nearest_yaw_threshold; + // debug marker + visualization_msgs::msg::MarkerArray m_debug_marker_array; + //!< initialize timer to work in real, simulation, and replay void initTimer(double period_s); /** @@ -187,7 +191,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /** * @brief check ego car is in stopped state */ - bool isStoppedState() const; + bool isStoppedState(); /** * @brief check if the trajectory has valid value @@ -196,7 +200,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController bool isTrajectoryShapeChanged() const; - bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; + bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd); rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index a90fa0fd61da6..c0ec6e181bfe9 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -154,6 +154,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} "~/output/predicted_trajectory", 1); m_pub_debug_values = node_->create_publisher( "~/output/lateral_diagnostic", 1); + m_pub_debug_marker = + node_->create_publisher("~/output/debug_marker", 1); // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations declareMPCparameters(); @@ -180,6 +182,7 @@ boost::optional MpcLateralController::run() autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; autoware_auto_planning_msgs::msg::Trajectory predicted_traj; tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values; + m_debug_marker_array.markers.clear(); if (!m_is_ctrl_cmd_prev_initialized) { m_ctrl_cmd_prev = getInitialControlCommand(); @@ -207,7 +210,9 @@ boost::optional MpcLateralController::run() } // Use previous command value as previous raw steer command m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; - return createLateralOutput(m_ctrl_cmd_prev); + const auto output = createLateralOutput(m_ctrl_cmd_prev); + m_pub_debug_marker->publish(m_debug_marker_array); + return output; } if (!is_mpc_solved) { @@ -218,7 +223,9 @@ boost::optional MpcLateralController::run() } m_ctrl_cmd_prev = ctrl_cmd; - return createLateralOutput(ctrl_cmd); + const auto output = createLateralOutput(ctrl_cmd); + m_pub_debug_marker->publish(m_debug_marker_array); + return output; } void MpcLateralController::setInputData(InputData const & input_data) @@ -229,17 +236,30 @@ void MpcLateralController::setInputData(InputData const & input_data) } bool MpcLateralController::isSteerConverged( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const + const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) { + bool is_converged = false; + std::stringstream ss; // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) { - return false; + ss << "is trajectory shape changed (not converged)"; + } else { + is_converged = std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad); + ss << "cmd: " << cmd.steering_tire_angle + << " current: " << m_current_steering_ptr->steering_tire_angle + << (is_converged ? " (converged)" : " (not converged)"); } - const bool is_converged = - std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < - static_cast(m_converged_steer_rad); + // create debug marker for converged + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); + marker.pose = m_current_kinematic_state_ptr->pose.pose; + marker.text = ss.str(); + m_debug_marker_array.markers.push_back(marker); return is_converged; } @@ -338,7 +358,7 @@ MpcLateralController::getInitialControlCommand() const return cmd; } -bool MpcLateralController::isStoppedState() const +bool MpcLateralController::isStoppedState() { // If the nearest index is not found, return false if (m_current_trajectory_ptr->points.empty()) {