Robotics StackExchange | Archived questions

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 /movebase/TebLocalPlannerROS/viapoints. 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 movebasesimple/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 viapoints code and then I am running the movebase goal code.

System: Ubuntu 20 ROS Noetic

Asked by mduz10 on 2022-08-16 04:48:51 UTC

Comments

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.

Asked by Mike Scheutzow on 2022-08-16 07:00:11 UTC

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.

Asked by mduz10 on 2022-08-16 07:57:09 UTC

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.

Asked by Mike Scheutzow on 2022-08-16 09:56:31 UTC

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.

Asked by mduz10 on 2022-08-16 11:18:16 UTC

Answers

You have

while(ros::ok() && count<2)

in the first code snippet with publishing via_points and

while (ros::ok() && count <1 )

in the second one, why are they different?

Do you want to go to via_points_1 (so (0,1)), via_points_2 (so (0,2)), then go from there to via_points_1 and via_points_2 again?

The usage of both code snippets isn't clear to me, could you please add explanation of your steps (e.g. commands in the terminal and their order)?

Asked by ljaniec on 2022-08-17 02:14:46 UTC

Comments

I edited my post. Coming to the first question, my aim there is to publish one of them twice, to publish one of them once. But I guess it's an insignificant detail. No matter how many times I publish, the problem does not go away.

Asked by mduz10 on 2022-08-17 03:57:17 UTC