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

feat: add pure pursuit debug #74

Merged
merged 1 commit into from
Sep 6, 2024
Merged

feat: add pure pursuit debug #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
5 changes: 4 additions & 1 deletion ...ichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <optional>
#include <rclcpp/rclcpp.hpp>
Expand All @@ -16,6 +17,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PointStamped;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;

Expand All @@ -30,7 +32,8 @@ class SimplePurePursuit : public rclcpp::Node {
// publishers
rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;
rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_raw_cmd_;

rclcpp::Publisher<PointStamped>::SharedPtr pub_lookahead_point_;

// timer
rclcpp::TimerBase::SharedPtr timer_;

Expand Down
9 changes: 9 additions & 0 deletions aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ SimplePurePursuit::SimplePurePursuit()
{
pub_cmd_ = create_publisher<AckermannControlCommand>("output/control_cmd", 1);
pub_raw_cmd_ = create_publisher<AckermannControlCommand>("output/raw_control_cmd", 1);
pub_lookahead_point_ = create_publisher<PointStamped>("/control/debug/lookahead_point", 1);

sub_kinematics_ = create_subscription<Odometry>(
"input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
Expand Down Expand Up @@ -103,6 +104,14 @@ void SimplePurePursuit::onTimer()
double lookahead_point_x = lookahead_point_itr->pose.position.x;
double lookahead_point_y = lookahead_point_itr->pose.position.y;

geometry_msgs::msg::PointStamped lookahead_point_msg;
lookahead_point_msg.header.stamp = get_clock()->now();
lookahead_point_msg.header.frame_id = "map";
lookahead_point_msg.point.x = lookahead_point_x;
lookahead_point_msg.point.y = lookahead_point_y;
lookahead_point_msg.point.z = 0;
pub_lookahead_point_->publish(lookahead_point_msg);

// calc steering angle for lateral control
double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) -
tf2::getYaw(odometry_->pose.pose.orientation);
Expand Down
45 changes: 30 additions & 15 deletions ...enge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,10 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- /System1
- /System1/TF1
- /System1/Grid1
- /Map1
- /Sensing1
- /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
- /Sensing1/GNSS1
- /Sensing1/GNSS1/PoseWithCovariance1
- /Sensing1/Odometry1/Shape1
- /Sensing1/BaseLink1
Expand All @@ -19,15 +16,14 @@ Panels:
- /Localization1
- /Localization1/EKF1/PoseHistory1
- /Planning1
- /Planning1/ScenarioPlanning1
- /Planning1/ScenarioPlanning1/ScenarioTrajectory1
- /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1
- /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Velocity1
- /Planning1/ScenarioPlanning1/LaneDriving1
- /MarkerArray1
- /Grid1
- /Control1
- /Control1/PointStamped1
Splitter Ratio: 0.557669460773468
Tree Height: 806
Tree Height: 357
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expand Down Expand Up @@ -1752,6 +1748,25 @@ Visualization Manager:
Name: Diagnostic
Enabled: true
Name: Planning
- Class: rviz_common/Group
Displays:
- Alpha: 1
Class: rviz_default_plugins/PointStamped
Color: 204; 41; 204
Enabled: true
History Length: 1
Name: PointStamped
Radius: 1
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /control/debug/lookahead_point
Value: true
Enabled: true
Name: Control
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Expand Down Expand Up @@ -1861,11 +1876,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 71.97769927978516
Scale: 41.36642837524414
Target Frame: base_link
Value: TopDownOrtho (rviz_default_plugins)
X: 3.45397686958313
Y: -1.1813154220581055
X: 4.791895866394043
Y: -8.544848442077637
Saved:
- Class: rviz_default_plugins/ThirdPersonFollower
Distance: 18
Expand Down Expand Up @@ -1911,18 +1926,18 @@ Window Geometry:
collapsed: false
Displays:
collapsed: false
Height: 2176
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
InitialPoseButtonPanel:
collapsed: false
QMainWindow State: 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
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 5364
X: 140
Y: 54
Width: 2490
X: 70
Y: 27