davetcoleman / moveit_ompl

0 stars 2 forks source link

Moveit does not execute my code in ompl. #2

Closed mariosx closed 7 years ago

mariosx commented 7 years ago

Hello,

I am trying for a while now to use some new planners that I developed in ompl_1.1.0 on Baxter with Moveit!. Note that the planners currently can plan only for a compound statement of SO2s. (They also require one more parameter in the constructor that I haven't find a way to pass a value with Moveit! yet)

Although I compiled everything successfully on indigo, following the wiki, and Moveit! seems to recognize my new planners, the code of my planners is not executed. There is a very hacky step on the process - every time I change my code in ompl, I compile and then I manually copy the libompl.so* files from "omplapp-1.1.0-Source/build/Release/lib" to "/opt/ros/indigo/lib/x86_64-linux-gnu". I know, it is not a good idea, but it was the only solution I found in order to compile ompl with moveit. For example the output from planning with baxter by launching rviz is the folowing:

    [ INFO] [1504031112.059653870, 215.020000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
    [ INFO] [1504031112.063874364, 215.024000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
    [ INFO] [1504031112.063986068, 215.024000000]: Planner configuration 'both_arms[RRTConnectkPLUS]' will use planner 'geometric::RRTConnectPLUS'. Additional configuration parameters will be set when the planner is constructed.
    [ INFO] [1504031112.065471377, 215.025000000]: both_arms[RRTConnectkPLUS]: Starting planning with 1 states already in datastructure
    [ INFO] [1504031112.065545440, 215.025000000]: both_arms[RRTConnectkPLUS]: Starting planning with 1 states already in datastructure
    [ INFO] [1504031112.065609836, 215.025000000]: both_arms[RRTConnectkPLUS]: Starting planning with 1 states already in datastructure
    [ INFO] [1504031112.065649846, 215.025000000]: both_arms[RRTConnectkPLUS]: Starting planning with 1 states already in datastructure
    [ INFO] [1504031112.078397753, 215.038000000]: both_arms[RRTConnectkPLUS]: Created 4 states (2 start + 2 goal)
    [ INFO] [1504031112.079526664, 215.039000000]: both_arms[RRTConnectkPLUS]: Created 5 states (2 start + 3 goal)
    [ INFO] [1504031112.080515153, 215.040000000]: both_arms[RRTConnectkPLUS]: Created 5 states (2 start + 3 goal)
    [ INFO] [1504031112.080826876, 215.040000000]: both_arms[RRTConnectkPLUS]: Created 5 states (2 start + 3 goal)
    [ INFO] [1504031112.080965363, 215.040000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.016339 seconds

As you can see Moveit! seems to use my new planner "geometric::RRTConnectPLUS" that uses a new sampler, currently planning for SO2s spaces. I tried to change the message printed from ompl in the function "ompl::base::PlannerStatus ompl::geometric::RRTConnectPLUS::solve" to something else: OMPL_INFORM("%s: SOMETHING Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_->size() + tGoal_->size()));

The message didn't change at all implying that there is different code executed instead of mine.

I can see only two different cases: a)My code is not updated and Moveit! uses an outdated library. b)Moveit! cannot even see my ompl library and uses another one.

But for both cases, every time I copy the libompl.so and I take care to keep only the updated version. Also I have search the whole system and there is no other libompl files other than the ones I use.

Does anyone have any idea on what is happening, or an alternative case that I haven't though yet?

I will really appreciate any help on the matter, Marios Xanthidis.

mariosx commented 7 years ago

Installation in a machine with fresh Ubuntu 14.04 solved the problem.

gxxzs commented 6 years ago

did you do this in ros indigo+ubuntu 14.04? I have some question in install moveit and ompl form source, can you post your process for execute my code in ompl? Thanks.

mariosx commented 6 years ago

Here are some notes that I kept for installing OMPL+MoveIt! for Baxter in a machine with fresh Ubuntu 14.04. I didn't have the free time to write a more presentable report or to double check everything. Please pay very close attention on the versions of each library.

Also, a feedback would be more than welcome since I am planning to write a small report for new users. I haven't found anything similar online and I hope that new users would have a far easier time than me to make everything working properly. Moreover, a script automating the whole process is in my plans, so any corrections in the following procedure will be very meaningful.

Thanks, Marios.


Dependecies for OMPL:

sudo apt-get install cmake
sudo apt-get install libeigen3-dev
sudo apt-get install build-essential
sudo apt-get install freeglut3-dev
sudo apt-get install libboost1.54-all-dev
sudo apt-get install python-setuptools
sudo apt-get install python-pip
sudo -H pip install -vU pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.7.tar.gz (maybe redundant)
sudo pip install pyopengl
sudo pip install Flask
sudo pip install Celery
sudo apt-get install libassimp-dev

Install libccd-2.0:
Download and extract libccd-v2.0 from https://github.com/danfis/libccd/releases
cd libccd-2.0/src
cmake ..
make
sudo make install

Install fcl-0.3.1:
Download and extract fcl-0.3.1 from https://github.com/flexible-collision-library/fcl/releases
cd fcl-0.3.1
mkdir build
cd build
cmake ..
make
sudo make install

Install ompl:

Download and extract omplapp-1.1.0-Source.tar.gz from https://bitbucket.org/ompl/ompl/downloads/

cd omplapp-1.1.0-Source
mkdir -p build/Release
cd build/Release
cmake ../..

Change CMakeLists.txt: Add "set(CMAKE_CXX_FLAGS "-fPIC")" at line 21.

make -j 4

In case of failure: 
sudo rm /usr/local/lib/libccd.a

Optionally although not needed we can create the python bindings for app:
make installpyplusplus && cmake . && make update_bindings

OMPL should be installed by now.

Adding new planners in ompl:

Add the planners in the proper space (/omplapp-1.1.0-Source/ompl/src/ompl/geometric/planners/rrt/), change the necessary code in /omplapp-1.1.0-Source/ompl/src/ompl/base and add the planners in:
a)/omplapp-1.1.0-Source/ompl/py-bindings/headers_geometric.txt anywhere.
b)/omplapp-1.1.0-Source/ompl/py-bindings/generate_bindings.py in line 538.

make -j 4.

Install ROS indigo as described here:       http://wiki.ros.org/indigo/Installation/Ubuntu
Create catkin workspace as described here:  http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment

Link your ompl to ROS and MoveIt!as described here: http://picknik.io/moveit_wiki/index.php?title=OMPL/Add_New_Planner

The process more analytically:

cd ~/omplapp-1.1.0-Source/build/Release/
cmake -DCMAKE_INSTALL_PREFIX=/opt/ros/indigo ../..
make -j 4
sudo make install

Install MoveIt! from source as described here: http://moveit.ros.org/install/source/

cd ~/catkin_ws
rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install python-wstool python-catkin-tools clang-format-3.8

cd src
wstool init .
wstool merge https://raw.githubusercontent.com/ros-planning/moveit/indigo-devel/moveit.rosinstall
wstool update
rosdep install -y --from-paths . --ignore-src --rosdistro indigo
cd ..
catkin config --extend /opt/ros/indigo --cmake-args -DCMAKE_BUILD_TYPE=Release       (Execute only after adding the planners if new planners needed to be added)
catkin_make                                                                        (Execute only after adding the planners if new planners needed to be added)

Install Baxter Gazebo (http://sdk.rethinkrobotics.com/wiki/Simulator_Installation):

sudo apt-get install gazebo2 ros-indigo-qt-build ros-indigo-driver-common ros-indigo-gazebo-ros-control ros-indigo-gazebo-ros-pkgs ros-indigo-ros-control ros-indigo-control-toolbox ros-indigo-realtime-tools ros-indigo-ros-controllers ros-indigo-xacro python-wstool ros-indigo-tf-conversions ros-indigo-kdl-parser

cd ~/catkin_ws/src
wstool merge https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/master/baxter_simulator.rosinstall
wstool update
cd ..
catkin_make
cp src/baxter/baxter.sh .

Install Baxter Moveit (http://sdk.rethinkrobotics.com/wiki/MoveIt_Tutorial):

cd ~/catkin_ws/src
git clone https://github.com/ros-planning/moveit_robots.git
cd ..
catkin_make

Configure Baxter:
Modify src/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp to include the new planners.
Modify /config/ompl_planning.yaml in YOURROBOT_moveit_config to include the option for the new planner. 

catkin config --extend /opt/ros/indigo --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make

Running Baxter:
(4 diffrent terminals) 

cd ~/catkin_ws; ./baxter.sh sim 
roslaunch baxter_gazebo baxter_world.launch

cd ~/catkin_ws; ./baxter.sh sim
rosrun baxter_tools enable_robot.py -e

cd ~/catkin_ws; ./baxter.sh sim
rosrun baxter_interface joint_trajectory_action_server.py

cd ~/catkin_ws; ./baxter.sh sim
roslaunch baxter_moveit_config baxter_grippers.launch