Running OMPL Using PyBind11

[FireBoyAJ24]

Description

• Resolves OMPL Python bindings not properly installed on ARM devices #398

Verification

• Run ros2 launch local_pathfinding main_launch.py on RPI
• Run ros2 launch local_pathfinding main_launch.py on X86

Resources

• https://gist.github.com/kervel/75d81b8c34e45a19706c661b90d02548
• https://github.com/pybind/cmake_example
• https://github.com/chrissunny94/floam
• https://pybind11.readthedocs.io/en/stable/basics.html

Extra Notes and Commands

pip3 install pybind11

local_pathfinding/src/build:
g++ -O3 -Wall -shared -std=c++11 -fPIC `python3 -m pybind11 --includes` ompl_bindings.cpp -o pyompl`python3-config --extension-suffix` -I/usr/include/ompl-1.6 -I/usr/include/eigen3 -L   -lompl

sudo ln -s /workspaces/sailbot_workspace/src/local_pathfinding/src/build/pyompl.cpython-310-aarch64-linux-gnu.so /workspaces/sailbot_workspace/src/local_pathfinding/local_pathfinding/pyompl.cpython-310-aarch64-linux-gnu.so

sudo ln -s /usr/share/libompl.so /usr/lib/libompl.so

export LD_LIBRARY_PATH=/usr/share:$LD_LIBRARY_PATH

[FireBoyAJ24]

Currently I am getting an issue where the OMPL optimization objective, StateCostIntegralObjective is not getting imported correctly.

I am getting the following error when I run ros2 launch local_pathfinding main_launch.py:

[SPDonaghy]

Do you need anything from me on this? Or is it all working now?

[FireBoyAJ24]

Do you need anything from me on this? Or is it all working now?

I still have issues accessing the classes and function from the pyompl module. If you are free could you help look through what could possibly causing this issue? If you're busy, then it's fine. I will be working on it on Saturday.

[SPDonaghy]

I see you've made some changes

Did it work before turning it into a cmake project?

What are the steps to build the module now?

[FireBoyAJ24]

To build sailompl, you just need to run tasks: build package - sailompl. Or Just run the build.sh script for sailompl to build the python bindings.

Then you can just run ros2 launch or python3 and import sailompl

[SPDonaghy]

I'll try reverting back to my last commit and see if I can get it working. I'll let you know!

[SPDonaghy]

Is this the behavior we are looking for?

ros@f72175179cf0:/workspaces/sailbot_workspace$ ros2 launch local_pathfinding main_launch.py
[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2024-10-31-19-59-12-751074-f72175179cf0-7317
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [navigate-1]: process started with pid [7318]
[INFO] [mock_global_path-2]: process started with pid [7320]
[navigate-1] Debug:   RRTstar: Planner range detected to be 0.879845
[navigate-1] Info:    RRTstar: Started planning with 1 states. Seeking a solution better than 0.00000.
[navigate-1] Info:    RRTstar: Initial k-nearest value of 45
[navigate-1] Info:    RRTstar: Found an initial solution with a cost of 82.40 in 3 iterations (3 vertices in the graph)
[mock_global_path-2] [INFO] [1730429954.033298423] [mgp_main:97]: Retreiving current position from http://localhost:3005/api/gps
[mock_global_path-2] [INFO] [1730429954.047440141] [mgp_main:117]: GPS data changed by more than 45.0 km. Running global path callback
[mock_global_path-2] [INFO] [1730429954.048862390] [mgp_main:160]: Global path file is: mock_global_path.csv
[mock_global_path-2]  Reading path
[mock_global_path-2] [INFO] [1730429954.050229279] [mgp_main:194]: Some waypoints in the global path exceed the maximum interval spacing of 30.0 km. Interpolating between waypoints and generating path
[mock_global_path-2] URL Error: [Errno 111] Connection refused
[mock_global_path-2] [WARN] [1730429954.055605001] [mgp_main:230]: Failed to post path to http://localhost:8081/global-path
[mock_global_path-2] [INFO] [1730429954.520541625] [mgp_main:160]: Global path file is: mock_global_path.csv
[mock_global_path-2]  Reading path
[mock_global_path-2] [INFO] [1730429954.521650107] [mgp_main:194]: Some waypoints in the global path exceed the maximum interval spacing of 30.0 km. Interpolating between waypoints and generating path
[mock_global_path-2] URL Error: [Errno 111] Connection refused
[mock_global_path-2] [WARN] [1730429954.534389407] [mgp_main:230]: Failed to post path to http://localhost:8081/global-path
[mock_global_path-2] [INFO] [1730429955.021592941] [mgp_main:160]: Global path file is: mock_global_path.csv
[mock_global_path-2]  Reading path
[mock_global_path-2] [INFO] [1730429955.022500640] [mgp_main:194]: Some waypoints in the global path exceed the maximum interval spacing of 30.0 km. Interpolating between waypoints and generating path
[mock_global_path-2] URL Error: [Errno 111] Connection refused
[navigate-1] Info:    RRTstar: Created 388 new states. Checked 75433 rewire options. 1 goal states in tree. Final solution cost 82.400
[mock_global_path-2] [WARN] [1730429955.028248836] [mgp_main:230]: Failed to post path to http://localhost:8081/global-path
[navigate-1] Info:    Solution found in 1.009402 seconds
[navigate-1] [INFO] [1730429955.057369112] [navigate_main.local_path:100]: Updating local path

[FireBoyAJ24]

That looks good! We currently are not publishing desired heading as that is not yet fully implemented.

[SPDonaghy]

Awesome ok I will test on rpi soon and then submit for review.

[SPDonaghy]

I'm thinking to just have a short build script that builds the bindings and then creates a symlink to the bindings in the directories where ROS and Python look for python modules. Then just update a couple environment variables in .bashrc and it all works fine.

[FireBoyAJ24]

PR #452 is a better implementation of OMPL bindings using PyBind11. Therefore, I will close this PR.