Skip to content

Commit

Permalink
Added traj_closed_loop #5 and some updates on #1 #2 #4
Browse files Browse the repository at this point in the history
  • Loading branch information
horverno committed May 17, 2023
1 parent 7232176 commit 4d721f6
Show file tree
Hide file tree
Showing 13 changed files with 225 additions and 32 deletions.
4 changes: 4 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
13 changes: 11 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
```



Expand Down
15 changes: 15 additions & 0 deletions include/wayp_plan_tools/common.hpp
Original file line number Diff line number Diff line change
@@ -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_
4 changes: 2 additions & 2 deletions launch/single_goal_pursuit.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
2 changes: 1 addition & 1 deletion launch/test.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
2 changes: 1 addition & 1 deletion launch/waypoint_loader.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
2 changes: 1 addition & 1 deletion launch/waypoint_saver.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
2 changes: 1 addition & 1 deletion launch/waypoint_to_target.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
121 changes: 121 additions & 0 deletions src/follow_the_carrot.cpp
Original file line number Diff line number Diff line change
@@ -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;
}
18 changes: 13 additions & 5 deletions src/multiple_goal_pursuit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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()
{
Expand All @@ -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)
Expand Down
1 change: 1 addition & 0 deletions src/single_goal_pursuit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
33 changes: 26 additions & 7 deletions src/stanley_control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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()
{
Expand All @@ -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)
Expand Down
Loading

0 comments on commit 4d721f6

Please sign in to comment.