Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(trajectory_follower): pub steer converged marker #2441

Merged
merged 3 commits into from
Dec 12, 2022
Merged

feat(trajectory_follower): pub steer converged marker #2441

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
315d6ad
feat(trajectory_follower): pub steer converged marker
kosuke55 Dec 4, 2022
9b57497
Revert "feat(trajectory_follower): pub steer converged marker"
kosuke55 Dec 8, 2022
7c5cf1c
add steer converged debug marker in contoller_node
kosuke55 Dec 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions control/trajectory_follower/src/mpc_lateral_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -234,6 +234,7 @@ bool MpcLateralController::isSteerConverged(
// 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()) {
RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed");
Copy link
Contributor Author

@kosuke55 kosuke55 Dec 8, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

controller_node can not get this information, so just print debug message.

return false;
}

Expand Down
2 changes: 2 additions & 0 deletions control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
control_cmd_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;

enum class LateralControllerMode {
INVALID = 0,
Expand All @@ -106,6 +107,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
LongitudinalControllerMode getLongitudinalControllerMode(
const std::string & algorithm_name) const;
void publishDebugMarker() const;
};
} // namespace trajectory_follower_nodes
} // namespace control
Expand Down
30 changes: 30 additions & 0 deletions control/trajectory_follower_nodes/src/controller_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
"~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
"~/output/control_cmd", rclcpp::QoS{1}.transient_local());
debug_marker_pub_ =
create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", rclcpp::QoS{1});

// Timer
{
Expand Down Expand Up @@ -168,6 +170,34 @@ void Controller::callbackTimerControl()
out.lateral = lateral_output_->control_cmd;
out.longitudinal = longitudinal_output_->control_cmd;
control_cmd_pub_->publish(out);

publishDebugMarker();
}

void Controller::publishDebugMarker() const
{
visualization_msgs::msg::MarkerArray debug_marker_array{};

// steer converged marker
{
auto marker = tier4_autoware_utils::createDefaultMarker(
"map", this->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 = input_data_.current_odometry_ptr->pose.pose;

std::stringstream ss;
const double current = input_data_.current_steering_ptr->steering_tire_angle;
const double cmd = lateral_output_->control_cmd.steering_tire_angle;
const double diff = current - cmd;
ss << "current:" << current << " cmd:" << cmd << " diff:" << diff
<< (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)");
marker.text = ss.str();

debug_marker_array.markers.push_back(marker);
}

debug_marker_pub_->publish(debug_marker_array);
}

} // namespace trajectory_follower_nodes
Expand Down