From bbf14505aaf83bbda90645c912d69cfe93be0c2f Mon Sep 17 00:00:00 2001 From: Shotaro Itahara Date: Wed, 7 Aug 2024 22:27:56 +0900 Subject: [PATCH 1/4] =?UTF-8?q?feat:=20pure=5Fpursuit=E3=81=AE=E8=A6=8B?= =?UTF-8?q?=E3=81=A6=E3=81=84=E3=82=8B=E7=82=B9=E3=82=92rviz2=E3=81=A7?= =?UTF-8?q?=E5=8F=AF=E8=A6=96=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../simple_pure_pursuit.hpp | 18 ++++--- .../src/simple_pure_pursuit.cpp | 51 ++++++++++++++++++- 2 files changed, 62 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp index e9433945..8d82262d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp @@ -9,6 +9,8 @@ #include #include #include +#include +#include namespace simple_pure_pursuit { @@ -18,6 +20,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using visualization_msgs::msg::Marker; class SimplePurePursuit : public rclcpp::Node { public: @@ -29,6 +32,7 @@ class SimplePurePursuit : public rclcpp::Node { // publishers rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr mkr_cmd_; // timer rclcpp::TimerBase::SharedPtr timer_; @@ -40,17 +44,19 @@ class SimplePurePursuit : public rclcpp::Node { // pure pursuit parameters - const double wheel_base_; - const double lookahead_gain_; - const double lookahead_min_distance_; - const double speed_proportional_gain_; - const bool use_external_target_vel_; - const double external_target_vel_; + double wheel_base_; + double lookahead_gain_; + double lookahead_min_distance_; + double speed_proportional_gain_; + bool use_external_target_vel_; + double external_target_vel_; + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; private: void onTimer(); bool subscribeMessageAvailable(); + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); }; } // namespace simple_pure_pursuit diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index aa3ac2b1..736185e1 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -25,6 +25,7 @@ SimplePurePursuit::SimplePurePursuit() external_target_vel_(declare_parameter("external_target_vel", 0.0)) { pub_cmd_ = create_publisher("output/control_cmd", 1); + mkr_cmd_ = create_publisher("debug/pursuit_lookahead", 1); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); @@ -34,6 +35,10 @@ SimplePurePursuit::SimplePurePursuit() using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 30ms, std::bind(&SimplePurePursuit::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&SimplePurePursuit::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = SimplePurePursuit::add_on_set_parameters_callback(parameter_change_cb); } AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp) @@ -100,7 +105,31 @@ void SimplePurePursuit::onTimer() } double lookahead_point_x = lookahead_point_itr->pose.position.x; double lookahead_point_y = lookahead_point_itr->pose.position.y; - + { + // publish lookahead point marker + auto marker_msg = Marker(); + marker_msg.header.frame_id = "map"; + marker_msg.header.stamp = now(); + marker_msg.ns = "basic_shapes"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::SPHERE; + marker_msg.action = visualization_msgs::msg::Marker::ADD; + marker_msg.pose.position.x = lookahead_point_x; + marker_msg.pose.position.y = lookahead_point_y; + marker_msg.pose.position.z = 80; + marker_msg.pose.orientation.x = 0.0; + marker_msg.pose.orientation.y = 0.0; + marker_msg.pose.orientation.z = 0.0; + marker_msg.pose.orientation.w = 1.0; + marker_msg.scale.x = 3.0; + marker_msg.scale.y = 3.0; + marker_msg.scale.z = 3.0; + marker_msg.color.r = 1.0f; + marker_msg.color.g = 0.0f; + marker_msg.color.b = 0.0f; + marker_msg.color.a = 1.0; + mkr_cmd_->publish(marker_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); @@ -122,8 +151,28 @@ bool SimplePurePursuit::subscribeMessageAvailable() } return true; } +rcl_interfaces::msg::SetParametersResult SimplePurePursuit::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "lookahead_gain") { + lookahead_gain_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "lookahead_gain changed to %f", lookahead_gain_); + } else if (parameter.get_name() == "lookahead_min_distance") { + lookahead_min_distance_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "lookahead_min_distance changed to %f", lookahead_min_distance_); + } else if (parameter.get_name() == "external_target_vel") { + external_target_vel_ = parameter.as_double(); + RCLCPP_INFO(SimplePurePursuit::get_logger(), "external_target_vel changed to %f", external_target_vel_); + } + } + return result; +} } // namespace simple_pure_pursuit + + int main(int argc, char const * argv[]) { rclcpp::init(argc, argv); From 6e45cedfae7c0098eb00af5c35045299019ab2a9 Mon Sep 17 00:00:00 2001 From: Shotaro Itahara Date: Sun, 11 Aug 2024 19:30:07 +0900 Subject: [PATCH 2/4] =?UTF-8?q?pure=5Fpursuit=E3=81=AE=E3=83=91=E3=83=A9?= =?UTF-8?q?=E3=83=A1=E3=82=BF=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../aichallenge_submit_launch/launch/reference.launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index 8e2fe222..c8e8180c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -205,9 +205,9 @@ - - - + + + From aa3889570a9a0a55fcdf844714e43425782642d8 Mon Sep 17 00:00:00 2001 From: Shotaro Itahara Date: Sun, 11 Aug 2024 19:46:33 +0900 Subject: [PATCH 3/4] feat: added rviz2 display setting --- .../aichallenge_system_launch/config/autoware.rviz | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz index c76a8956..1539328f 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz @@ -1769,6 +1769,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /aichallenge/pitstop/area_marker Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /debug/pursuit_lookahead + Value: true Enabled: true Global Options: Background Color: 10; 10; 10 From 469fc8a74e9373332771b1fd570c538b529cfcf3 Mon Sep 17 00:00:00 2001 From: Shotaro Itahara Date: Mon, 12 Aug 2024 19:36:49 +0900 Subject: [PATCH 4/4] =?UTF-8?q?Revert=20"pure=5Fpursuit=E3=81=AE=E3=83=91?= =?UTF-8?q?=E3=83=A9=E3=83=A1=E3=82=BF=E8=AA=BF=E6=95=B4"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 6e45cedfae7c0098eb00af5c35045299019ab2a9. --- .../aichallenge_submit_launch/launch/reference.launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index c8e8180c..8e2fe222 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -205,9 +205,9 @@ - - - + + +