path follower with multiple waypoints

[cogsys-tuebingen]

path follower with multiple waypoints

[betwo]

Hi Mert,

How far apart are your waypoints? Is the delay caused by the path planner?

Assuming that it is not a planning delay, please try the following:
When you specify a goal for navigate_to_goal you can pass along different options. One of them is the init_mode. When set to "INIT_MODE_CONTINUE", this will cause the follower not to stop when a new path is set. Please have a look at the Goal message

# init_mode: Determines how the path follower reacts to a new path:
#  INIT_MODE_STOP will cause the robot to brake
#  INIT_MODE_CONTINUE will not send a brake command
uint8 INIT_MODE_STOP = 0
uint8 INIT_MODE_CONTINUE = 1
uint8 init_mode

[mertmzk]

Hi Betwo,

Thanks for your response. They are a meter away from each others. I am not sure what is causing that.
I have changed the init_node parameter to 1 but nothing has changed. It still slows down when reaching to intermediate goals. I use gazebo_example_pioneer.launch and here my goals configuration:

        self.goal_sent = True
	goal = NavigateToGoalGoal()
        goal.follower_options.velocity=1.5
	goal.goal.pose.header.frame_id = 'map'
	goal.goal.pose.header.stamp = rospy.Time.now()
        goal.goal.pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
                                     Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))
	goal.planner_options.grow_obstacles= True
	goal.planner_options.obstacle_growth_radius= 0.449999988079
	goal.planner_options.goal_dist_threshold= 0.25
	goal.planner_options.goal_angle_threshold_degree= 5
	goal.planner_options.reversed= False
	goal.planner_options.allow_forward= True
	goal.planner_options.allow_backward= True
	goal.planner_options.ackermann_la= 1.0
	goal.planner_options.ackermann_steer_steps= 2
	goal.planner_options.ackermann_max_steer_angle_degree= 40.0
	goal.planner_options.ackermann_steer_delta_degree= 20.0
	goal.follower_options.init_mode=1
	goal.failure_mode= 1
        self.client.send_goal(goal)
	success = self.client.wait_for_result(rospy.Duration(60)) 
        state = self.client.get_state()

Thanks

[mertmzk]

Hi @betwo

I also checked to planning time and it is usually in between 20-100 ms. Do you think that could be the reason for the delay?

Is that possible to to not the stop robot while it is planning the path?

Thanks

[betwo]

Hey @mertmzk,

sorry for the delay, didn't find time sooner.

I suppose the problem is that you are using the action client in a synchronous way:

self.client.send_goal(goal)
After this call, the path following is done asynchronously.

success = self.client.wait_for_result(rospy.Duration(60))
But with this call, the action is made synchronous again and your client blocks, until the robot has reached the goal.

Then the next way point is sent, which takes around 100ms as you said, before the robot starts moving again. (Bear in mind that the path following controller will slow down the robot shortly before the goal, which further amplifies the experienced delay.)

To solve the issue I currently see a few possible solutions, depending on your use case

1. Do not call wait_for_result. Instead use tf to find out if you are close to your way point, then call cancel_goal() and send_goal() with the next way point. (Using INIT_MODE_CONTINUE.)
2. (Will be very time consuming) Instead of specifying only the goal pose, use the kinematic path planner and path follower directly: Use the path planning action to plan paths between all your way points and connect them into one path, then send this path to the follower.
3. (Will not really work in narrow environments) Same as 2., but instead of using a slow kinematic planner, calculate the complete path to your last way point using a 2d planner. (Note: This path is not guaranteed to be feasible but very efficient to compute.) Then use the path follower with a local planner to follow this path.

We used those solutions the past. Here is a demo with asynchronous execution and local planning: https://www.youtube.com/watch?v=4O7twdWFm4s
The local planning would not be necessary, if the robot would move slower (here it is up to 3m/s)

Hope this helps, please do not hesitate to ask further questions!

Cheers

[mertmzk]

Hi @betwo

Thanks for your reply. I have solved it using first solution. But currently I am having different problems. I appreciate if you could help me to solve this.

I was testing all in simulation and it was working fine in my laptop. But when I deploy the code in my robot--running on Odroid XU4-- I am facing with long delay from path planner when I use map which was created by gmapping. It is sometime giving an error and not activating path_follower but sometime working with very long delays around 30K-50K ms. If I deactivate map then it works fine but I want to get it working with a map.

I have played with couple of parameters on planner (2d planner) but they did not solve the problem.

Any suggestion to solve this problem?

Thanks

[betwo]

Strange, I would expect such a behaviour from the kinematic planner in a large map that does not allow for a kinematic path to the goal. Then the search will run into the timeout.

But the 2d planner does not respect kinematic constraints, so it much faster and should take nowhere close to 30-50 seconds (even if there exists no feasible path.)

Do you use GMapping in simulation as well? And does the map have a similiar size and makeup?

You could try to set the debug flag render_open_cells in the planner and then observe the search in RViz on the topic cells (type is nav_msgs::GridCells.)
This will show you the current frontier in the search.

I hope that I can help you more if you describe (or post a few screenshots of) the search frontier over the long time that the search takes.
Also, please post more details about the error that you sometimes get, that might indicate something.

[mertmzk]

Hi @betwo

Thanks for your rapid response. Here some more information:

Output messages from terminal including 2 errors:

pose: 
  header: 
    seq: 0
    stamp: 1538659564.621807172
    frame_id: map
  pose: 
    position: 
      x: 0.553813
      y: 0.497545
      z: 0
    orientation: 
      x: 0
      y: 0
      z: 0.955064
      w: -0.2964
map: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  info: 
    map_load_time: 0.000000000
    resolution: 0
    width: 0
    height: 0
    origin: 
      position: 
        x: 0
        y: 0
        z: 0
      orientation: 
        x: 0
        y: 0
        z: 0
        w: 0
  data[]
map_search_min_value: 0
map_search_min_candidates: 0
min_dist: 0

[ INFO] [1538659568.664511197]: Start Action! Requested velocity: 0.1
[ INFO] [1538659568.664657322]: Goal just went active
[ INFO] [1538659568.764824576]: Wait for follow_path action server...
[ INFO] [1538659568.765406199]: got request with channel 
[ INFO] [1538659568.765577199]: remapped channel to /plan_path
[ INFO] [1538659568.825192061]: waiting for planner @ /plan_path
[ INFO] [1538659569.184992189]: waiting for path
[ INFO] [1538659569.685300798]: still waiting for path
Failed to find match for field 'label'.
[ INFO] [1538659571.685742483]: still waiting for path
[ INFO] [1538659573.686165961]: still waiting for path
[ INFO] [1538659575.686520647]: still waiting for path
[ INFO] [1538659577.686945208]: still waiting for path
[ INFO] [1538659579.687449102]: still waiting for path
[ INFO] [1538659581.687870038]: still waiting for path
[ INFO] [1538659583.688270474]: still waiting for path
[ INFO] [1538659585.688718035]: still waiting for path
[ INFO] [1538659587.689109013]: still waiting for path
[ERROR] [1538659589.189675506]: Path planner failed. Final state: ACTIVE
[ WARN] [1538659589.206508235]: preempting!!
[ WARN] [1538659589.207221399]: done preempting path planner
[ WARN] [1538659589.290147468]: No path found. Abort goal.
[ INFO] [1538659589.290887923]: DONE with action state ABORTED
[ WARN] [1538659589.291025256]: Did not reach goal :(
[ INFO] [1538659589.291126047]: Result code: 6 NO_PATH_FOUND
[ INFO] [1538659589.291236380]: Additional Text: 
[ INFO] [1538659589.291126047]: Result code: 6 NO_PATH_FOUND
[ INFO] [1538659589.291236380]: Additional Text: 
[ INFO] [1538659654.558527612]: still planning
[ INFO] [1538659654.558696361]: preempted path planner
[ERROR] [1538659654.558952444]: To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: 2
[ INFO] [1538659654.559158610]: path planning took 85352ms

Planner launch file:

<?xml version="1.0"?>
<launch>
   <node name="path_planner" type="path_planner_node" pkg="path_planner"
    		output="screen" respawn="true">
        <param name="use_map_topic" value="true" />
        <param name="use_map_service" value="false" />
        <param name="map_service" value="dynamic_map" />
        <param name="map_topic" value="/map" />
        <param name="use_unknown_cells" value="false" />
        <param name="use_cost_map" value="false" />

        <param name="use_collision_gridmap" value="false" />
        <param name="grow_obstacles" value="0.2" />

        <param name="render_open_cells" value="true"/>

        <param name="penalty/backward" value="1.0" />
        <param name="penalty/turn" value="0.0" />

        <param name="oversearch_distance" value="0.05" />

        <param name="algorithm" value="2d" />

        <param name="preprocess" value="false" />
        <param name="postprocess" value="false" />

        <param name="use_cloud" value="true" />
        <param name="use_scan_front" value="true" />
        <param name="use_scan_back" value="false" />

        <param name="size/forward" value="0.2" />
        <param name="size/backward" value="-0.6" />
        <param name="size/width" value="0.70" />

        <remap from="scan/front/filtered" to="scan_filtered" />
    </node>
</launch>

Rviz output with render_open_cells:

The goals that I am sending from rviz quite close to the robot and on obstacle free path as seen in the figure.

Thanks

[betwo]

The screen shot actually looks alright. Seems like the search has reached the goal.

How long did it take until the RViz visualization looked like this?
(This should take practically no time at all for a short distance like this.)

Please try the following parameter settings and check if they have an effect:

1. oversearch_distance = 0.0
2. grow_obstacles = 0.0
3. use_unknown_cells = true
4. reversed=false and reversed=true

Also, please add screen shots of:

1. The actual planning map (planner/map)
2. The markers published (visualization_marker / visualization_marker_array)

[mertmzk]

Hi @betwo

It was taking 85 seconds until Rviz looked as it was in previous example.

I have tried with your suggested parameters, it is much better, between 2-3 seconds, but still not perfect. Whenever it replans the route in long paths it is stopping that 2-3 seconds to replan the route.

Here I attached screenshots with 'planner/map' and visual markers.

Thanks

[betwo]

Sorry, again, for the delay.

Does the robot stop for planning before it reaches the end of the old path?
If yes, then the FollowerOptions::INIT_MODE_CONTINUE seems not to be set correctly.
If no, your high level script starts planning too late.

I simulated that on my machine by enabling FollowerOptions::INIT_MODE_CONTINUE in the highlevel_dummy_node and using RViz (see https://youtu.be/NHjYErWGcZg).

When planning is started early enough, no stop command is sent.
This can be tested with rostopic echo /cmd_vel/linear | grep "x: 0.0" -> no zero-command should be sent.

[betwo]

I am closing this issue for now due to the long inactivity.