feat(control): improved pure pursuit control

aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/include/simple_pure_pursuit/simple_pure_pursuit.hpp

@@ -9,6 +9,8 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 
 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<AckermannControlCommand>::SharedPtr pub_cmd_;
+  rclcpp::Publisher<Marker>::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<rclcpp::Parameter> &parameters);
 };
 
 }  // namespace simple_pure_pursuit

aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp

@@ -25,6 +25,7 @@ SimplePurePursuit::SimplePurePursuit()
   external_target_vel_(declare_parameter<float>("external_target_vel", 0.0))
 {
   pub_cmd_ = create_publisher<AckermannControlCommand>("output/control_cmd", 1);
+  mkr_cmd_ = create_publisher<Marker>("debug/pursuit_lookahead", 1);
 
   sub_kinematics_ = create_subscription<Odometry>(
     "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<rclcpp::Parameter> &parameters){
+  auto result = rcl_interfaces::msg::SetParametersResult();
+  result.successful = true;
+
+  for (const auto &parameter : 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);

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