Updated MPL to support slam::maps by converting to envire.

manifest.xml

@@ -10,6 +10,7 @@
   <depend package="base/cmake" />
   <depend package="base/types" />
   <depend package="slam/envire" />
+  <depend package="slam/maps" />
   <depend package="external/ompl" />
   <depend package="external/sbpl" />
   <depend package="gui/vizkit3d" optional="1" />

src/CMakeLists.txt

@@ -36,7 +36,9 @@ rock_library(motion_planning_libraries
         ompl/validators/TravMapValidator.hpp 
         ompl/objectives/TravGridObjective.hpp
         ompl/spaces/SherpaStateSpace.hpp
-    DEPS_PKGCONFIG envire
+    DEPS_PKGCONFIG
+        envire
+        maps
         ompl
         sbpl
         base-types

src/MotionPlanningLibraries.cpp

@@ -12,9 +12,12 @@
 namespace motion_planning_libraries
 {
 
+Eigen::Affine3d MotionPlanningLibraries::mWorld2Local = Eigen::Affine3d::Identity();
+
 // PUBLIC
 MotionPlanningLibraries::MotionPlanningLibraries(Config config) : 
         mConfig(config),
+        mpEnv(nullptr),
         mpTravGrid(NULL), 
         mpTravData(),
         mpLastTravGrid(NULL),
@@ -197,6 +200,39 @@ bool MotionPlanningLibraries::setTravGrid(envire::Environment* env, std::string
     return true;
 }
 
+bool MotionPlanningLibraries::setTravGrid(const maps::grid::TraversabilityGrid& trav)
+{
+    boost::intrusive_ptr<envire::TraversabilityGrid> envireTrav = convertToEnvire(trav);
+    std::string uuid = "/trav_map";
+    envireTrav->setUniqueId(uuid);
+
+    if (!mpEnv)
+        mpEnv = boost::shared_ptr<envire::Environment>(new envire::Environment);
+
+    // Detach item if there is already a map in the environment.
+    // Not storing the returned intrusive_ptr causes the item to self delete.
+    boost::intrusive_ptr<envire::TraversabilityGrid> trav_ptr = mpEnv->getItem<envire::TraversabilityGrid>(uuid);
+    if (trav_ptr)
+        mpEnv->detachItem(trav_ptr.get());
+
+    // Check for a frame node to reuse.
+    std::list<envire::FrameNode*> frameNodes = mpEnv->getRootNode()->getChildren();
+    envire::FrameNode* travFrameNode;
+    if (frameNodes.empty())
+        travFrameNode = new envire::FrameNode();
+    else
+        travFrameNode = frameNodes.front();
+
+    mpEnv->attachItem(envireTrav.get());
+    mpEnv->getRootNode()->addChild(travFrameNode);
+    envireTrav->setFrameNode(travFrameNode);
+
+    // Set transform in case it was updated or hasn't been set yet.
+    travFrameNode->setTransform(mWorld2Local);
+
+    return setTravGrid(mpEnv.get(), uuid);
+}
+
 bool MotionPlanningLibraries::setStartState(struct State new_state) {
     if(mpPlanningLib == NULL) {
         LOG_WARN("Planning library has not been allocated yet");
@@ -746,6 +782,16 @@ bool MotionPlanningLibraries::getSbplMotionPrimitives(struct SbplMotionPrimitive
     }
 }
 
+void MotionPlanningLibraries::setWorld2Local(Eigen::Affine3d world2local)
+{
+    mWorld2Local = world2local;
+}
+
+Eigen::Affine3d MotionPlanningLibraries::getWorld2Local()
+{
+    return mWorld2Local;
+}
+
 bool MotionPlanningLibraries::world2grid(envire::TraversabilityGrid const* trav,
         base::samples::RigidBodyState const& world_pose, 
         base::samples::RigidBodyState& grid_pose,
@@ -929,4 +975,40 @@ void MotionPlanningLibraries::collectCellUpdates( envire::TraversabilityGrid* ol
     LOG_INFO("Number of unchanged cells %d", cell_counter_same);
 }
 
+boost::intrusive_ptr<envire::TraversabilityGrid> MotionPlanningLibraries::convertToEnvire(const maps::grid::TraversabilityGrid& traversabilityGrid) const
+{
+    maps::grid::Vector2ui numCells = traversabilityGrid.getNumCells();
+    maps::grid::Vector2d resolution = traversabilityGrid.getResolution();
+    maps::grid::Vector3d localFrame = traversabilityGrid.translation();
+
+    boost::intrusive_ptr<envire::TraversabilityGrid> envTravGrid(new envire::TraversabilityGrid(numCells.x(), numCells.y(),
+                                                                                                resolution.x(), resolution.y(),
+                                                                                                localFrame.x(), localFrame.y()));
+
+    std::vector<maps::grid::TraversabilityClass> travClasses = traversabilityGrid.getTraversabilityClasses();
+    envire::TraversabilityClass travClass;
+
+    for (size_t travClassId = 0; travClassId < travClasses.size(); ++travClassId)
+    {
+        travClass = envire::TraversabilityClass(travClasses.at(travClassId).getDrivability());
+        envTravGrid->setTraversabilityClass(travClassId, travClass);
+    }
+
+    uint8_t travClassId;
+    double probability;
+
+    uint8_t* trav_ptr = envTravGrid->getGridData(envTravGrid->TRAVERSABILITY).origin();
+    uint8_t* prob_ptr = envTravGrid->getGridData(envTravGrid->PROBABILITY).origin();
+    maps::grid::TraversabilityGrid::const_iterator it = traversabilityGrid.begin();
+    for (; it < traversabilityGrid.end(); ++it, ++trav_ptr, ++prob_ptr)
+    {
+        travClassId = it->getTraversabilityClassId();
+        probability = it->getProbability();
+        *trav_ptr = travClassId;
+        *prob_ptr = probability;
+    }
+
+    return envTravGrid;
+}
+
 } // namespace motion_planning_libraries

src/MotionPlanningLibraries.hpp

@@ -6,6 +6,7 @@
 #include <base/Trajectory.hpp>
 
 #include <envire/maps/TraversabilityGrid.hpp>
+#include <maps/grid/TraversabilityGrid.hpp>
 
 #include "Config.hpp"
 #include "State.hpp"
@@ -141,6 +142,7 @@ class MotionPlanningLibraries
 
     boost::shared_ptr<AbstractMotionPlanningLibrary> mpPlanningLib;
 
+    boost::shared_ptr<envire::Environment> mpEnv;
     envire::TraversabilityGrid* mpTravGrid;
     boost::shared_ptr<TravData> mpTravData;
     // Receives a copy of each trav grid, which is used for partial update testing.
@@ -152,6 +154,7 @@ class MotionPlanningLibraries
     bool mNewGoalReceived;
     double mLostX; // Used to trac discretization error.
     double mLostY;
+    static Eigen::Affine3d mWorld2Local;
 
  public: 
     enum MplErrors mError; 
@@ -167,6 +170,13 @@ class MotionPlanningLibraries
      * \todo "If possible cell updates should be used."
      */
     bool setTravGrid(envire::Environment* env, std::string trav_map_id);
+
+    /**
+     *  @overload allows to directly pass a maps::grid:TraversabilityGrid.
+     *  @param trav : Traversability grid to be set.
+     *  @returns true if the traversability grid was successfully set.
+     */
+    bool setTravGrid(const maps::grid::TraversabilityGrid& trav);
 
     inline bool travGridAvailable() {
         return mpTravGrid != NULL;
@@ -285,6 +295,19 @@ class MotionPlanningLibraries
     }
 
     bool getSbplMotionPrimitives(struct SbplMotionPrimitives& prims);
+
+    /**
+     * Set the transformation between world and local coordinates.
+     * @param world2local : transformation between the world frame and the local frame.
+     *                      Will be set to identity by default.
+     */
+    static void setWorld2Local(Eigen::Affine3d world2local);
+
+    /**
+     * Get the transformation between world and local coordinates.
+     * @returns : the transformation between the world frame and the local frame.
+     */
+    static Eigen::Affine3d getWorld2Local();
 
     /**
      * Converts the world pose to grid coordinates including the transformed orientation.
@@ -310,7 +333,15 @@ class MotionPlanningLibraries
     bool gridlocal2world(envire::TraversabilityGrid const* trav,
         base::samples::RigidBodyState const& grid_local_pose,
         base::samples::RigidBodyState& world_pose);
-
+
+    /**
+     * Converts a maps::grid::TraversabilityGrid to an envire::TraversabilityGrid as an
+     * intrusive_ptr so it can be passed to the motion planner.
+     * @param traversabilityGrid : traversabilityGrid to be converted.
+     * @returns an intrusive_ptr to the converted traversavility grid.
+     */
+    boost::intrusive_ptr<envire::TraversabilityGrid> convertToEnvire(const maps::grid::TraversabilityGrid& traversabilityGrid) const;
+
  private:
     /**
      * Extracts the traversability map \a trav_map_id from the passed environment.

test/test_MotionPlanning.cpp

@@ -71,6 +71,65 @@ BOOST_AUTO_TEST_CASE(sbpl_mprims)
     mprims.createPrimitives();
     mprims.storeToFile("test.mprim");
 }
+
+BOOST_AUTO_TEST_CASE(test_convertToEnvire)
+{
+    maps::grid::TraversabilityCell defaultCell = maps::grid::TraversabilityCell();
+    maps::grid::Vector2ui numCells(1000, 1500);
+    maps::grid::Vector2d resolution(0.1, 0.2);
+    maps::grid::TraversabilityGrid travGrid = maps::grid::TraversabilityGrid(numCells, resolution, defaultCell);
+    travGrid.translate(Eigen::Vector3d(1, 1, 0));
+
+    for (size_t i = 0; i < 10; ++i)
+    {
+        uint8_t retID;
+        travGrid.registerNewTraversabilityClass(retID, maps::grid::TraversabilityClass(0.1 * i));
+        BOOST_CHECK_EQUAL(retID, i);
+    }
+
+    travGrid.setTraversabilityAndProbability(0, 0.0, 0, 0);
+    travGrid.setTraversabilityAndProbability(1, 0.1, 0, 14);
+    travGrid.setTraversabilityAndProbability(2, 0.2, 9, 0);
+    travGrid.setTraversabilityAndProbability(3, 0.3, 9, 14);
+    travGrid.setTraversabilityAndProbability(4, 0.4, 4, 4);
+    travGrid.setTraversabilityAndProbability(5, 0.5, 7, 3);
+
+    motion_planning_libraries::MotionPlanningLibraries mpl = motion_planning_libraries::MotionPlanningLibraries();
+    boost::intrusive_ptr<envire::TraversabilityGrid> envireTravGrid = mpl.convertToEnvire(travGrid);
+
+    // Check number of cells.
+    BOOST_CHECK_EQUAL(envireTravGrid->getCellSizeX(), travGrid.getNumCells().x());
+    BOOST_CHECK_EQUAL(envireTravGrid->getCellSizeY(), travGrid.getNumCells().y());
+
+    // Check cell sizes.
+    BOOST_CHECK_EQUAL(envireTravGrid->getScaleX(), travGrid.getResolution().x());
+    BOOST_CHECK_EQUAL(envireTravGrid->getScaleY(), travGrid.getResolution().y());
+
+    // Check size (redundant but why not).
+    BOOST_CHECK_EQUAL(envireTravGrid->getSizeX(), travGrid.getSize().x());
+    BOOST_CHECK_EQUAL(envireTravGrid->getSizeY(), travGrid.getSize().y());
+
+    // Check offset.
+    BOOST_CHECK_EQUAL(envireTravGrid->getOffsetX(), travGrid.translation().x());
+    BOOST_CHECK_EQUAL(envireTravGrid->getOffsetY(), travGrid.translation().y());
+
+    // Check classes.
+    BOOST_CHECK_EQUAL(envireTravGrid->getTraversabilityClasses().size(), travGrid.getTraversabilityClasses().size());
+    for (size_t i = 0; i < envireTravGrid->getTraversabilityClasses().size(); ++i)
+    {
+        BOOST_CHECK_EQUAL(envireTravGrid->getTraversabilityClass(i).getDrivability(), travGrid.getTraversabilityClass(i).getDrivability());
+    }
+
+    // Check traversability and probability.
+    for (size_t x = 0; x < envireTravGrid->getCellSizeX(); ++x)
+    {
+        for (size_t y = 0; y < envireTravGrid->getCellSizeY(); ++y)
+        {
+            BOOST_CHECK_EQUAL(envireTravGrid->getTraversability(x, y).getDrivability(), travGrid.getTraversability(x, y).getDrivability());
+            BOOST_CHECK_CLOSE(envireTravGrid->getProbability(x, y), travGrid.getProbability(x, y), 0.001);
+        }
+    }
+}
 
 #if 0
 

viz/MotionPlanningLibrariesSbplSplineVisualization.cpp

@@ -121,7 +121,7 @@ void MotionPlanningLibrariesSbplSplineVisualization::addPrimitives(osg::Group* g
   const std::vector<SplinePrimitive>& prims = primitives.getPrimitiveForAngle(mAngleNum);
   for(const SplinePrimitive& prim : prims)
   {
-    if(prim.endAngle != mEndAngle)
+    if(static_cast<int>(prim.endAngle) != mEndAngle)
       continue;
 
     if(const_cast<SplinePrimitive&>(prim).spline.getCurvatureMax() > maxCurvature)