SBPL fails to initialize

[wasih123]

Hello I am using ros-melodic for development. I have built sbpl from a pre-built binary(sudo apt-get install ros-melodic-sbpl). After this I cloned this repo and built sbpl_lattice_planner as a catkin package and it compiles fine. The problem comes when I run the complete navigation stack. If I run the example launch file provided: roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch I get this kind of a log: [ INFO] [1554814674.980236083, 0.200000000]: Loading from pre-hydro parameter style [ INFO] [1554814674.998183759, 0.200000000]: Using plugin "static_layer" [ INFO] [1554814675.002134127, 0.200000000]: Requesting the map... [ INFO] [1554814675.182887999, 0.400000000]: Resizing costmap to 2332 X 1825 at 0.025000 m/pix [ INFO] [1554814675.279035651, 0.500000000]: Received a 2332 X 1825 map at 0.025000 m/pix [ INFO] [1554814675.284756494, 0.500000000]: Using plugin "obstacle_layer" [ INFO] [1554814675.290046413, 0.500000000]: Subscribed to Topics: base_scan [ INFO] [1554814675.328114580, 0.500000000]: Using plugin "inflation_layer" [ INFO] [1554814675.347855227, 0.500000000]: Name is SBPLLatticePlanner setting parameter cost_inscribed_thresh to 19 setting parameter cost_possibly_circumscribed_thresh to 4 [ERROR] [1554814675.351010168, 0.500000000]: SBPL initialization failed! [move_base_node-3] process has died [pid 19356, exit code 1, cmd /home/wasih/catkin_ws/devel/lib/move_base/move_base name:=move_base_node log:=/home/wasih/.ros/log/11b16e44-5ac7-11e9-9ca2-dca2666cf4e9/move_base_node-3.log]. log file: /home/wasih/.ros/log/11b16e44-5ac7-11e9-9ca2-dca2666cf4e9/move_base_node-3*.log

As you can see, SBPL is somehow failing to initialize. I have searched for hours on this, but no progress so far. I would be very happy if someone points out to me the possible reason for this. Thanks a lot

[mintar]

I've tried, but I couldn't reproduce your problem. Since I'm still running Kinetic, I've used Docker to run the Melodic version of sbpl_lattice_planner. I couldn't quickly figure out how to run graphical programs inside Docker, so I'm running everything except move_base with the sbpl_lattice_planner on the host.

Steps to reproduce my setup:

1. On the host, launch everything first (if you want, you can comment out the move_base include from the launch file):

source /opt/ros/<your-ros-distro>/setup.bash
roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch

2. Save the following file as Dockerfile-melodic:

FROM ros:melodic-ros-core

RUN apt-get update && apt-get install -y ros-melodic-sbpl-lattice-planner ros-melodic-move-base

3. Build it:

docker build -t sbpl_melodic -f Dockerfile-melodic .

4. Run the Docker container:

docker run --net=host sbpl_melodic /bin/bash -c "source /opt/ros/melodic/setup.bash && roslaunch sbpl_lattice_planner move_base_sbpl.xml"

Now you can send goals from the RViz GUI, and the simulated robot should start moving.

---

From your log output, it's evident that you built move_base from source (and perhaps other packages as well). Something seems to have gone wrong along the way there, so please try to run from a clean ROS install, without sourcing your workspace.

Do not run source ~/catkin_ws/devel/setup.bash.

Instead run this:

source /opt/ros/melodic/setup.bash
roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch

Everything should run now. If that works, you can create a new catkin workspace and start adding packages from your old workspace (~/catkin_ws) until it breaks again. This should help isolating the problem.

[wasih123]

Hi I am sorry but I still get the same error. I install all packages through source in my workspace and I don't think that should cause any trouble. move_base works perfectly fine for other planners like the global planner but I don't know why SBLLattice_Planner is unable to work as global planner in base::Global Planner. I have tried reinstalling sbpl but still the same problem. Is there any specific way or what is the standard way to install sbpl and sbpl_lattice_planner? Also, it is clear from the log that sbpl_lattice_planner was running but somehow, sbpl(library) failed to initialize. How do I debug this since I have installed sbpl as a binary and can't print. What could be the reason for this assuming all other packages and move_base is working. Thanks a lot for cooperating.

[wasih123]

Hello After removing sbpl completely and installing again, the above problem was gone, maybe the issue was because ros was treating sbpl as a package(libsbpl.so was present in catkin_ws/devel/lib). Now, some other error comes up: [ INFO] [1554897398.760610744, 2.470000000]: Name is SBPLLatticePlanner terminate called after throwing an instance of 'SBPL_Exception' what(): ERROR: failed to read in motion primitive file My primitive file is: primitive_filename: /home/wasih/catkin_ws/src/adt_gazebo/config/bike.mprim I have attached my primitive file also. Please help in this regard. It would help a lot. Thanks bike.txt

[mintar]

Are you building SBPL from source? If yes, you can try the branch enable_roslog from my fork to log errors from SBPL to the ROS logging framework:

https://github.com/mintar/sbpl/tree/enable_roslog

Using this SBPL fork, when I replace sbpl_lattice_planner/matlab/mprim/pr2.mprim with your bike.txt file and run roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch, I get the following error:

[ERROR] [1554906688.808057951, 0.800000000]: ERROR: invalid resolution 0.050000 (instead of 0.025000) in the dynamics file

This was easily fixed by replacing 0.025 by 0.05 in sbpl_lattice_planner/launch/move_base/local_costmap_params_close.yaml and sbpl_lattice_planner/launch/move_base_sbpl_fake_localization_2.5cm.launch. Then I got another error:

[ERROR] [1554906770.403062420, 0.700000000]: ERROR: invalid angular resolution 1 angles (instead of 16 angles) in the motion primitives file

So obviously SBPL doesn't like the numberofangles: 1 in your mprim file.

[wasih123]

Thanks a lot for responding and helping! I think I need to use your fork because otherwise, I won't be able to see error logs inside sbpl. Thanks for that. The problem is that even, pr2.mprim causes the same error. So could you suggest something that I should try? Thanks

[mintar]

When you say that pr2.mprim causes the same error, you mean when you start that with your own launch file with modified parameters, right? The default roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch works, correct?

The thing that's usually wrong is the map resolution. The global costmap resolution needs to match the mprim file (0.05 m in the case of pr2.mprim).

If that doesn't solve your problem, I need detailed step-by-step instructions to reproduce your environment. It's best if you start from a fresh catkin workspace, with only /opt/ros/melodic/setup.bash sourced and nothing else.

[wasih123]

Yes, the default roslaunch sbpl_lattice_planner move_base_sbpl_fake_localization_2.5cm.launch works perfectly fine now. Thanks a lot for helping me! So the problem is with my ill-framed bike.txt. Can you please suggest how to write a motion primitive file for a bike, i.e. only forward motion with some steer. I thought I could just give one angle and some forward-turn arcs at various steering angles. How do I completely disallow backwards or those arcs of motion which are not permissible?(i.e. they are greater than the maximum steering angle of the bike). Thanks a lot again!

[mintar]

There are some mprim files at https://github.com/sbpl/sbpl/tree/master/matlab/mprim; have a look at the "unicycle" ones and the Matlab files to generate them.