Plan request don't publish anything

[caiofis]

I am trying to use this wrapper to the teb_local_planner without success so far. I have a global planner that publishes as set of waypoints as a nav_msgs::Path, I was hoping that I could uses those by remapping this global path to the /add_waypoints and than sending a empty msg on /plan. It said that was planning but I got no output on the /local_plan topic.
My second attempt was by making a python wrapper that subscribes to this global path topic, populates a teb_planner_pa_msgs/Request and than publish it on /request. This time it generated a path with 3 poses, but all of them on the initial pose, so on rviz all that I can see is a red arrow on my start pose.

That's how I populate the Request msg:

def pathCallback(data):                                                                                                          
    request = Request()                                                                                                          
    request.start = data.poses[0].pose                                                                                           
    request.goal = data.poses[-1].pose                                                                                           
    request.waypoints = data                                                                                                    
                                                                                                                                  
    request.start_vel.linear.x = 20                                                                                              
    pub.publish(request)

Can you see here I am doing something wrong? I guess that I didn't understood the required workflow

[BhaktiDanve]

Hello,
Sorry for late response!

About your first attempt: Just by adding waypoints as global plan on /add_waypoints topic and publishing empty msg on /plan would not lead to any plan creation. You have to mention the start and the goal pose by publishing on their respective topics(/setStart2d, /setGoal2d). After that you can publish empty msg on /plan topic. Adding waypoints is optional, whereas mentioning start and goal pose is a must for the plan to be generated. Start and goal can also be passed through the /request topic, which I think you tried in the second part.

About the second part of your Python wrapper: I don't see any fault in it. It should work ideally. Try checking what data you are receiving on /respond topic after you publish data on /request topic. You can also confirm if the data argument of your pathCallback function is correct (i.e. start and goal might be same which is creating the problem you mentioned).

Some other improvements in the package are in line. It will be updated in upcoming days.

Regards
Bhakti