diff --git a/manifest.xml b/manifest.xml index faa91ad..07679fe 100644 --- a/manifest.xml +++ b/manifest.xml @@ -10,6 +10,7 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 85a4dea..6811e18 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/MotionPlanningLibraries.cpp b/src/MotionPlanningLibraries.cpp index adcc6e3..d3fa798 100644 --- a/src/MotionPlanningLibraries.cpp +++ b/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 envireTrav = convertToEnvire(trav); + std::string uuid = "/trav_map"; + envireTrav->setUniqueId(uuid); + + if (!mpEnv) + mpEnv = boost::shared_ptr(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 trav_ptr = mpEnv->getItem(uuid); + if (trav_ptr) + mpEnv->detachItem(trav_ptr.get()); + + // Check for a frame node to reuse. + std::list 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 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 envTravGrid(new envire::TraversabilityGrid(numCells.x(), numCells.y(), + resolution.x(), resolution.y(), + localFrame.x(), localFrame.y())); + + std::vector 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 diff --git a/src/MotionPlanningLibraries.hpp b/src/MotionPlanningLibraries.hpp index 23dd2f1..6aab6d0 100644 --- a/src/MotionPlanningLibraries.hpp +++ b/src/MotionPlanningLibraries.hpp @@ -6,6 +6,7 @@ #include #include +#include #include "Config.hpp" #include "State.hpp" @@ -141,6 +142,7 @@ class MotionPlanningLibraries boost::shared_ptr mpPlanningLib; + boost::shared_ptr mpEnv; envire::TraversabilityGrid* mpTravGrid; boost::shared_ptr 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 convertToEnvire(const maps::grid::TraversabilityGrid& traversabilityGrid) const; + private: /** * Extracts the traversability map \a trav_map_id from the passed environment. diff --git a/test/test_MotionPlanning.cpp b/test/test_MotionPlanning.cpp index bc7a85d..8264835 100644 --- a/test/test_MotionPlanning.cpp +++ b/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 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 diff --git a/viz/MotionPlanningLibrariesSbplSplineVisualization.cpp b/viz/MotionPlanningLibrariesSbplSplineVisualization.cpp index 8adbf8a..d55a6df 100644 --- a/viz/MotionPlanningLibrariesSbplSplineVisualization.cpp +++ b/viz/MotionPlanningLibrariesSbplSplineVisualization.cpp @@ -121,7 +121,7 @@ void MotionPlanningLibrariesSbplSplineVisualization::addPrimitives(osg::Group* g const std::vector& prims = primitives.getPrimitiveForAngle(mAngleNum); for(const SplinePrimitive& prim : prims) { - if(prim.endAngle != mEndAngle) + if(static_cast(prim.endAngle) != mEndAngle) continue; if(const_cast(prim).spline.getCurvatureMax() > maxCurvature)