sikang / mpl_ros Goto Github PK

Hi Sikang,
I'm currently running a visual slam and stereo depth together with voxblox mapping engine, which can publish surface point cloud from the incremental map.

How would you recommend to implement the real-time replan using this package?

Thank you in advance.
Chang

Hi @sikang, thanks for providing this wonderful project. By following your instructions in the map making sections, I could record the voxel_map topic through the launch file. However, when I tried to implement my custom model in ellipsoid_planner_node, the custom map didn't show up by showing the following error message.

[ WARN] [1657181193.172610214]: Fail to find '/cloud' in '/root/catkin_ws/src/mpl_ros/mpl_test_node/maps/building/aa.bag', make sure md5sum are equivalent.
[test_primitive-1] process has died [pid 4277, exit code -11, cmd /root/catkin_ws/devel/lib/mpl_test_node/ellipsoid_planner_node ~pose:=/move_base_simple/goal __name:=test_primitive __log:=/root/.ros/log/8e369782-fdcb-11ec-9e19-88366cf6d71f/test_primitive-1.log].
log file: /root/.ros/log/8e369782-fdcb-11ec-9e19-88366cf6d71f/test_primitive-1*.log

I thought it was caused by the topic cloud is not recorded because rosbag record /voxel_map only records /voxel_map topic. I assumed that rviz shows map only through pointcloud data not voxel map. I have tried with many times and knew that if the bag file doesn't contain cloud topic, then the above error message will come out. So, I recorded all the topics through the command rosbag record -a and it removes the issue.

I want to implement your algorithm in custom built environments (gazebo world). Are there any easier way to get custom map in .stl format in Gazebo? If is not, is there any way I could open my custom map with voxel map topic?

Many thanks.

Hi @sikang, I would like to convert OccupancyGrid map which is widely used in ROS navigation to Polymap (Polyhedron2D or decomp_ros_msgs::PolyhedronArray), but I just can not figure out the geometry relationship between them. Could you give me some advice on how to do it? Thank you!

hi sikang:
I am running your simple code poly_map_planner_node, but the memory increase and the rviz doesn't show any thing.After a while my computer crash.My memory is 64G.
do you know why about this?

Hi !
I would really love to implement this package on a real quad
Any suggestion? What are the 'missing pieces' ?
I could not find any information about input/output parameters ...

Thank you !

Hello Sikang, I seem to be facing the following issue while using mpl_ros for generating trajectory using OccMapPlanner. I have generated a occupancy gridmap using SLAM and fed it into the OccMapPlanner. The planner doesn't recognize the obstacles/unknown space and generates a trajectory through the obstacles as shown below.

Hi!
I am trying to install mpl_ros with catkin_make on Ubuntu18.04, and get error:

error: ‘class rviz::PointCloudCommon’ has no member named ‘getCallbackQueue’
   update_nh_.setCallbackQueue(point_cloud_common_->getCallbackQueue());

I guess the problem comes from the wrong version of Rviz, any suggestion?

Ｄｅａｒ　sikang
CMakeFiles/mesh_sampling.dir/src/mapping_utils/mesh_sampling.cpp.o: In function vtkRenderingCore_AutoInit::~vtkRenderingCore_AutoInit()': mesh_sampling.cpp:(.text._ZN25vtkRenderingCore_AutoInitD2Ev[_ZN25vtkRenderingCore_AutoInitD5Ev]+0x13): undefined reference to vtkRenderingOpenGL2_AutoInit_Destruct()'
CMakeFiles/mesh_sampling.dir/src/mapping_utils/mesh_sampling.cpp.o: In function _GLOBAL__sub_I__Z16uniform_sampling15vtkSmartPointerI11vtkPolyDataEmRN3pcl10PointCloudINS2_8PointXYZEEE': mesh_sampling.cpp:(.text.startup+0x10c4): undefined reference to vtkRenderingOpenGL2_AutoInit_Construct()'
collect2: error: ld returned 1 exit status
CMakeFiles/mesh_sampling.dir/build.make:334: recipe for target '/home/zlt/z0323test/devel_isolated/planning_ros_utils/lib/planning_ros_utils/mesh_sampling' failed
make[2]: *** [/home/zlt/z0323test/devel_isolated/planning_ros_utils/lib/planning_ros_utils/mesh_sampling] Error 1
CMakeFiles/Makefile2:685: recipe for target 'CMakeFiles/mesh_sampling.dir/all' failed
make[1]: *** [CMakeFiles/mesh_sampling.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....

what should i do to make it work ?

Hello Sikang, I am trying to understand the map_replanner_node and I had the following questions related to it.

1. By using subscriber callbacks addCloudCallback() and clearCloudCallback(), we are updating the map as the bot is moving. Now, if we use the traditional local_planner/global_planner framework of ROS, we use the Lidar data to update the localmap in odom frame and the local trajectory is planned in the odom frame. But according to your code, it looks like addCloudCallback() and clearCloudCallback() is working in the global frame and updates the global map. So if I use my Lidar data, it would have to be transformed from base_frame ->odom_frame->map_frame before updating the map. Is my understanding correct ?

2. What topic should clearCloudCallback() subscribe to ? It is obvious that addCloudCallback() would subscribe to the Lidar pointcloud to update the map in it's surrounding based on it's range. What about clearCloudCallback() ? Shouldn't both callbacks subscribe to the same Lidar data topic?

3. What is the use of subtreeCallback() ?

4. At what rate is the replanner running ? It looks like planning thread is being run separately.

Thank you.

Hi !

I tried running example 4 (plan in SE(3) with ellispoid model) with original .launch files and the result shows
[MPPlanner] Cannot find a traj!
[WARN] [1533132548.397945260]: Failed! Takes 0.000197 sec for planning, expand [0] nodes
To test whether I set up the map correctly, I tried using other maps under the "mpl_ros/mpl_test_node/maps" folder, or set the starting point and the goal point closer and there are no obstacles in between, but the problem is not solved.
I would like to ask if there is any parameter in test.launch, or other issues that I should notice to generate correct trajectory?

Thanks !

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

Dear Sikang,
Thank you for sharing the code. I am a PhD student and am interested in using your code but in python. I want to ask have you considered implementing the approach in python or writing a pybinding wrapper? Do you expect the python version to be much slower? Any suggestions are appreciated. Thank you in advance.

Dear Sikang

First thank you for sharing such amazing piece of work! We would like to use it both on simulation and on a real quad and I have few questions:
1- one of our main challenge is to go thru a door with a quad which is slightly too big for the door (narrow gap) - my question is related to the inputs of the trajectory computation. We are using a 2D lidar but we can create a partial 3D map anyway - however as the map is incrementally built, we do not have a complete known Map - some area will be labeled as unknown. Does the algorithm is able to deal with such a map? If not what kind of pre-processing is required?

2- the second one is to compute a trajectory in a 2D occupancy grid, but incomplete, meaning that there are also some unknown cells. But here I guess that if we provide a 2D map with the "frontiers" marked as occupied, it should work. Besides I think that we need to inflate the obstacles, am I right?

Thank you

Regards

Hi Sikang, Thank you for sharing your code. I am playing around with different parameters in the file: mpl_ros/mpl_test_node/launch/map_planner_node/test.launch. Specifically, changing start/goal positions and dt. However, sometimes rviz gives wierd trajectories:
E.g. with the following setup:

 <!-- Set start and goal -->
 <param name="goal_x" value="3.0"/>
 <param name="goal_y" value="17.0"/>
 <param name="goal_z" value="0.05"/>
 <param name="start_vx" value="0.0"/>
 <param name="start_vy" value="2.0"/>
 <param name="start_vz" value="0.0"/>
 <param name="start_x" value="12.0"/>
 <param name="start_y" value="1.5"/>
 <param name="start_z" value="0.05"/>
 <!-- Set dynamic constraints -->
 <param name="v_max" value="2.0"/>
 <param name="a_max" value="1.0"/>
 <param name="yaw_max" value="0.5"/>
 <param name="u" value="1.0"/>
 <param name="u_yaw" value="0.5"/>
 <param name="dt" value="0.25"/>
 <param name="use_3d" value="true"/>
 <param name="use_yaw" value="false"/>

Rviz gives:

My questions are:

• Why would the trajectory not reaching the goal (I suppose the goal is the red circle)?
• Why would there be a line segment in the middle of the trajectory?
• Why does the line segment not having any velocity indication?
  It seems like if I use dt=1.0, the trajectory would look normal.

Thank you for your time,
Weidong

Is it possible to plan a trajectory between two points, but half of the trajectory with input=jerk and the other half with input=velocity?
If so, how could it be achieved?

Thanks!!

Hi Sikang, thanks for sharing your code. I'm trying to apply the yaw planning, but some problem with the result. The output yaw angle seems to be the same as velocity direction, any way to change the parameter of theta to relax the hard constraint? I have notice that there is a parameter of "tol_yaw", that's only for goal region right?


Thanks for your time,
Johnson

Hi! Thank you for your library!

I tried to use this planner with rotors simulator with gazebo.
Then, I want to set the center of the VoxelMap to a quadrotor's position.
When I just set the origin with VoxelMapUtil's setMap function, it just shifted the map.

How can I fix this?

Thank you!

Dear Sikang

First thank you for sharing such amazing piece of work! We would like to use it both on simulation and on a real quad .I would like to ask if this package can be used on a four-wheeled robot? If not, what changes can be made? Thank you very much for your answer.

Thank you so much for sharing the code. The testing launches works amazing!
It seems to me that all the testing examples are with 2D trajectory planing, I am wondering if you could suggest some guideline to plan a 3d trajectory, as shown in the 3D voxel map figure in the readme?

Thank you in advance.
Chang

My ubuntu is 18.04 bionic, so according to this line 1, the ellipsoid_planner_node will not be compiled.

If I manually delete this line, there will be a conflicting declaration 'typedef struct LZ4_stream_t' bug appearing. My question is: can this be solved? Can ellipsoid_planner_node be executed on ubuntu 18.04 bionic with some kinds of workaround?