Ellipsoid planner stuck

[FaboNo]

Dear Sikang

Here is my first test. I had to create bagfile from the current map with the sensor_msgs::pointcloud (in the bag if filled in the topic 'cloud' only) and I am able to see it:

However the planner is stuck somewhere... and cannot produce any trajectory.

I kept unchange ellipsoid_planner_node.cpp, what I changed is the test.launch file as follow:

<launch>
  <arg name="debug" default="false"/>
  <arg name="dt" default="0.2"/>
  <arg name="max_num" default="-1"/>
  <!--<arg name="debug_valgrind" default="false"/>-->

  <arg name="prefix" value="" unless="$(arg debug)"/>
  <arg name="prefix" value="gdb -ex run --args" if="$(arg debug)"/>

  <node pkg="mpl_test_node"
    type="ellipsoid_planner_node"
    name="test_primitive"
    launch-prefix="$(arg prefix)"
    output="screen">
    <remap from="~pose" to="/move_base_simple/goal"/>
    <param name="file" value="$(find mpl_test_node)/maps/test/test.bag"/>
    <param name="topic" value="/cloud"/>
    <!-- Set start and goal -->
    <param name="start_x" value="0.0"/>
    <param name="start_y" value="0.0"/>
    <param name="start_z" value="1.3"/>
    <param name="start_vx" value="0.0"/>
    <param name="start_vy" value="0.0"/>
    <param name="start_vz" value="0.0"/>
    <param name="goal_x" value="3.5"/>
    <param name="goal_y" value="-4.0"/>
    <param name="goal_z" value="1.3"/>
    <!-- Set range -->
    <param name="origin_x" value="0.0"/>
    <param name="origin_y" value="0.0"/>
    <param name="origin_z" value="0.0"/>
    <param name="range_x" value="15.0"/>
    <param name="range_y" value="15.0"/>
    <param name="range_z" value="1.5"/>
    <!-- Set dynamic constraints -->
    <param name="dt" value="$(arg dt)"/>
    <param name="v_max" value="10.0"/>
    <param name="a_max" value="10"/>
    <param name="u_max" value="60.0"/>
    <!--<param name="t_max" value="$(eval dt*10)"/>-->
    <param name="t_max" value="-1"/>
    <param name="use_3d" value="false"/>
    <param name="use_prior" value="false"/>
    <param name="use_acc" value="true"/>
    <param name="use_jrk" value="false"/>
    <param name="num" value="2"/>
    <param name="w" value="10000"/>
    <param name="epsilon" value="2"/>
    <param name="max_num" value="$(arg max_num)"/>
    <param name="robot_r" value="0.5"/>
  </node>

</launch>

The output of the planner is:

[ INFO] [1556283495.131876975]: Get data!
[ INFO] [1556283495.140878846]: Takse 0.000000 sec to set up map!
[EllipsoidPlanner] PLANNER VERBOSE ON
[PlannerBase] set epsilon: 2.000000
[PlannerBase] set v_max: 10.000000
[PlannerBase] set a_max: 10.000000
[PlannerBase] set dt: 0.200000
[PlannerBase] set w: 10000.000000
[PlannerBase] set max num: -1
[PlannerBase] set tol_pos: 2.000000
[PlannerBase] set tol_vel: 2.000000
[PlannerBase] set tol_acc: 100.000000
Start:
pos:   0   0 1.3
vel: 0 0 0
acc: 0 0 0
use jrk!
Goal:
pos: 3.5  -4 1.3
vel: 0 0 0
acc: 0 0 0
use jrk!

++++++++++++++++++++ env_base ++++++++++++++++++
+                  w: 10000.00               +
+               wyaw: 1.00               +
+                 dt: 0.20               +
+              t_max: inf               +
+              v_max: 10.00               +
+              a_max: 10.00               +
+              j_max: -1.00               +
+            yaw_max: -1.00               +
+              U num: 25                +
+            tol_pos: 2.00               +
+            tol_vel: 2.00               +
+            tol_acc: 100.00               +
+            tol_yaw: -1.00               +
+heur_ignore_dynamics: 1                 +
++++++++++++++++++++ env_base ++++++++++++++++++

Start from new node!

I guess I did not provide the right information... I have few questions:

• Regarding the meaning for the origin+range values? Should it be like a bounding box of the known map with the origin=a specific corner of the bounding box?
• Do you accept in the planner negative values for the point cloud coordinates?
• Do we need to provide a "floor" as well (which is not that simple to create) ?

Thank you for your support

Fabrice

UPDATE

From the pic below I suspect that both start and goal should with positive coordinates (actually all points) - can you confirm? At least I know also that there is no need for a "floor" as well.

but I am facing another issue with the trajectory because on rviz, it does not reach the goal - do you have any idea ?

Thanks again

Fabrice

[sikang]

Hi Fabrice,

Sorry for the late response, I was traveling last week. It seems like the problem is the bounding box: the original goal you set with "y = -4.0" is indeed outside of the bounding box. You can set the origin of the bounding box by "origin_y = -4.5" in the launch file.

There should be a check for valid goal location and a visualization for the bounding box, let me update the test node. And you should be able to visualize it in the latest commit.

Sikang

[FaboNo]

Hi Sikang, No problem, Hope that you had a good trip! Yes by changing the value of the bbox, it works now:

As you have developed a nice tools, I have a couple of questions for you :)

If I want to create a trajectory in 3D space I have to set use_3d = true and also num = 3 in the launch file, am I right? In order to reach the target, I had to decrease the tolerance: planner_->setTol(0.5, 1.0, 100.0);, but I noticed that it reached the goal still with a velocity on x and y:

t: 3 pos: 5.48 -3.8  1.3 vel:  0.6 -0.6    0
g: 32520.000000, rhs: inf, h: 666.666667
t: 2.8 pos:  5.24 -3.56   1.3 vel:  1.8 -1.8    0
g: 30520.000000, rhs: inf, h: 1466.666667
t: 2.6 pos:   4.8 -3.12   1.3 vel:  2.4 -2.4    0
g: 28160.000000, rhs: inf, h: 2933.333333
t: 2.4 pos:  4.32 -2.68   1.3 vel:  2.4 -1.8    0
g: 25980.000000, rhs: inf, h: 4400.000000
t: 2.2 pos:  3.84 -2.44   1.3 vel:  2.4 -0.6    0
g: 23980.000000, rhs: inf, h: 5533.333333
t: 2 pos: 3.36 -2.4  1.3 vel: 2.4   0   0
g: 21800.000000, rhs: inf, h: 7133.333333
t: 1.8 pos: 2.88 -2.4  1.3 vel: 2.4   0   0
g: 19800.000000, rhs: inf, h: 8733.333333
t: 1.6 pos:   2.4 -2.36   1.3 vel:  2.4 -0.6    0
g: 17620.000000, rhs: inf, h: 10333.333333
t: 1.4 pos:  1.96 -2.12   1.3 vel:  1.8 -1.8    0
g: 15440.000000, rhs: inf, h: 11800.000000
t: 1.2 pos:  1.68 -1.68   1.3 vel:  1.2 -2.4    0
g: 13080.000000, rhs: inf, h: 12733.333333
t: 1 pos: 1.44 -1.2  1.3 vel:  1.2 -2.4    0
g: 11080.000000, rhs: inf, h: 13533.333333
t: 0.8 pos:  1.16 -0.72   1.3 vel:  1.8 -2.4    0
g: 8900.000000, rhs: inf, h: 14466.666667
t: 0.6 pos:  0.72 -0.28   1.3 vel:  2.4 -1.8    0
g: 6540.000000, rhs: inf, h: 15933.333333
t: 0.4 pos:  0.28 -0.04   1.3 vel:  1.8 -0.6    0
g: 4360.000000, rhs: inf, h: 17400.000000
t: 0.2 pos: 0.04    0  1.3 vel: 0.6   0   0
g: 2180.000000, rhs: inf, h: 18200.000000
t: 0 pos:   0   0 1.3

is it normal?

Thank you for your support

Fabrice

[sikang]

num = 3 is setting the discretization in control -- the larger value indicates a finer discretization. If you want to do 3D search, should set use_3d = true instead.

The final state may not necessarily be static in this case, if you want to enforce zero velocity, you can set the second term in planner_->setTol(0.5, 1.0, 100.0); to be zero (change 1.0 -> 0.0). The SetTol function takes input as position, velocity, acceleration tolerance sequentially.

Let me know if you have any other questions :)

[FaboNo]

Thank you very much for your answers. I will certainly have more questions when I will take a look at the replanning part - first I have to dig into it :) Best Fabrice