From 9b57497f48e2c3d596f28eee5f1d55ab71b6b780 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 8 Dec 2022 20:53:30 +0900 Subject: [PATCH] Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917bc542d5b533150f6abba086121e800974. Signed-off-by: kosuke55 --- .../mpc_lateral_controller.hpp | 8 ++--- .../src/mpc_lateral_controller.cpp | 36 +++++-------------- 2 files changed, 10 insertions(+), 34 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 94447bee91121..d38312505fed7 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -80,7 +80,6 @@ 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; @@ -134,9 +133,6 @@ 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); /** @@ -191,7 +187,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController /** * @brief check ego car is in stopped state */ - bool isStoppedState(); + bool isStoppedState() const; /** * @brief check if the trajectory has valid value @@ -200,7 +196,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController bool isTrajectoryShapeChanged() const; - bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd); + bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; 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 c0ec6e181bfe9..a90fa0fd61da6 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -154,8 +154,6 @@ 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(); @@ -182,7 +180,6 @@ 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(); @@ -210,9 +207,7 @@ 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; - const auto output = createLateralOutput(m_ctrl_cmd_prev); - m_pub_debug_marker->publish(m_debug_marker_array); - return output; + return createLateralOutput(m_ctrl_cmd_prev); } if (!is_mpc_solved) { @@ -223,9 +218,7 @@ boost::optional MpcLateralController::run() } m_ctrl_cmd_prev = ctrl_cmd; - const auto output = createLateralOutput(ctrl_cmd); - m_pub_debug_marker->publish(m_debug_marker_array); - return output; + return createLateralOutput(ctrl_cmd); } void MpcLateralController::setInputData(InputData const & input_data) @@ -236,30 +229,17 @@ void MpcLateralController::setInputData(InputData const & input_data) } bool MpcLateralController::isSteerConverged( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) + const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const { - 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()) { - 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)"); + return false; } - // 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); + const bool is_converged = + std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad); return is_converged; } @@ -358,7 +338,7 @@ MpcLateralController::getInitialControlCommand() const return cmd; } -bool MpcLateralController::isStoppedState() +bool MpcLateralController::isStoppedState() const { // If the nearest index is not found, return false if (m_current_trajectory_ptr->points.empty()) {