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;
if(flag_){
ROS_INFO("STRAIGHT");
yaw = tf::getYaw(new_start.pose.orientation);
}else{
ROS_INFO("non STRAIGHT");
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;
}
else
{
//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;
}
++n;
plan.push_back(new_goal);
}
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 180 degrees from the baseline frame.
Originally posted by JackFrost67 on ROS Answers with karma: 3 on 2022-06-22
Post score: 0
Original comments
Comment by Mike Scheutzow on 2022-06-22:
Are you asking about generating nav_msgs/Path, or about geometry_msgs/Twist?
Comment by JackFrost67 on 2022-06-22:
Hi, it's a global planner so the result should be a nav_msgs/Path
Comment by Mike Scheutzow on 2022-06-22:\
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.
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.
Comment by JackFrost67 on 2022-06-23:
thanks for the answer.
- 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.
- 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 180 degrees from the baseline frame.
Comment by Mike Scheutzow on 2022-06-23:
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.
Comment by JackFrost67 on 2022-06-24:
ok, done...
