Add build support for melodic

base_placement_plugin/CMakeLists.txt

@@ -21,7 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
-
+find_package(HDF5 REQUIRED)
 include(${QT_USE_FILE})
 
 
@@ -80,9 +80,9 @@ catkin_package(
 qt4_wrap_cpp(base_placement_plugin_MOCS ${base_placement_plugin_HDRS} )
 qt4_wrap_ui(base_placement_plugin_UIS_H ${base_placement_plugin_UIS} )
 
-include_directories(${base_placement_plugin_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS})
+include_directories(${base_placement_plugin_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS})
 add_library(${PROJECT_NAME} ${base_placement_plugin_SRCS} ${base_placement_plugin_MOCS} ${base_placement_plugin_UIS_H}) # ${base_placement_plugin_UIS_H}
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}  yaml-cpp -lhdf5 -lhdf5_cpp  create_marker)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}  yaml-cpp ${HDF5_LIBRARIES} create_marker)
 ## target_link_libraries(${PROJECT_NAME} yaml-cpp)
 
 

base_placement_plugin/include/base_placement_plugin/create_marker.h

@@ -9,13 +9,19 @@
 #include <Eigen/Eigen>
 #include <eigen_conversions/eigen_msg.h>
 
-#include<moveit/move_group_interface/move_group.h>
 #include<moveit/robot_state/robot_state.h>
 
 #include<moveit/robot_model_loader/robot_model_loader.h>
 #include<moveit/robot_model/robot_model.h>
 #include<moveit/robot_model/joint_model_group.h>
 #include<moveit/robot_model/link_model.h>
+#if ROS_VERSION_MINIMUM(1,12,0)
+    #include <moveit/move_group_interface/move_group_interface.h>
+    typedef moveit::planning_interface::MoveGroupInterface MoveGroupInterface;
+#else
+    #include <moveit/move_group_interface/move_group.h>
+    typedef moveit::planning_interface::MoveGroup MoveGroupInterface;
+#endif
 
 
 typedef std::multimap<std::vector<double>,geometry_msgs::Pose> BasePoseJoint;
@@ -52,7 +58,7 @@ class CreateMarker{
 
   std::string group_name_;
   ros::AsyncSpinner spinner;
-  boost::scoped_ptr<moveit::planning_interface::MoveGroup> group_;
+  boost::scoped_ptr<MoveGroupInterface> group_;
   std::string parent_link;
   moveit::core::RobotModelConstPtr robot_model_; //Robot model const pointer
 };

base_placement_plugin/include/base_placement_plugin/place_base.h

@@ -8,8 +8,14 @@
 #include<base_placement_plugin/create_marker.h>
 
 #include<moveit/planning_scene_monitor/planning_scene_monitor.h>
-#include<moveit/move_group_interface/move_group.h>
 #include<moveit/robot_model_loader/robot_model_loader.h>
+#if ROS_VERSION_MINIMUM(1,12,0)
+    #include <moveit/move_group_interface/move_group_interface.h>
+    typedef moveit::planning_interface::MoveGroupInterface MoveGroupInterface;
+#else
+    #include <moveit/move_group_interface/move_group.h>
+    typedef moveit::planning_interface::MoveGroup MoveGroupInterface;
+#endif
 #include<moveit/robot_model/robot_model.h>
 #include<moveit/robot_model/joint_model_group.h>
 #include<moveit/robot_state/robot_state.h>

base_placement_plugin/plugin.xml

@@ -1,7 +1,7 @@
 <library path="lib/libbase_placement_plugin">
   <class name="base_placement_plugin/Base Placement Plug-in"
          type="base_placement_plugin::AddWayPoint"
-         base_class_type="rviz::Panel">
+         base_class_type="rviz::Panel"/>
     <description>
       Tool for visualizing base placement methods
     </description>

base_placement_plugin/src/create_marker.cpp

@@ -10,7 +10,7 @@ double unifRand()
 CreateMarker::CreateMarker(std::string group_name) : spinner(1), group_name_(group_name)
 {
   spinner.start();
-  group_.reset(new moveit::planning_interface::MoveGroup(group_name_));
+  group_.reset(new MoveGroupInterface(group_name_));
   //ROS_INFO_STREAM("Selected planning group: "<< group_->getName());
   robot_model_ = group_->getRobotModel();
 }

base_placement_plugin/src/widgets/base_placement_widget.cpp

@@ -4,8 +4,8 @@
 
 #include <map_creator/hdf5_dataset.h>
 
-#include <H5Cpp.h>
-#include <hdf5.h>
+#include <hdf5/serial/H5Cpp.h>
+#include <hdf5/serial/hdf5.h>
 
 namespace base_placement_plugin
 {

map_creator/CMakeLists.txt

@@ -17,6 +17,8 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 find_package(octomap REQUIRED)
+find_package(HDF5 REQUIRED)
+find_package(MPI REQUIRED)
 
 add_message_files(
  FILES
@@ -43,16 +45,15 @@ catkin_package(
     roscpp
   CATKIN_DEPENDS message_runtime 
 )
-
-include_directories(include ${catkin_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${HDF5_INCLUDE_DIR} ${PCL_INCLUDE_DIR})
+include_directories(include ${catkin_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${HDF5_INCLUDE_DIRS} ${PCL_INCLUDE_DIR} ${MPI_CXX_INCLUDE_DIRS})
 
 add_library(sphere_discretization  src/sphere_discretization.cpp)
 add_library(kinematics  src/kinematics.cpp )
 add_library(hdf5_dataset  src/hdf5_dataset.cpp)
 
 target_link_libraries(sphere_discretization  ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} ${PCL_LIBRARY_DIRS})
 target_link_libraries(kinematics  ${catkin_LIBRARIES} -llapack)
-target_link_libraries(hdf5_dataset  ${catkin_LIBRARIES} -lhdf5 -lhdf5_cpp)
+target_link_libraries(hdf5_dataset  ${catkin_LIBRARIES} ${HDF5_LIBRARIES} ${MPI_CXX_LIBRARIES})
 
 install(TARGETS sphere_discretization kinematics hdf5_dataset
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

map_creator/include/map_creator/hdf5_dataset.h

@@ -1,7 +1,7 @@
 #ifndef HDF5_DATASET_H
 #define HDF5_DATASET_H
-#include "H5Cpp.h"
-#include <hdf5.h>
+#include <hdf5/serial/H5Cpp.h>
+#include <hdf5/serial/hdf5.h>
 #include <iostream>
 #include <ros/ros.h>
 

map_creator/include/map_creator/ikfast.h

@@ -1,5 +1,5 @@
 // -*- coding: utf-8 -*-
-// Copyright (C) 2012-2014 Rosen Diankov <[email protected]>
+// Copyright (C) 2012 Rosen Diankov <[email protected]>
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -41,14 +41,12 @@
 #define IKFAST_HEADER_COMMON
 
 /// should be the same as ikfast.__version__
-/// if 0x10000000 bit is set, then the iksolver assumes 6D transforms are done without the manipulator offset taken into
-/// account (allows to reuse IK when manipulator offset changes)
-#define IKFAST_VERSION 0x10000048
+#define IKFAST_VERSION 61
 
 namespace ikfast
 {
 /// \brief holds the solution for a single dof
-template < typename T >
+template <typename T>
 class IkSingleDOFSolutionBase
 {
 public:
@@ -61,15 +59,15 @@ class IkSingleDOFSolutionBase
   unsigned char jointtype;     ///< joint type, 0x01 is revolute, 0x11 is slider
   unsigned char maxsolutions;  ///< max possible indices, 0 if controlled by free index or a free joint itself
   unsigned char indices[5];  ///< unique index of the solution used to keep track on what part it came from. sometimes a
-                             ///solution can be repeated for different indices. store at least another repeated root
+  /// solution can be repeated for different indices. store at least another repeated root
 };
 
 /// \brief The discrete solutions are returned in this structure.
 ///
 /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
 /// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its
 /// prototype is:
-template < typename T >
+template <typename T>
 class IkSolutionBase
 {
 public:
@@ -83,24 +81,23 @@ class IkSolutionBase
   virtual void GetSolution(T* solution, const T* freevalues) const = 0;
 
   /// \brief std::vector version of \ref GetSolution
-  virtual void GetSolution(std::vector< T >& solution, const std::vector< T >& freevalues) const
+  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
   {
     solution.resize(GetDOF());
     GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
   }
 
   /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
   ///
-  /// 0 always points to the first value accepted by the ik function.
   /// \return vector of indices indicating the free parameters
-  virtual const std::vector< int >& GetFree() const = 0;
+  virtual const std::vector<int>& GetFree() const = 0;
 
   /// \brief the dof of the solution
-  virtual const int GetDOF() const = 0;
+  virtual int GetDOF() const = 0;
 };
 
 /// \brief manages all the solutions
-template < typename T >
+template <typename T>
 class IkSolutionListBase
 {
 public:
@@ -112,13 +109,11 @@ class IkSolutionListBase
   ///
   /// \param vinfos Solution data for each degree of freedom of the manipulator
   /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can
-  /// freely set. The indices are of the configuration that the IK solver accepts rather than the entire robot, ie 0
-  /// points to the first value accepted.
-  virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos,
-                             const std::vector< int >& vfree) = 0;
+  /// freely set.
+  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
 
   /// \brief returns the solution pointer
-  virtual const IkSolutionBase< T >& GetSolution(size_t index) const = 0;
+  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
 
   /// \brief returns the number of solutions stored
   virtual size_t GetNumSolutions() const = 0;
@@ -129,13 +124,12 @@ class IkSolutionListBase
 };
 
 /// \brief holds function pointers for all the exported functions of ikfast
-template < typename T >
+template <typename T>
 class IkFastFunctions
 {
 public:
   IkFastFunctions()
     : _ComputeIk(NULL)
-    , _ComputeIk2(NULL)
     , _ComputeFk(NULL)
     , _GetNumFreeParameters(NULL)
     , _GetFreeParameters(NULL)
@@ -149,10 +143,8 @@ class IkFastFunctions
   virtual ~IkFastFunctions()
   {
   }
-  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase< T >&);
+  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
   ComputeIkFn _ComputeIk;
-  typedef bool (*ComputeIk2Fn)(const T*, const T*, const T*, IkSolutionListBase< T >&, void*);
-  ComputeIk2Fn _ComputeIk2;
   typedef void (*ComputeFkFn)(const T*, T*, T*);
   ComputeFkFn _ComputeFk;
   typedef int (*GetNumFreeParametersFn)();
@@ -174,11 +166,11 @@ class IkFastFunctions
 // Implementations of the abstract classes, user doesn't need to use them
 
 /// \brief Default implementation of \ref IkSolutionBase
-template < typename T >
-class IkSolution : public IkSolutionBase< T >
+template <typename T>
+class IkSolution : public IkSolutionBase<T>
 {
 public:
-  IkSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos, const std::vector< int >& vfree)
+  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
   {
     _vbasesol = vinfos;
     _vfree = vfree;
@@ -205,19 +197,19 @@ class IkSolution : public IkSolutionBase< T >
     }
   }
 
-  virtual void GetSolution(std::vector< T >& solution, const std::vector< T >& freevalues) const
+  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
   {
     solution.resize(GetDOF());
     GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
   }
 
-  virtual const std::vector< int >& GetFree() const
+  virtual const std::vector<int>& GetFree() const
   {
     return _vfree;
   }
-  virtual const int GetDOF() const
+  virtual int GetDOF() const
   {
-    return static_cast< int >(_vbasesol.size());
+    return static_cast<int>(_vbasesol.size());
   }
 
   virtual void Validate() const
@@ -239,14 +231,10 @@ class IkSolution : public IkSolutionBase< T >
           throw std::runtime_error("2nd index >= max solutions for joint");
         }
       }
-      if (!std::isfinite(_vbasesol[i].foffset))
-      {
-        throw std::runtime_error("foffset was not finite");
-      }
     }
   }
 
-  virtual void GetSolutionIndices(std::vector< unsigned int >& v) const
+  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const
   {
     v.resize(0);
     v.push_back(0);
@@ -277,29 +265,29 @@ class IkSolution : public IkSolutionBase< T >
     }
   }
 
-  std::vector< IkSingleDOFSolutionBase< T > > _vbasesol;  ///< solution and their offsets if joints are mimiced
-  std::vector< int > _vfree;
+  std::vector<IkSingleDOFSolutionBase<T> > _vbasesol;  ///< solution and their offsets if joints are mimiced
+  std::vector<int> _vfree;
 };
 
 /// \brief Default implementation of \ref IkSolutionListBase
-template < typename T >
-class IkSolutionList : public IkSolutionListBase< T >
+template <typename T>
+class IkSolutionList : public IkSolutionListBase<T>
 {
 public:
-  virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos, const std::vector< int >& vfree)
+  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
   {
     size_t index = _listsolutions.size();
-    _listsolutions.push_back(IkSolution< T >(vinfos, vfree));
+    _listsolutions.push_back(IkSolution<T>(vinfos, vfree));
     return index;
   }
 
-  virtual const IkSolutionBase< T >& GetSolution(size_t index) const
+  virtual const IkSolutionBase<T>& GetSolution(size_t index) const
   {
     if (index >= _listsolutions.size())
     {
       throw std::runtime_error("GetSolution index is invalid");
     }
-    typename std::list< IkSolution< T > >::const_iterator it = _listsolutions.begin();
+    typename std::list<IkSolution<T> >::const_iterator it = _listsolutions.begin();
     std::advance(it, index);
     return *it;
   }
@@ -315,7 +303,7 @@ class IkSolutionList : public IkSolutionListBase< T >
   }
 
 protected:
-  std::list< IkSolution< T > > _listsolutions;
+  std::list<IkSolution<T> > _listsolutions;
 };
 }
 
@@ -358,12 +346,7 @@ typedef double IkReal;
    effector coordinate system.
  */
 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
-                          ikfast::IkSolutionListBase< IkReal >& solutions);
-
-/** \brief Similar to ComputeIk except takes OpenRAVE boost::shared_ptr<RobotBase::Manipulator>*
- */
-IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
-                           ikfast::IkSolutionListBase< IkReal >& solutions, void* pOpenRAVEManip);
+                          ikfast::IkSolutionListBase<IkReal>& solutions);
 
 /// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
 IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);

map_creator/include/map_creator/kinematics.h

@@ -7,7 +7,6 @@
 //#include "abb_irb2400_manipulator_ikfast_solver.cpp"
 //#include "ur5_ikfast.cpp"
 
-
 #include <stdio.h>
 #include <stdlib.h>
 #include <vector>

map_creator/include/map_creator/mh5_ikfast.cpp

@@ -20,6 +20,7 @@
 #define IKFAST_HAS_LIBRARY
 #include "ikfast.h"  // found inside share/openrave-X.Y/python/ikfast.h
 using namespace ikfast;
+using namespace std;
 
 // check if the included ikfast version matches what this file was compiled with
 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]

map_creator/src/create_capability_map.cpp

@@ -11,7 +11,8 @@
 #include <sstream>
 #include <map_creator/sphere_discretization.h>
 #include <map_creator/kinematics.h>
-#include<map_creator/hdf5_dataset.h>
+#include <map_creator/hdf5_dataset.h>
+#include <boost/format.hpp>
 
 //struct stat st;