Improving behavior of the planner to follow peak points of the global plan strictly. #39
Conversation
Added a peakViaPoint( ) so that the local planner strictly follows the peak points of the global plan.
added as a requirement of teb_local_planner_ros.cpp
There was a problem hiding this comment.
Thank you very much for the proposed changes.
I like the idea to add just significant viapoints instead of using the current naive uniform grid.
However, I have some comments that I would like you to address or discuss before merging.
A general discussion: is it sufficient for most scenarios to find just one peak point or should we try to find all local maxima and local mimima of the curvature of the transformed plan to have all peak points?
src/teb_local_planner_ros.cpp
Outdated
| @@ -311,6 +346,9 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) | |||
| // update via-points container | |||
| updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); | |||
|
|
|||
| //@mudit edit : creating a via point for including peak points of the global plan in the robot's path | |||
| peakViaPoint(robot_pose_.x(),robot_pose_.y(),robot_goal_.x(),robot_goal_.y(),transformed_plan); | |||
There was a problem hiding this comment.
If viapoints are activated, your method peakViaPoint(...) is called after updateViaPointsContainer(...) has already collected viapoints from the global plan. Hence the final list of viapoints is not in a strict order anymore. However, for some efficiency improvements later, we would like to retain the strict order. Could you try to migrate the peak-point computation into updateViaPointsContainer? Or alternatively: we could switch between: "No viapoints, uniform viapoints and peak viapoints.
src/teb_local_planner_ros.cpp
Outdated
| void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, | ||
| std::vector<geometry_msgs::PoseStamped>& transformed_plan) //@mudit | ||
| { | ||
| double planLineSlope= (localGoal_y-robot_y)/(localGoal_x-robot_x); // slope of the line connecting robot pose and local goal |
There was a problem hiding this comment.
we should probably make sure that (localGoal_x-robot_x)!=0
src/teb_local_planner_ros.cpp
Outdated
| @@ -206,6 +206,41 @@ bool TebLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& | |||
| return true; | |||
| } | |||
|
|
|||
| void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, | |||
| std::vector<geometry_msgs::PoseStamped>& transformed_plan) //@mudit | |||
| { | |||
There was a problem hiding this comment.
Related to the complete method:
Parameters: mix of camalCase and lowercase_underderscore style. Please use the latter one acording to ROS C++ styleguide ;-)
Please also check indentation and insert a new line after braces "{".
There was a problem hiding this comment.
I have made changes according to ROS styleguide.
src/teb_local_planner_ros.cpp
Outdated
| } | ||
| else | ||
|
|
||
| cfg_.hcp.max_number_classes=4; // ROS_INFO("Number : %d",cfg_.hcp.max_number_classes); // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y |
There was a problem hiding this comment.
It is not intended to change the general config class inside the planner.
I would suggest to avoid accessing max_number_classes here generally.
|
Hello, Local planner should try to find all local maxima and local mimima of the curvature of the transformed plan to have all peak points. All these peak points will act as sub-goals for the local planner which will result in better traceability of the global plan. Rgds |
|
Hello Sir, |
There was a problem hiding this comment.
Hello,
I have made changes according to ROS C++ styleguide. But cannot find an alternative to change parameters max_number_classes or enable_multithreading. This is required because max_number_classes while using peakViaPoint( ), max_number_classes should be equal to 1. This saves computation cost in terms of time taken to reach the goal.
Moving the method peakViaPoint( ) into updateViaPointsContainer(...) is not required because via points follow the strict order when via_points_ordered is true. Even while using uniform via points with peakViaPoint( ) and via_points_ordered is true, a strict order is followed.
src/teb_local_planner_ros.cpp
Outdated
| @@ -206,6 +206,41 @@ bool TebLocalPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& | |||
| return true; | |||
| } | |||
|
|
|||
| void TebLocalPlannerROS::peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, | |||
| std::vector<geometry_msgs::PoseStamped>& transformed_plan) //@mudit | |||
| { | |||
There was a problem hiding this comment.
I have made changes according to ROS styleguide.
Used robot_circumscribed_radius so that minimum curvature of global plan on which a peak point will be present is a function of robot's radius.
Hello Sir,
I have made certain changes to the existing teb_local_planner. It was seen that the teb_local_planner did not follow the peak points of the global plan strictly. It is required because if the teb_local_planner follows the peak points of the global plan strictly, the local planner works more efficiently in terms of time as well as the robot does not get stuck. I have experienced that with the changes made in the code, the robot takes less time than the normal time elapsed to follow the same goal in the existing teb_local_planner.Also, the teb_local_planner does not get stuck in infinite loops of finding an alternative path. In the peakViaPoints( ), I have created a method so that the planner publishes a viaPoint at the peak of the goal plan taken into consideration by the teb_local_planner. Please look forward to the changes I have made.
-- Mudit Jindal