ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A
Ask Your Question

Why my Straight line planner doesn't work on all maps?

asked 2022-06-22 10:33:02 -0500

JackFrost67 gravatar image

updated 2022-06-24 03:53:58 -0500

I'm working on an elementary but dramatic global planner. It should plan a simple straight-line path from a starting point, which is the baseline center, and a goal, which is a 2D nav goal that I set from the GUI of RVIZ, or by hands putting the coordinates of the goal. The job is simple, but I can't work it out. What I have done till now is, pick up the coordinates of the starting point and the goal (expressed on the map frame), calculate the angular coefficient, and using this coefficient calculate the x coordinate of each waypoint on the straight by applying a translation of delta (which is the increment I want to apply) and a rotation of angular coefficient. Then the y coordinate is calculated by simple linear interpolation. It turns out that the path is correct for some maps (generated using the famous Gmapping), but for others, it is not. It really depends on how the baseline frame is positioned relative to the map frame (sometimes the path is turned 180 degrees from where it should be), and I'm wondering if I'm encountering a problem with the representation of the Euler angles. I know it is a stupid question and I'm wrong on something, but currently, I'm really struggling.

float yaw = 0.f;

  yaw = tf::getYaw(new_start.pose.orientation);
  yaw = (goal_.pose.position.y - start.pose.position.y) / (goal_.pose.position.x - start.pose.position.x); //angular coefficient

int id = 0;
int n = 0;
float goal_distance = sqrt(pow((start.pose.position.x - goal_.pose.position.x), 2) + pow((start.pose.position.y - goal_.pose.position.y), 2));

while(n * delta < goal_distance){

  geometry_msgs::PoseStamped new_goal = goal_;        

  if (flag_)
    //finding the points of the path that connect the goal (which is the projection of the start, n meters away) and the start, currently working on all maps
    new_goal.pose.position.x = (n * delta) * cos(yaw) + start.pose.position.x;
    new_goal.pose.position.y = (n * delta) * sin(yaw) + start.pose.position.y;
    new_goal.pose.orientation = new_start.pose.orientation;

        //finding the points of the path that connect the goal and the start, currently not working on some maps
        sign = (goal_.pose.position.x - start.pose.position.x >= 0) ? 1 : -1; //doesn't make sense, but it works
    new_goal.pose.position.x = sign * (n * delta) * cos(yaw) + start.pose.position.x;
    new_goal.pose.position.y = (((new_goal.pose.position.x - start.pose.position.x) * (goal_.pose.position.y - start.pose.position.y)) / (goal_.pose.position.x - start.pose.position.x)) + start.pose.position.y;
    new_goal.pose.orientation = goal_.pose.orientation;


This planner draws a straight line using as a goal the projection of the current pose meters away, or a straight line using as a goal the point selected from RVIZ. As you can see, in the second part, there is a lot going on, that's 100% wrong. Firstly I ... (more)

edit retag flag offensive close merge delete


Are you asking about generating nav_msgs/Path, or about geometry_msgs/Twist?

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-22 11:34:41 -0500 )edit

Hi, it's a global planner so the result should be a nav_msgs/Path

JackFrost67 gravatar image JackFrost67  ( 2022-06-22 11:52:49 -0500 )edit
  1. All poses in your plan (except maybe the last) should have the same yaw value (expressed as a quaternion, of course.) It is the angle of the vector drawn from start_xy to goal_xy.

  2. Make sure you passing the correct units to the sine/cosine functions (radians vs. degrees.)

If this isn't enough of a hint, you probably need to add some code to your description.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-22 13:41:36 -0500 )edit

thanks for the answer. 1. working with quaternions is a pain in the ass, yeah they resolve lots of problems, but yet a pain in the ass For the angle, I calculate it as a delta on y / delta on x, I think that's correct. 2. always use radians.

I made a Pastebin with the interesting part. This planner draws a straight line using as a goal the projection of the current pose meters away, or a straight line using as a goal the point selected from RVIZ. As you can see, in the second part, there is a lot going on, that's 100% wrong. Firstly I'm using a sign to calculate the x coordinate of the n-th point of the path, which is incorrect cause it is a transformation, but this was the only way I can make it work if the map frame is rotated ...(more)

JackFrost67 gravatar image JackFrost67  ( 2022-06-23 12:45:51 -0500 )edit

We want these questions/answers to be usable for future readers, so please edit your description, copy/paste that code into it, then format it with the 101010 button.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-23 15:24:32 -0500 )edit

ok, done...

JackFrost67 gravatar image JackFrost67  ( 2022-06-24 03:54:46 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2022-06-24 07:28:00 -0500

PatoInso gravatar image

To compute your yawin radians I think it should be yaw = atan2(goal.y - start.y, goal.x - start.x) instead of yaw = (goal.y - start.y) / (goal.x - start.x)

Otherwise, here is a tutorial for a plugin that plans a straight line. While the wrapping code is for ROS2 the core logic to compute the straight line is fully described:

edit flag offensive delete link more


Also, your new_goal.pose.orientation is not correct. While traveling your robot needs to point toward the goal (yaw in your code), but the goal_.orientation is usually not this same value. When your robot reaches the goal_ x,y, it should then rotate in place to the requested goal_.orientation.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-06-24 13:09:15 -0500 )edit

mmmm, yeah, it should be equal to the yaw I calculate. In the end, the rotational components of the pose of each waypoint on the path are not so important, the local planner is kinda agnostic to that components. In any case, I will fix it too. Thanks also to @PatoInso, on Monday I will apply the modifications and test them. I will let you know how it goes.

JackFrost67 gravatar image JackFrost67  ( 2022-06-25 04:12:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools


Asked: 2022-06-22 10:33:02 -0500

Seen: 32 times

Last updated: Jun 24