jkk-research / wayp_plan_tools

🏎 Waypoint and planner tools for ROS 2 with minimal dependencies.

wayp_plan_tools ROS 2 package

Waypoint and planner tools for ROS 2 with minimal dependencies.

Planner / control nodes:

• single_goal_pursuit: Pure pursuit (for vehicles / robots), a simple cross-track error method
• multiple_goal_pursuit: Multiple goal pursuit for vehicles / robots an implementation of our paper
• stanley_control: Stanley controller, a heading error + cross-track error method
• follow_the_carrot: Follow-the-carrot, the simplest controller

Waypoint nodes:

• waypoint_saver: saves the waypoints to a csv
• waypoint_loader: loads the waypoints from a csv to a ROS 2 topic
• waypoint_to_target: translates the global waypoint array to local target waypoint(s), thus it makes possible to use the controller nodes in a standalone way

Build

It is assumed that the workspace is ~/ros2_ws/.

Terminal 1 🔴 clone

cd ~/ros2_ws/src

git clone https://github.com/jkk-research/wayp_plan_tools

Terminal 1 🔴 build

cd ~/ros2_ws

colcon build --packages-select wayp_plan_tools --symlink-install

Terminal 2 🔵 run

source ~/ros2_ws/install/setup.bash

ros2 launch wayp_plan_tools waypoint_saver.launch.py

Control nodes

In this project the single_goal_pursuit, the multiple_goal_pursuit, stanley_control and the follow_the_carrot controllers are implemented. A high level overview about the algorithms is visible on the following figure:

single_goal_pursuit node

The "classic" pure pursuit implementation

multiple_goal_pursuit node

Multiple goal pursuit for vehicles / robots, an implementation of our paper

follow_the_carrot node

Follow-the-carrot, the simplest controller

waypoint_to_target node

Reads the waypoint array and speeds, from that it creates single or multiple goal points.

It also provides a /metrics_wayp array topic with the following elements:

Array element	Meaning	Const
[0]	current lateral distance to the waypoint (signed, cross-track error)	CUR_LAT_DIST_SIGNED
[1]	current lateral distance to the waypoint (absolute value)	CUR_LAT_DIST_ABS
[2]	average lateral distance over time	AVG_LAT_DISTANCE
[3]	maximum lateral distance over time	MAX_LAT_DISTANCE
[4]	current waypoint ID	CUR_WAYPOINT_ID
[5]	target waypoint ID	TRG_WAYPOINT_ID
[6]	target waypoint longitudinal distance	TRG_WAY_LON_DIST

waypoint_saver node

Saves the waypoints to a csv. Important parameters are file_name and file_dir. Set from terminal or from a launch file

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. Set from terminal or from a launch file

ros2 run wayp_plan_tools waypoint_loader --ros-args -p file_name:=tmp1.csv -p file_dir:=/mnt/bag/waypoints

Usage with a simulator

Please refer to github.com/jkk-research/sim_wayp_plan_tools.

Cite & paper

If you use any of this code please consider citing the paper:

@Article{horvath2020multigoalpursuit, 
    title={Theoretical background and application of multiple goal pursuit trajectory follower}, 
    volume={48}, 
    url={https://hjic.mk.uni-pannon.hu/index.php/hjic/article/view/914}, 
    DOI={10.33927/hjic-2020-03}, 
    number={1}, 
    journal={Hungarian Journal of Industry and Chemistry}, 
    author={Horváth, Ernő and Pozna, Claudiu and Kőrös, Péter and Hajdu, Csaba and Ballagi, Áron}, 
    year={2020}, 
    month={Jul.}, 
    pages={11–17} 
}

wayp_plan_tools's Issues

Make the package independent

Remove Lexus3 hardcoded constants, params and other artifacts

Follow the carrot controller for reference

Although, pure pursuit and Stanley are in many regards better controller it is worth to implement a simple method as a reference.

Stops in equidistant distance

Possibility to add stops in equidistant distance at the waypoint following.

Use a standard `ROS 2` way of file access in package instead of `colcon_cd`

wayp_plan_tools/launch/waypoint_loader.launch.py

Line 8 in 3c7bfef

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()

Also in https://github.com/jkk-research/sim_wayp_plan_tools

Simple obstacle avoidance

based on @umiklos work: https://github.com/umiklos/obstacle_avoidance

Param for m/s or km/h marker

Display RVIZ2 markers either in km/h (default) or m/s.

wayp_plan_tools/src/waypoint_loader.cpp

Line 270 in 1720b85

stream << std::fixed << std::setprecision(1) << speeds[id] * 3.6 << " km/h";

Trajectory loop closure bool

Add a boolean and detect whether trajectory is linear (loop closure false) circular (loop closure true).

If false, stop after last waypoint reached

wayp_plan_tools/src/waypoint_to_target.cpp

Lines 228 to 236 in 7232176

	if (target_waypoint >= int(msg.poses.size() - 1))
	{
	if (last_waypoint_reached == false)
	{
	last_waypoint_reached_time = this->now();
	}
	RCLCPP_INFO_STREAM(this->get_logger(), "Last waypoint reached " << std::fixed << std::setprecision(1) << (last_waypoint_reached_time - this->now()).nanoseconds() / -1e9 << "s ago");
	last_waypoint_reached = true;
	}

If true, repeat it

wayp_plan_tools/src/waypoint_to_target.cpp

Lines 169 to 172 in 7232176

	if(closest_waypoint > int(msg.poses.size()) - 10)
	{
	closest_waypoint = 0;
	}

Stopping logic

Related to #16

Add `MAX_LAT_DISTANCE`

Add maximum lateral distance over time to the /metrics_wayp array

Read waypoint file once / at reinit

Read waypoint file once or at reinit. Currently loadWaypointsForVer3 and verifyFileConsistency is in a loop.

wayp_plan_tools/src/waypoint_loader.cpp

Lines 220 to 232 in 1720b85

	if (!verifyFileConsistency(file_path.c_str()))
	{
	RCLCPP_DEBUG_STREAM(this->get_logger(), "lane data is something wrong...");
	return;
	}
	RCLCPP_DEBUG_STREAM(this->get_logger(), "lane data is valid. publishing...");
	std::vector<geometry_msgs::msg::Pose> wps;
	std::vector<float> speeds;
	std_msgs::msg::Float32MultiArray speed_array;
	geometry_msgs::msg::PoseArray wp_array;
	visualization_msgs::msg::MarkerArray mark_array;
	loadWaypointsForVer3(file_path.c_str(), &wps, &speeds);

Stanley controller

Interpolation between the waypoints

• Position
• Orientation
• Speed
• Test in simulation
• Test in real-world

Add closest waypoint marker

Maybe MarkerArray instead of Marker

wayp_plan_tools/src/waypoint_to_target.cpp

Line 98 in 6387dd8

goal_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("lexus3/pursuittarget_marker", 10);

/cmd_vel angular in radians/second

geometry_msgs/Twist.angular.z should be in radians/second

wayp_plan_tools/src/single_goal_pursuit.cpp

Line 54 in 951bf4b

steering_angle *= 14.8; // TODO: move to driver, geometry_msgs/Twist.angular.z should be in radians/second

https://github.com/jkk-research/lexus_bringup/blob/d4fd2aac0ced32390d1521308ba67d82aaadd4f6/src/speed_control.cpp#L181-L182

Multiple goal pursuit controller

Fix `CUR_LAT_DIST_SIGNED`

The distance is OK, the + / - sign is sometimes wrong

Multiple waypoints (lanes)

Read multiple waypoints (lanes) in waypoint_loader.cpp

This is already prepared with a vector of strings, currently only one is used:

wayp_plan_tools/src/waypoint_loader.cpp

Line 298 in 1720b85

std::vector<std::string> multi_file_path_;