diff --git a/control/vehicle_cmd_gate/src/marker_helper.hpp b/control/vehicle_cmd_gate/src/marker_helper.hpp index 44c43a3630151..7b968a858ccdb 100644 --- a/control/vehicle_cmd_gate/src/marker_helper.hpp +++ b/control/vehicle_cmd_gate/src/marker_helper.hpp @@ -106,6 +106,30 @@ inline visualization_msgs::msg::Marker createStringMarker( return marker; } +inline visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + inline void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 967f207004a58..43b66b8730ccc 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -82,6 +82,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) filter_activated_flag_pub_ = create_publisher("~/is_filter_activated/flag", durable_qos); + debug_marker_pub_ = create_publisher( + "~/is_steering_converged/debug_marker", rclcpp::QoS{1}); + // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, @@ -844,8 +847,36 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated) } bool VehicleCmdGate::isSteeringConverged(AckermannLateralCommand & lateral_cmd) { - return std::fabs(lateral_cmd.steering_tire_angle - current_steer_) < - steering_convergence_threshold_; + const bool is_converged = + std::fabs(lateral_cmd.steering_tire_angle - current_steer_) < steering_convergence_threshold_; + publishConvergenceDebugMarker(lateral_cmd, is_converged); + return is_converged; +} + +void VehicleCmdGate::publishConvergenceDebugMarker( + const AckermannLateralCommand & lat_cmd, const bool is_converged) const +{ + visualization_msgs::msg::MarkerArray debug_marker_array{}; + + // steer converged marker + { + auto marker = createDefaultMarker( + "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.99)); + marker.pose = current_kinematics_.pose.pose; + + std::stringstream ss; + const double current = current_steer_; + const double cmd = lat_cmd.steering_tire_angle; + const double diff = current - cmd; + ss << "current:" << current << " cmd:" << cmd << " diff:" << diff + << (is_converged ? " (converged)" : " (not converged)"); + marker.text = ss.str(); + + debug_marker_array.markers.push_back(marker); + } + + debug_marker_pub_->publish(debug_marker_array); } } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 20a9fc8b7cdeb..0d6d6bd48f214 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -111,6 +111,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; @@ -249,7 +250,8 @@ class VehicleCmdGate : public rclcpp::Node // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); - + void publishConvergenceDebugMarker( + const AckermannLateralCommand & lat_cmd, const bool is_converged) const; std::unique_ptr logger_configure_; };