When I give way with via_point it constantly follows the path
Hi!
I have a 2 Wheel Differential Drive robot and I am using TEB Local planner for the local and global planners. I want to draw my own path by giving via points and I am sending certain points to the topic /move_base/TebLocalPlannerROS/via_points. I want it to go to the first point (0,1), then to the 2nd point (0,2), and finally, to the goal, which I entered in move_base_simple/goal. However, when it reaches the last point I want, instead of finishing the task, it comes back to the beginning point (0,1) and this cycle continues forever.
How can I solve this problem?
Is there a way to do this?
This is my code for publishing via_points:
int main(int argc, char **argv)
{
ros::init(argc,argv,"Publish_via_point");
ros::NodeHandle nh;
ros::Publisher via_pub;
via_pub = nh.advertise<nav_msgs::Path>("/move_base/TebLocalPlannerROS/via_points",1);
nav_msgs::Path via_points_msg;
via_points_msg.header.stamp = ros::Time::now();
via_points_msg.header.frame_id = "map";
geometry_msgs::PoseStamped via_points_1;
via_points_1.pose.position.x = 0;
via_points_1.pose.position.y = 1;
geometry_msgs::PoseStamped via_points_2;
via_points_2.pose.position.x = 0;
via_points_2.pose.position.y = 2;
via_points_msg.poses = {via_points_1,via_points_2};
ros::Rate loop_rate(0.5);
int count = 0;
while(ros::ok() && count<1)
{
via_pub.publish(via_points_msg);
ROS_INFO("Via_points are published. !!");
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
This is my code for sending goal:
int main(int argc, char **argv){
ros::init(argc,argv,"Publish_goal");
ros::NodeHandle nh;
ros::Publisher pub;
ros::Subscriber sub;
double x, y, z=0.0;
pub = nh.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal",1);
int count = 0;
ros::Rate loop_rate(1.0);
while (ros::ok() && count <1 )
{
geometry_msgs::PoseStamped goal;
goal.header.stamp = ros::Time::now();
goal.header.frame_id= "map";
cout << "Please enter the x : \t" ;
cin >> x ;
cout << "Please enter the y : \t" ;
cin >> y ;
goal.pose.position.x = x;
goal.pose.position.y = y;
goal.pose.position.z = 0.0;
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;
goal.pose.orientation.z = 0.0;
goal.pose.orientation.w = 1.0;
ROS_INFO("Goal position and orientation : %f, %f",goal.pose.position.x,goal.pose.position.y);
pub.publish(goal);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
};
First, I am running the via_points code and then I am running the move_base goal code.
System: Ubuntu 20 ROS Noetic
The TEB via_points don't cause the behavior you are describing. Are you sending a goal to the
move_base
action server? If not, please explain how you are using the navigation stack. If you are following a tutorial, provide a url.You can edit your post by clicking on the "edit" button near the end of the description.
Thanks for your answer. I edited my post. As I mentioned above, I send a via_points first and then a goal. I hope I could explain it better.
Are you entering the same goal over and over? If the robot is already at the X,Y you enter, sending it to that same goal should return to the same spot.
Actually, no. I'm only entering the points and goal once. When I send to the last via_point, it reaches the point I want, but after reaching it, it does not stop and comes back to the first via_point.