Added traj_closed_loop #5 and some updates on #1 #2 #4

CMakeLists.txt

@@ -35,6 +35,10 @@ set(ament_dependencies
   tf2_geometry_msgs  
 )
 
+include_directories(
+  include
+)
+
 add_executable(waypoint_saver src/waypoint_saver.cpp)
 ament_target_dependencies(waypoint_saver  ${ament_dependencies} )
 add_executable(waypoint_loader src/waypoint_loader.cpp)

README.md

@@ -56,9 +56,18 @@ It also provides a `/metrics_wayp` array topic with the following  elements:
 <img src="csv/lookahead01.svg" width=60% />
 
 # `waypoint_saver` node 
-Saves the waypoints to a csv.
+Saves the waypoints to a csv. Important parameters are `file_name` and `file_dir`. Set from terminal or from a [launch file](launch/waypoint_saver.launch.py)
+
+``` py
+ros2 run wayp_plan_tools waypoint_saver --ros-args -p file_name:=tmp1.csv -p file_dir:=/mnt/bag/waypoints
+```
+
 # `waypoint_loader` node
-Loads the waypoints from a csv to a ROS 2 topic.
+Loads the waypoints from a csv to a ROS 2 topic. Set from terminal or from a [launch file](launch/waypoint_loader.launch.py)
+
+``` py
+ros2 run wayp_plan_tools waypoint_loader --ros-args -p file_name:=tmp1.csv -p file_dir:=/mnt/bag/waypoints
+```
 
 
 

include/wayp_plan_tools/common.hpp

@@ -0,0 +1,15 @@
+#ifndef COMMON_HPP_
+#define COMMON_HPP_
+
+//#include <iostream>
+//#include "rclcpp/rclcpp.hpp"
+//#include "geometry_msgs/msg/pose.hpp"
+//#include "geometry_msgs/msg/pose_stamped.hpp"
+//#include "geometry_msgs/msg/pose_array.hpp"
+
+namespace common_wpt
+{
+
+} // namespace common_wpt
+
+#endif  // COMMON_HPP_

launch/single_goal_pursuit.launch.py

@@ -4,8 +4,8 @@
 
 def generate_launch_description():
 
-    pkg_name = 'wayp_plan_tools'
-    pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+    #pkg_name = 'wayp_plan_tools'
+    #pkg_dir = os.popen('/bin/bash -c "cd && source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
     #print(pkg_dir)
 
     return LaunchDescription([

launch/test.launch.py

@@ -5,7 +5,7 @@
 def generate_launch_description():
 
     pkg_name = 'wayp_plan_tools'
-    pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+    pkg_dir = os.popen('/bin/bash -c "cd && source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
     #print(pkg_dir)
 
     return LaunchDescription([

launch/waypoint_loader.launch.py

@@ -5,7 +5,7 @@
 def generate_launch_description():
 
     pkg_name = 'wayp_plan_tools'
-    pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+    pkg_dir = os.popen('/bin/bash -c "cd && source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
     #print(pkg_dir)
 
     return LaunchDescription([

launch/waypoint_saver.launch.py

@@ -5,7 +5,7 @@
 def generate_launch_description():
 
     pkg_name = 'wayp_plan_tools'
-    pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+    pkg_dir = os.popen('/bin/bash -c "cd && source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
     #print(pkg_dir)
 
     return LaunchDescription([

launch/waypoint_to_target.launch.py

@@ -5,7 +5,7 @@
 def generate_launch_description():
 
     #pkg_name = 'wayp_plan_tools'
-    #pkg_dir = os.popen('/bin/bash -c "source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
+    #pkg_dir = os.popen('/bin/bash -c "cd && source /usr/share/colcon_cd/function/colcon_cd.sh && colcon_cd %s && pwd"' % pkg_name).read().strip()
     #print(pkg_dir)
 
     return LaunchDescription([

src/follow_the_carrot.cpp

@@ -0,0 +1,121 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <math.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/color_rgba.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_array.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+//#include "wayp_plan_tools/common.hpp"
+
+std::string waypoint_topic = "/lexus3/pursuitgoal";
+geometry_msgs::msg::Twist pursuit_vel;
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+class FollowTheCarrot : public rclcpp::Node
+{
+  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    result.reason = "success";
+    for (const auto &param : parameters)
+    {
+      RCLCPP_INFO_STREAM(this->get_logger(), "Param update: " << param.get_name().c_str() << ": " << param.value_to_string().c_str());
+      if (param.get_name() == "waypoint_topic")
+      {
+        waypoint_topic = param.as_string();
+        sub_w_ = this->create_subscription<geometry_msgs::msg::PoseArray>(waypoint_topic, 10, std::bind(&FollowTheCarrot::waypointCallback, this, _1));
+      }
+      if (param.get_name() == "wheelbase")
+      {
+        wheelbase = param.as_double();
+      }
+    }
+    return result;
+  }
+
+public:
+  FollowTheCarrot() : Node("follow_the_carrot_node")
+  {
+    RCLCPP_INFO_STREAM(this->get_logger(), "follow_the_carrot_node started: ");
+    this->declare_parameter<std::string>("waypoint_topic", "");
+    this->declare_parameter<float>("wheelbase", wheelbase);
+    this->get_parameter("waypoint_topic", waypoint_topic);
+    this->get_parameter("wheelbase", wheelbase);
+
+    goal_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/lexus3/cmd_vel", 10);
+    reinit_pub_ = this->create_publisher<std_msgs::msg::Bool>("/lexus3/control_reinit", 10);
+    sub_w_ = this->create_subscription<geometry_msgs::msg::PoseArray>(waypoint_topic, 10, std::bind(&FollowTheCarrot::waypointCallback, this, _1));
+    sub_s_ = this->create_subscription<std_msgs::msg::Float32>("/lexus3/pursuitspeedtarget", 10, std::bind(&FollowTheCarrot::speedCallback, this, _1));
+    timer_ = this->create_wall_timer(50ms, std::bind(&FollowTheCarrot::timerLoop, this));
+    callback_handle_ = this->add_on_set_parameters_callback(std::bind(&FollowTheCarrot::parametersCallback, this, std::placeholders::_1));
+    std::this_thread::sleep_for(600ms);
+    reinitControl();
+    std::this_thread::sleep_for(600ms);
+    reinitControl();
+  }
+
+private:
+  // simple direction as a steering angle
+  float calcFollowTheCarrotAngle(float goal_x, float goal_y) const
+  {
+    // calculate the angle between 0,0 to goal_x, goal_y in radians
+    float steering_angle = atan2(goal_y, goal_x);
+    return steering_angle;
+  }
+
+  void speedCallback(const std_msgs::msg::Float32 &msg) const
+  {
+    pursuit_vel.linear.x = msg.data;
+  }
+
+  void waypointCallback(const geometry_msgs::msg::PoseArray &msg) const
+  {
+    pursuit_vel.angular.z = calcFollowTheCarrotAngle(msg.poses[0].position.x, msg.poses[0].position.y);
+  }
+  void timerLoop()
+  {
+    // RCLCPP_INFO_STREAM(this->get_logger(), "timer");
+    goal_pub_->publish(pursuit_vel);
+  }
+  void reinitControl()
+  {
+    RCLCPP_INFO_STREAM(this->get_logger(), "reinitControl");
+    std_msgs::msg::Bool reinit;
+    reinit.data = true;
+    reinit_pub_->publish(reinit);
+  }
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr sub_w_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_s_;
+  // parameters
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr goal_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reinit_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  float wheelbase = 2.789; // Lexus3
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+};
+
+int main(int argc, char **argv)
+{
+
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<FollowTheCarrot>());
+  rclcpp::shutdown();
+  return 0;
+}

src/multiple_goal_pursuit.cpp

@@ -68,8 +68,7 @@ class MultipleGoalPursuit : public rclcpp::Node
   }
 
 private:
-  // multiple pursuit steering angle calc
-  // TODO
+  // a single pursuit steering angle calc as an instance of multiple pursuit
   float calcPursuitAngle(float goal_x, float goal_y) const
   {
     float alpha = atan2(goal_y, goal_x);
@@ -83,9 +82,17 @@ class MultipleGoalPursuit : public rclcpp::Node
     pursuit_vel.linear.x = msg.data;
   }
 
-  void waypointCallback(const geometry_msgs::msg::PoseArray &msg) const
+  void waypointCallback(const geometry_msgs::msg::PoseArray &msg)
   {
-    pursuit_vel.angular.z = calcPursuitAngle(msg.poses[0].position.x, msg.poses[0].position.y);
+    // determine the size of the array, the number of goal points
+    goalpoint_count = msg.poses.size();
+    float sum_angle = 0;
+    for(int i = 0; i < goalpoint_count; i++)
+    {
+      sum_angle += calcPursuitAngle(msg.poses[i].position.x, msg.poses[i].position.y);
+    }
+    // TODO: this is a simple average of the angles, but the original paper uses optimization to find the best angle
+    pursuit_vel.angular.z = sum_angle / goalpoint_count;
   }
   void timerLoop()
   {
@@ -107,7 +114,8 @@ class MultipleGoalPursuit : public rclcpp::Node
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reinit_pub_;
   rclcpp::TimerBase::SharedPtr timer_;
   float wheelbase = 2.789; // Lexus3
-  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+  int goalpoint_count = 1; // number of goal points
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_; 
 };
 
 int main(int argc, char **argv)

src/single_goal_pursuit.cpp

@@ -18,6 +18,7 @@
 
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2/LinearMath/Matrix3x3.h"
+//#include "wayp_plan_tools/common.hpp"
 
 std::string waypoint_topic = "/lexus3/pursuitgoal";
 geometry_msgs::msg::Twist pursuit_vel;

src/stanley_control.cpp

@@ -44,6 +44,10 @@ class StanleyControl : public rclcpp::Node
       {
         wheelbase = param.as_double();
       }
+      if(param.get_name() == "heading_err_rate")
+      {
+        heading_err_rate = param.as_double();
+      }
     }
     return result;
   }
@@ -54,8 +58,10 @@ class StanleyControl : public rclcpp::Node
     RCLCPP_INFO_STREAM(this->get_logger(), "stanley_control_node started: ");
     this->declare_parameter<std::string>("waypoint_topic", "");
     this->declare_parameter<float>("wheelbase", wheelbase);
+    this->declare_parameter<float>("heading_err_rate", heading_err_rate);
     this->get_parameter("waypoint_topic", waypoint_topic);
     this->get_parameter("wheelbase", wheelbase);
+    this->get_parameter("heading_err_rate", heading_err_rate);
 
     goal_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/lexus3/cmd_vel", 10);
     reinit_pub_ = this->create_publisher<std_msgs::msg::Bool>("/lexus3/control_reinit", 10);
@@ -68,15 +74,19 @@ class StanleyControl : public rclcpp::Node
   }
 
 private:
-  // TODO:
-  // heading error
-  // cross-track error
-  float calcStanley(float goal_x, float goal_y, float goal_yaw, float current_yaw) const
+  // TODO: field test
+  // This approach has 2 modifications compared to the original stanley controller
+  // 1. The cross-track error is calculated based on the lookahead distance, not the closest waypoint distance
+  // 2. Instead of the front axle, the rear axle is used as the reference point (the same as the pure pursuit controller) 
+  float calcStanley(float goal_x, float goal_y, float goal_yaw) const
   {
     float alpha = atan2(goal_y, goal_x);
     float lookahead_distance = sqrt(pow(goal_x, 2) + pow(goal_y, 2));
-    float steering_angle = atan2(2.0 * wheelbase * sin(alpha) / (lookahead_distance), 1);
-    return steering_angle;
+    // cross-track error
+    float cross_track_error = atan2(2.0 * wheelbase * sin(alpha) / (lookahead_distance), 1);
+    // heading error
+    float heading_error = goal_yaw * heading_err_rate;
+    return cross_track_error + heading_error;
   }
 
   void speedCallback(const std_msgs::msg::Float32 &msg) const
@@ -86,7 +96,15 @@ class StanleyControl : public rclcpp::Node
 
   void waypointCallback(const geometry_msgs::msg::PoseArray &msg) const
   {
-    pursuit_vel.angular.z = calcStanley(msg.poses[0].position.x, msg.poses[0].position.y, 0, 0);
+    tf2::Quaternion q(
+        msg.poses[0].orientation.x,
+        msg.poses[0].orientation.y,
+        msg.poses[0].orientation.z,
+        msg.poses[0].orientation.w);
+    tf2::Matrix3x3 m(q);
+    double goal_roll, goal_pitch, goal_yaw;
+    m.getRPY(goal_roll, goal_pitch, goal_yaw);
+    pursuit_vel.angular.z = calcStanley(msg.poses[0].position.x, msg.poses[0].position.y, goal_yaw);
   }
   void timerLoop()
   {
@@ -109,6 +127,7 @@ class StanleyControl : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
   float wheelbase = 2.789; // Lexus3
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+  float heading_err_rate = 1.0; // TODO: tune
 };
 
 int main(int argc, char **argv)