diff --git a/.gitignore b/.gitignore index 8e1074e7f4c6a6cf54540dd40fe64c894f8cac6f..8ef807fe05bb2b817100dc49165f28ce65ffc2ad 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,7 @@ # This is generated by catkin /CMakeLists.txt + +# Catkin Ignore files for "blacklisting" packages +CATKIN_IGNORE + diff --git a/crosbot/CMakeLists.txt b/crosbot/CMakeLists.txt index 18fb045e8296d162b38fafc1426f52bb6df47eca..35e280ee38b514cbc0f60e3fbe1b8c593a44c1a6 100644 --- a/crosbot/CMakeLists.txt +++ b/crosbot/CMakeLists.txt @@ -55,8 +55,11 @@ catkin_package( LIBRARIES crosbot CATKIN_DEPENDS roscpp tf geometry_msgs sensor_msgs # DEPENDS system_lib + CFG_EXTRAS ${PROJECT_NAME}-extras.cmake ) +INCLUDE ( cmake/${PROJECT_NAME}-extras.cmake.in ) + ########### ## Build ## ########### diff --git a/crosbot/cmake/crosbot-extras.cmake.in b/crosbot/cmake/crosbot-extras.cmake.in new file mode 100644 index 0000000000000000000000000000000000000000..b138e9b627e2646e347d53dbe95d09832fb31490 --- /dev/null +++ b/crosbot/cmake/crosbot-extras.cmake.in @@ -0,0 +1,8 @@ +if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU") + execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) + + if (GCC_VERSION VERSION_GREATER 4.7 OR GCC_VERSION VERSION_EQUAL 4.7) + SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11" ) + endif () +endif () diff --git a/crosbot/include/crosbot/data.hpp b/crosbot/include/crosbot/data.hpp index da588f528cbc7865cf4e27c7d1df0babe8234dfa..2c5d80178110fca71d39dd986b57fbe151d2d4be 100644 --- a/crosbot/include/crosbot/data.hpp +++ b/crosbot/include/crosbot/data.hpp @@ -383,7 +383,7 @@ class Data : public HandledObject { public: Time timestamp; - Data(Time stamp = Time::now()) : timestamp(stamp) {} + Data(Time stamp = Time::now()) : HandledObject(), timestamp(stamp) {} virtual ~Data() {} }; diff --git a/crosbot/include/crosbot/geometry.hpp b/crosbot/include/crosbot/geometry.hpp index bdb51312e13ad245995f64e4a899efcc352155e4..98c30679ee8ecf238583fc5553b78b0c5bf663d7 100644 --- a/crosbot/include/crosbot/geometry.hpp +++ b/crosbot/include/crosbot/geometry.hpp @@ -13,7 +13,7 @@ #endif #include -#include +//#include #include #include @@ -122,7 +122,7 @@ public: } inline bool hasNAN() const { - return std::isnan(x) || std::isnan(y); + return ::isnan(x) || ::isnan(y); } inline bool isFinite() const { @@ -246,7 +246,7 @@ public: } inline bool hasNAN() const { - return std::isnan(x) || std::isnan(y) || std::isnan(z); + return ::isnan(x) || ::isnan(y) || ::isnan(z); } inline bool isFinite() const { @@ -385,7 +385,7 @@ public: } inline bool hasNAN() const { - return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w); + return ::isnan(x) || ::isnan(y) || ::isnan(z) || ::isnan(w); } inline bool isFinite() const { @@ -466,7 +466,7 @@ public: Point2D position; double orientation; - Pose2D() {} + Pose2D() : orientation(0) {} Pose2D(const Point2D& position, const double& orientation) : position(position), orientation(orientation) {} Pose2D(const Pose2D& p) : position(p.position), orientation(p.orientation) {} @@ -486,12 +486,12 @@ public: } inline bool hasNAN() const { - return position.hasNAN() || std::isnan(orientation); + return position.hasNAN() || ::isnan(orientation); } inline bool isFinite() const { return position.isFinite() && - !std::isnan(orientation) && + !::isnan(orientation) && orientation != INFINITY && orientation != -INFINITY; } @@ -742,11 +742,11 @@ inline bool operator!=(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& * Tests that a value isn't corrupt. */ inline bool hasNAN(const geometry_msgs::Point& p) { - return std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z); + return ::isnan(p.x) || ::isnan(p.y) || ::isnan(p.z); } inline bool hasNAN(const geometry_msgs::Quaternion& q) { - return std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w); + return ::isnan(q.x) || ::isnan(q.y) || ::isnan(q.z) || ::isnan(q.w); } inline bool hasNAN(const geometry_msgs::Pose& p) { diff --git a/crosbot/include/crosbot/handle.hpp b/crosbot/include/crosbot/handle.hpp index fcfd70257b117fdc72574291d02d2bcaafee66e4..0d503604fc52f43f642861f7e489a4451f2ebfd4 100644 --- a/crosbot/include/crosbot/handle.hpp +++ b/crosbot/include/crosbot/handle.hpp @@ -14,7 +14,13 @@ #include #include #include + +#define REF_COUNT_TYPE long int + +#if __cplusplus < 201103L + #include +#include namespace crosbot { @@ -23,21 +29,26 @@ namespace crosbot { */ class HandledObject { private: - int _refCount; + REF_COUNT_TYPE _refCount; Mutex _refMutex; public: HandledObject() { _refCount = 0; } - + virtual ~HandledObject() { }; - + void __incRef() { Lock lock(_refMutex); ++_refCount; } + void __incRef() const { + Lock lock(*((Mutex *)&_refMutex)); + ++*((int *)&_refCount); + } + void __decRef() { Lock lock(_refMutex); --_refCount; @@ -50,11 +61,89 @@ public: } } - int __getRef() { + void __decRef() const { + Lock lock(*((Mutex *)&_refMutex)); + --*((int *)&_refCount); + if (_refCount < 0) { + fprintf(stderr, "How did the ref count go below zero?\n"); + } + if (_refCount == 0) { + lock.unlock(); + delete this; + } + } + + REF_COUNT_TYPE __getRef() const { return _refCount; } }; +} // namespace crosbot + +#else + +#include +#include + +namespace crosbot { + +/** + * A reference counted self managed object. + */ +class HandledObject { +private: + std::atomic< REF_COUNT_TYPE > _refCount; +public: + HandledObject() { + _refCount = 0; + } + + HandledObject(const HandledObject&) { + _refCount = 0; + } + + virtual ~HandledObject() { + }; + + void __incRef() { + ++_refCount; + } + + void __incRef() const { + ++*((std::atomic< int > *)&_refCount); + } + + void __decRef() { + --_refCount; + if (_refCount < 0) { + fprintf(stderr, "How did the ref count go below zero?\n"); + } + if (_refCount == 0) { + delete this; + } + } + + void __decRef() const { + --*((std::atomic< int > *)&_refCount); + if (_refCount < 0) { + fprintf(stderr, "How did the ref count go below zero?\n"); + } + if (_refCount == 0) { + delete this; + } + } + + REF_COUNT_TYPE __getRef() const { + return _refCount.load(); + } +}; + +} // namespace crosbot + +#endif + +namespace crosbot { + inline bool operator==(const HandledObject& lhs, const HandledObject& rhs) { return &(lhs) == &(rhs); diff --git a/crosbot/include/crosbot/thread.hpp b/crosbot/include/crosbot/thread.hpp index d8d2bde3105e85005eee7821b067440ab62a353d..b16e967aa0566fc7e4aba7f3e1bf19828450da9d 100644 --- a/crosbot/include/crosbot/thread.hpp +++ b/crosbot/include/crosbot/thread.hpp @@ -14,7 +14,7 @@ namespace crosbot { -#define DEFAULT_WAIT_FOR_THREAD_CLOSE 10000 +#define DEFAULT_WAIT_FOR_THREAD_CLOSE 10000 // Milliseconds struct _ThreadData; /** @@ -167,6 +167,7 @@ public: rwlockPtr = NULL; mutexPtr->lock(); locked = true; + write = false; } Lock(ReadWriteLock& rwlock, bool write = false) { diff --git a/crosbot_explore/CMakeLists.txt b/crosbot_explore/CMakeLists.txt index a9fa427660ec1d83fa8ff802a6611145b2a52ebe..ec7bccadc0e0764879326f7a9ae1dc946f3cb3d0 100644 --- a/crosbot_explore/CMakeLists.txt +++ b/crosbot_explore/CMakeLists.txt @@ -73,11 +73,13 @@ add_library(crosbot_explore ## Declare a cpp executable add_executable(astar src/nodes/astar.cpp) add_executable(explorer src/nodes/explorer.cpp) +add_executable(move src/nodes/move.cpp) ## Add dependencies to the executable add_dependencies(crosbot_explore ${PROJECT_NAME}_gencpp) add_dependencies(astar ${PROJECT_NAME} crosbot_explore) add_dependencies(explorer ${PROJECT_NAME} crosbot_explore) +add_dependencies(move ${PROJECT_NAME} crosbot_explore) ## Specify libraries to link a library or executable target against target_link_libraries(crosbot_explore @@ -90,6 +92,9 @@ target_link_libraries(astar target_link_libraries(explorer ${catkin_LIBRARIES} crosbot_explore ) +target_link_libraries(move + ${catkin_LIBRARIES} crosbot_explore +) ############# ## Install ## @@ -103,7 +108,7 @@ target_link_libraries(explorer # ) ## Mark executables and/or libraries for installation -install(TARGETS crosbot_explore astar +install(TARGETS crosbot_explore astar move ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/crosbot_explore/include/crosbot_explore/astar.hpp b/crosbot_explore/include/crosbot_explore/astar.hpp index 9d6b24d6d8be90fa6ef64c5dcf28d2950fdeff32..d72868d9ca04d3e14ab7936edfd0dc865e22378b 100644 --- a/crosbot_explore/include/crosbot_explore/astar.hpp +++ b/crosbot_explore/include/crosbot_explore/astar.hpp @@ -15,7 +15,7 @@ namespace crosbot { #define DEFAULT_SKELETON_DISCOUNT 0.5 #define DEFAULT_RESTRICTED_RISK INFINITY -#define DEFAULT_RISK_K 5 // 3 +#define DEFAULT_RISK_K 25 // 5 // 3 typedef std::vector Plan; typedef std::vector CellPath; @@ -74,7 +74,7 @@ public: k(DEFAULT_RISK_K), cull(0), maxExpandC(20), expandScale(10) {} - CellPath getCellPath(VoronoiGridPtr grid, Index2D start, Index2D goal, ImagePtr); + virtual CellPath getCellPath(VoronoiGridPtr grid, Index2D start, Index2D goal, ImagePtr); }; } // namespace crosbot diff --git a/crosbot_explore/include/crosbot_explore/explorer.hpp b/crosbot_explore/include/crosbot_explore/explorer.hpp index 1dd0cafdf06d9c64cb91ecd3d0db465036803eb3..491a9aa834c9e1569b995ac5a6f295e180e2bcf5 100644 --- a/crosbot_explore/include/crosbot_explore/explorer.hpp +++ b/crosbot_explore/include/crosbot_explore/explorer.hpp @@ -156,10 +156,59 @@ protected: return true; } + inline double cellTraversibleRisk(const VoronoiGrid& voronoi, const Index2D& cell, const Index2D& pov) const { + double d = cell.distanceTo(pov); + double dx = (cell.x - pov.x) / d, + dy = (cell.y - pov.y) / d; + + double risk = 0; uint32_t stepCount = 0; + double restrictScale = voronoiConstraints.restricted / voronoi.resolution, + partialScale = (voronoiConstraints.partial - voronoiConstraints.restricted) / voronoi.resolution, + partialMax = voronoiConstraints.partial / voronoi.resolution, + expansionScale = voronoiConstraints.expand / voronoi.resolution, + expansionMax = (voronoiConstraints.expand + voronoiConstraints.partial) / voronoi.resolution; + for (double r = 0; r <= d; r += search.rayStep) { + Index2D rayCell(pov.x + dx*r, pov.y + dy*r); + + if (!CELL_IS_VALID(rayCell, voronoi)) + return INFINITY; + + const VoronoiGrid::VoronoiCell& vCell = voronoi.getVoronoiCell(rayCell); + + if (vCell.status == VoronoiGrid::VoronoiCell::Wall || + vCell.status == VoronoiGrid::VoronoiCell::Restricted || + vCell.status == VoronoiGrid::VoronoiCell::NotVisible) + return INFINITY; + + ++stepCount; + if (vCell.status & VoronoiGrid::VoronoiCell::Vacant) { + risk += 0.0; + } else if (vCell.status & VoronoiGrid::VoronoiCell::PatiallyRestricted) { + if (vCell.distanceFromWall > partialMax) + risk += 2; + else + risk += 2 + (partialMax - vCell.distanceFromWall) / partialScale; + } else { + if (vCell.distanceFromWall > expansionMax) + risk += 0.0; + else { + risk += (expansionMax - vCell.distanceFromWall) / expansionScale; + } + } + } + if (stepCount == 0) { + return INFINITY; + } + return risk / stepCount; + } + // functions for the wall follow implementation virtual Point findWall(const VoronoiGrid& voronoi, const Pose& robot, double startAngle); virtual Index2D findFirstSkeletonCell(const VoronoiGrid& voronoi, const Pose& robot, double startAngle); virtual Index2D findNextSkeletonCell(const VoronoiGrid& voronoi, const Index2D currentCell, const Index2D previous); + virtual Index2D findSaferSkeletonCell(const VoronoiGrid& voronoi, const Index2D currentCell, const Index2D robot); + virtual Index2D findSaferCell(const VoronoiGrid& voronoi, const Index2D currentCell, const Index2D robot); +// virtual void findSaferTarget(double& voronoiAngle, double& voronoiDistance, const Pose& robot); // functions for the waypoint follow implementation virtual double traversibleDistance(const VoronoiGrid& voronoi, const Point position, const double angle, const double max = INFINITY); diff --git a/crosbot_explore/launch/crosbot-fastslam.gui b/crosbot_explore/launch/crosbot-fastslam.gui new file mode 100644 index 0000000000000000000000000000000000000000..2499a8666e14b5a2bad6bff47db869b5c28b2629 --- /dev/null +++ b/crosbot_explore/launch/crosbot-fastslam.gui @@ -0,0 +1,11 @@ + + + + + + + \ No newline at end of file diff --git a/crosbot_explore/launch/crosbot-fastslam.launch b/crosbot_explore/launch/crosbot-fastslam.launch new file mode 100644 index 0000000000000000000000000000000000000000..e86a6118350b97ad910c417dfecd4331603eaa90 --- /dev/null +++ b/crosbot_explore/launch/crosbot-fastslam.launch @@ -0,0 +1,12 @@ + + + + + + odom_frame: /odom + + + + + + diff --git a/crosbot_explore/launch/explore.launch b/crosbot_explore/launch/explore.launch new file mode 100644 index 0000000000000000000000000000000000000000..fe24f47e54eaa7a5b5ec8439e883e7d29d2b7d50 --- /dev/null +++ b/crosbot_explore/launch/explore.launch @@ -0,0 +1,19 @@ + + + + + + + + + + maxVel: 0.25 + voronoi: + restrict: 0.20 + partial: 0.25 + orphan: 10 + search: + distance: 0.40 + + + \ No newline at end of file diff --git a/crosbot_explore/launch/move.launch b/crosbot_explore/launch/move.launch new file mode 100644 index 0000000000000000000000000000000000000000..6cc0c6124f6993847366f43eaf4cfc08fdba87c8 --- /dev/null +++ b/crosbot_explore/launch/move.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + voronoi: + restrict: 0.20 + partial: 0.25 + + + diff --git a/crosbot_explore/src/astar.cpp b/crosbot_explore/src/astar.cpp index 6a34f6cbccbd44174e16f1d2c2035547b623195e..69c074c2d9532563235d122d25a28ea53be3cccf 100644 --- a/crosbot_explore/src/astar.cpp +++ b/crosbot_explore/src/astar.cpp @@ -9,6 +9,8 @@ #include #include +#include + namespace crosbot { Plan PathPlanner::getPath(VoronoiGridPtr voronoi, Pose start, Pose goal, ImagePtr image) { @@ -37,15 +39,15 @@ Plan PathPlanner::getPath(VoronoiGridPtr voronoi, Pose start, Pose goal, ImagePt else if ((unsigned int)startCell.y >= voronoi->height) startCell.y = voronoi->height - 1; - if (startCell.x < 0) - startCell.x = 0; - else if ((unsigned int)startCell.x >= voronoi->width) - startCell.x = voronoi->width - 1; + if (endCell.x < 0) + endCell.x = 0; + else if ((unsigned int)endCell.x >= voronoi->width) + endCell.x = voronoi->width - 1; - if (startCell.y < 0) - startCell.y = 0; - else if ((unsigned int)startCell.y >= voronoi->height) - startCell.y = voronoi->height - 1; + if (endCell.y < 0) + endCell.y = 0; + else if ((unsigned int)endCell.y >= voronoi->height) + endCell.y = voronoi->height - 1; CellPath cells = getCellPath(voronoi, startCell, endCell, image); diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp index e030fbc0e3f3519fb4e23c6f76f7843622001b29..880ca427127628c5c769515ffd071ba7ca5e56fc 100644 --- a/crosbot_explore/src/explorer.cpp +++ b/crosbot_explore/src/explorer.cpp @@ -144,7 +144,9 @@ Pose Explorer::findWallFollowTarget(const VoronoiGrid& voronoi, const Pose& robo return Pose(INFINITY, INFINITY, INFINITY); } - return Pose(voronoi.getPosition(currentCell), Quaternion()); + Index2D saferCell = findSaferSkeletonCell(voronoi, currentCell, robotCell); + + return Pose(voronoi.getPosition(saferCell), Quaternion()); } Point Explorer::findWall(const VoronoiGrid& voronoi, const Pose& robot, double startAngle) { @@ -323,6 +325,267 @@ Index2D Explorer::findNextSkeletonCell(const VoronoiGrid& voronoi, const Index2D return Index2D(-1,-1); } +Index2D Explorer::findSaferSkeletonCell(const VoronoiGrid& voronoi, const Index2D currentCell, const Index2D robot) { + uint32_t n = 0; + Index2D safeCell = currentCell; + if (currentCell == robot) + return currentCell; + +// double currentRisk = cellTraversibleRisk(voronoi, currentCell, robot); +// Index2D searchOrder[4]; +// searchOrder[0] = Index2D(0,1); +// searchOrder[1] = Index2D(0,-1); +// searchOrder[2] = Index2D(1,0); +// searchOrder[3] = Index2D(-1,0); +// +// bool riskDecreasing = true; +// for (; n < search.maxIterations && riskDecreasing; ++n) { +// riskDecreasing = false; +// for (int ne = 0; ne < 4; ++ne) { +// Index2D neighbour = safeCell + searchOrder[ne]; +// +// if (!CELL_IS_VALID(neighbour, voronoi)) +// continue; +// +// const VoronoiGrid::VoronoiCell& cell = voronoi.cells[neighbour.y * voronoi.width + neighbour.x]; +// if (!(cell.status & VoronoiGrid::VoronoiCell::Skeleton)) +// continue; +// +// double neighbourRisk = cellTraversibleRisk(voronoi, neighbour, robot); +// neighbourRisk *= 1 + neighbour.distanceTo(currentCell) / currentCell.distanceTo(robot); +// if (neighbourRisk < currentRisk) { +// safeCell = neighbour; +// currentRisk = neighbourRisk; +// riskDecreasing = true; +// } +// } +// } + + int32_t windowSize = ceil(search.searchDistance / voronoi.resolution) + 1; + double restrictedC = (voronoiConstraints.restricted / voronoi.resolution), + maxExpandC = ((voronoiConstraints.partial + voronoiConstraints.expand) / voronoi.resolution); + + double maxScore = 0, currentScore = -INFINITY, currentD = currentCell.distanceTo(robot); + for (int y = -windowSize; y <= windowSize; ++y) { + for (int x = -windowSize; x <= windowSize; ++x) { + Index2D cell2Check = currentCell + Index2D(x,y); + + if (!CELL_IS_VALID(cell2Check, voronoi)) + continue; + + const VoronoiGrid::VoronoiCell& vCell = voronoi.getVoronoiCell(cell2Check); + if (!(vCell.status & VoronoiGrid::VoronoiCell::Skeleton)) + continue; + double d = cell2Check.distanceTo(robot), score = 0; + double dx = (cell2Check.x - robot.x) / d, + dy = (cell2Check.y - robot.y) / d; + + int count = 0; + for (double r = 0; r <= d; r += search.rayStep) { + Index2D rayCell(robot.x + dx*r, robot.y + dy*r); + + if (!CELL_IS_VALID(rayCell, voronoi)) + continue; + + const VoronoiGrid::VoronoiCell& rCell = voronoi.getVoronoiCell(rayCell); + if ((rCell.status & VoronoiGrid::VoronoiCell::Wall) || + (rCell.status & VoronoiGrid::VoronoiCell::Restricted)) { + score = 0; + break; + } + + // The score for a raytraced cell is as 0..1 value representing distance from wall + double d = (rCell.distanceFromWall - restrictedC) / maxExpandC; + if (d > 1) + d = 1; + double cellScore; +// if (rCell.status & VoronoiGrid::VoronoiCell::Skeleton) { +// cellScore = 1.2 * d; +// } else + if (rCell.status & VoronoiGrid::VoronoiCell::Vacant) { + cellScore = 1; + } else if (rCell.status & VoronoiGrid::VoronoiCell::PatiallyRestricted) { + cellScore = 0.5 * d; + } else { + cellScore = d; + } + + score += cellScore; + ++count; +// ROS_INFO("Score Sum (%d, %d)(%.3lf) - %.2lf, %.2lf - %.2lf, %.2lf", cell2Check.x, cell2Check.y, score, +// rCell.distanceFromWall, restrictedC); + } + + if (count == 0) + continue; + + // Score for a cell is the mean of the intermediate ray traced cells + score = score / count; + + // Adjust score based on distance from the goal + double adjustment = 1 - pow((currentCell.distanceTo(cell2Check) / currentD), 2); + score *= adjustment; + +// ROS_INFO("Score (%d, %d)(%.3lf)", cell2Check.x, cell2Check.y, score); + if (currentCell == cell2Check) { + currentScore = score; + } + + if (score > maxScore) { + safeCell = cell2Check; + maxScore = score; + } + } + } + +// ROS_WARN("Preferring (%d, %d)(%.3lf) over (%d, %d)(%.3lf)", +// safeCell.x, safeCell.y, maxScore, +// currentCell.x, currentCell.y, currentScore); + return safeCell; +} + +Index2D Explorer::findSaferCell(const VoronoiGrid& voronoi, const Index2D currentCell, const Index2D robot) { + double voronoiDistance = currentCell.distanceTo(robot) * voronoi.resolution; + double score, maxScore = 0, meanScore, sint, cost, xf, yf, cosA; + int c, count, xi, yi; + + double vdSq = voronoiDistance*voronoiDistance, vd2cosA; + Index2D vectorCurrent = currentCell - robot; + + Index2D saferCell = currentCell; + int32_t windowSize = ceil(search.searchDistance / voronoi.resolution) + 1; + + double restrictedC = (voronoiConstraints.restricted / voronoi.resolution), + maxExpandC = ((voronoiConstraints.partial + voronoiConstraints.expand) / voronoi.resolution); + + for (int y = -windowSize; y <= windowSize; ++y) { + for (int x = -windowSize; x <= windowSize; ++x) { + Index2D cell2Check = currentCell + Index2D(x,y); + + if (!CELL_IS_VALID(cell2Check, voronoi)) + continue; + + const VoronoiGrid::VoronoiCell& vCell = voronoi.getVoronoiCell(cell2Check); + if ((vCell.status & VoronoiGrid::VoronoiCell::Wall) || + (vCell.status & VoronoiGrid::VoronoiCell::Restricted)) + continue; + + double d = cell2Check.distanceTo(robot); + double dx = (cell2Check.x - robot.x) / d, + dy = (cell2Check.y - robot.y) / d; + + score = count = 0; + for (double r = 0; r <= d; r += search.rayStep) { + Index2D rayCell(robot.x + dx*r, robot.y + dy*r); + + if (!CELL_IS_VALID(rayCell, voronoi)) + continue; + + const VoronoiGrid::VoronoiCell& rCell = voronoi.getVoronoiCell(rayCell); + if ((rCell.status & VoronoiGrid::VoronoiCell::Wall) || + (rCell.status & VoronoiGrid::VoronoiCell::Restricted)) { + score = 0; break; + } + double d = (rCell.distanceFromWall - restrictedC) / maxExpandC; + if (d > 1) + d = 1; + double cellScore; + if (rCell.status & VoronoiGrid::VoronoiCell::Skeleton) { + cellScore = 1.2 * d; + } else if (rCell.status & VoronoiGrid::VoronoiCell::Vacant) { + cellScore = 1; + } else if (rCell.status & VoronoiGrid::VoronoiCell::PatiallyRestricted) { + cellScore = 0.5 * d; + } else { + cellScore = d; + } + + score += cellScore; + ++count; + } + + if (count == 0) + continue; + + meanScore = score / count; + + // adjust meanScore based on distance to goal + + Index2D vectorCheck = cell2Check - robot; + double theta = atan2(currentCell.y - robot.y, currentCell.x - robot.x) - + atan2(cell2Check.y - robot.y, cell2Check.x - robot.x); + cosA = cos(theta); + vd2cosA = voronoiDistance*cosA*2; + double r = currentCell.distanceTo(cell2Check); + + d = sqrt(vdSq + r*r - r*vd2cosA); + meanScore = (meanScore / exp2(2*d*search.searchDistance/voronoiDistance)); + + if (meanScore > maxScore) { + saferCell = cell2Check; + maxScore = meanScore; + } + } + } + + return saferCell; +} + +//void Explorer::findSaferTarget(double& voronoiAngle, double& voronoiDistance, const Pose& robot) { +// double preferredAngle = voronoiAngle, preferredDist = voronoiDistance; +// double score, maxScore = 0, meanScore, sint, cost, xf, yf, cosA; +// +// double vdSq = voronoiDistance*voronoiDistance, vd2cosA; +// +// int c, count, xi, yi; +// VoronoiCell *cell = NULL; +// +// for (double theta = -angleAcceptDiff; theta <= angleAcceptDiff; theta += searchRes) { +// sint = sin(theta + voronoiAngle + localMap->robotPose.o.yaw), +// cost = cos(theta + voronoiAngle + localMap->robotPose.o.yaw), +// cosA = cos(theta); +// +// vd2cosA = voronoiDistance*cosA*2; +// +// score = count = 0; +// +// for (double ray = searchStep; ray <= searchDistance; ray += searchStep) { +// GET_CELL_FOR_RAY(ray, c); +// if (c == -1 || cell->status == VoronoiCell::Occupied || cell->status == VoronoiCell::Obstacle) { +// break; +// } +// double d = (cell->voronoiDistance - localMap->restrictedC) / localMap->maxExpansionC; +// if (d > 1) +// d = 1; +// double cellScore; +// if (cell->status == VoronoiCell::Horizon) { +// cellScore = 1.2 * d; +// } else if (cell->status == VoronoiCell::Vacant) { +// cellScore = 1; +// } else { +// cellScore = d; +// } +// +// score += cellScore; +// ++count; +// +// meanScore = score / count; +// +// // adjust meanScore based on distance to goal +// d = sqrt(vdSq + ray*ray - ray*vd2cosA); +// meanScore = (meanScore / exp2(2*d*searchDistance/voronoiDistance)); +// +// if (meanScore > maxScore) { +// preferredAngle = theta + voronoiAngle; preferredDist = ray; +// maxScore = meanScore; +// } +// } +// } +// +// NORMALISE_ANGLE(preferredAngle); +// voronoiAngle = preferredAngle; voronoiDistance = preferredDist; +//} + Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) { if (search.waypoints.size() == 0) search.setWaypoints(SearchParameters::createStair(), robot); diff --git a/crosbot_explore/src/nodes/astar.cpp b/crosbot_explore/src/nodes/astar.cpp index c2aa670b6be8c2c436ad944af1908644a637fe3c..3469e274f8de4dca537a4b2020e35cdb81428d5f 100644 --- a/crosbot_explore/src/nodes/astar.cpp +++ b/crosbot_explore/src/nodes/astar.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -134,12 +135,12 @@ public: // TODO: read voronoi constraints ConfigElementPtr voronoiCfg = cfg->getChild("voronoi"); - if (voronoiCfg != NULL) { - voronoiConstraints.restricted = voronoiCfg->getParamAsDouble("restrict", voronoiConstraints.restricted); - voronoiConstraints.partial = voronoiCfg->getParamAsDouble("partial", voronoiConstraints.partial); - voronoiConstraints.expand = voronoiCfg->getParamAsDouble("expand", voronoiConstraints.expand); - voronoiConstraints.orphanThreshold = voronoiCfg->getParamAsInt("orphan", voronoiConstraints.orphanThreshold); - } + if (voronoiCfg != NULL) { + voronoiConstraints.restricted = voronoiCfg->getParamAsDouble("restrict", voronoiConstraints.restricted); + voronoiConstraints.partial = voronoiCfg->getParamAsDouble("partial", voronoiConstraints.partial); + voronoiConstraints.expand = voronoiCfg->getParamAsDouble("expand", voronoiConstraints.expand); + voronoiConstraints.orphanThreshold = voronoiCfg->getParamAsInt("orphan", voronoiConstraints.orphanThreshold); + } gridSub = nh.subscribe("map", 1, &AstarNode::callbackOccGrid, this); historySub = nh.subscribe("history", 1, &AstarNode::callbackHistory, this); diff --git a/crosbot_explore/src/nodes/explorer.cpp b/crosbot_explore/src/nodes/explorer.cpp index 2a6e72a5f10aaf6e3e16ddf49e14672f3559a2c7..7de487723d00920fdf97a02c38d65877bb78fcbb 100644 --- a/crosbot_explore/src/nodes/explorer.cpp +++ b/crosbot_explore/src/nodes/explorer.cpp @@ -26,7 +26,7 @@ using namespace crosbot_explore; class ExplorerNode : public Explorer { protected: - VoronoiGrid::Constraints voronoiConstraints; +// VoronoiGrid::Constraints voronoiConstraints; std::string baseFrame; ros::Subscriber gridSub, historySub; @@ -87,7 +87,7 @@ public: } bool callbackSetMode(crosbot_explore::SetMode::Request& req, crosbot_explore::SetMode::Response& res) { - switch (req.mode.data) { + switch (req.mode) { case 1: resume(); break; case 2: @@ -96,6 +96,8 @@ public: case 3: search.side = SearchParameters::Right; search.strategy = SearchParameters::WallFollow; break; + case 4: + search.strategy = SearchParameters::Waypoint; break; case 0: default: pause(); break; } @@ -120,6 +122,7 @@ public: ConfigElementPtr searchCfg = cfg->getChild("search"); if (searchCfg != NULL) { search.searchDistance = searchCfg->getParamAsDouble("search", search.searchDistance); + search.searchDistance = searchCfg->getParamAsDouble("distance", search.searchDistance); search.searchAngle = searchCfg->getParamAsDouble("angle", search.searchAngle); } if (cfg != NULL) { @@ -129,7 +132,7 @@ public: gridSub = nh.subscribe("map", 1, &ExplorerNode::callbackOccGrid, this); historySub = nh.subscribe("history", 1, &ExplorerNode::callbackHistory, this); - imagePub = nh.advertise("image", 1); + imagePub = nh.advertise("explore/image", 1); velPub = nh.advertise("cmd_vel", 1); statusPub = nh.advertise("status", 1); waypointSrv = nh.advertiseService("/explore/follow_path", &ExplorerNode::callbackFollowPath, this); @@ -167,7 +170,7 @@ public: tf::StampedTransform transform; try { - tfListener.waitForTransform(mapFrame, baseFrame, ros::Time(0), ros::Duration(2)); +// tfListener.waitForTransform(mapFrame, baseFrame, ros::Time(0), ros::Duration(5.0)); tfListener.lookupTransform(mapFrame, baseFrame, ros::Time(0), transform); } catch (tf::TransformException& e) { LOG("ExplorerNode::getLatestPose(): Exception caught(%s).\n", e.what()); diff --git a/crosbot_explore/src/nodes/move.cpp b/crosbot_explore/src/nodes/move.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9034afc3f5deed2fde04655e11b38e66c1d0e371 --- /dev/null +++ b/crosbot_explore/src/nodes/move.cpp @@ -0,0 +1,255 @@ +/* + * move.cpp + * + * Created on: 27/08/2014 + * Author: mmcgill + */ + +#include +#include +#include +#include +#include + + +#include +#include +#include +#include + + +namespace crosbot { + +#define DEFAULT_BASEFRAME "base_link" + +class MoveNode { +protected: + ros::Subscriber goalSub, mapSub; + + class PlanningThread : public Thread { + public: + Pose goal; + nav_msgs::OccupancyGridConstPtr latestMap; + tf::TransformListener tfListener; + ReadWriteLock dataLock; + + crosbot::AStarPlanner planner; + bool operating, explorerPaused; + ros::Publisher debugPub, pathPub; + ros::ServiceClient followPathSrv, setModeSrv; + + VoronoiGrid::Constraints voronoiConstraints; + std::string baseFrame; + + uint32_t updateInterval; // Microseconds + double distanceThreshold, angleThreshold; + PlanningThread() : + goal(INFINITY, INFINITY, INFINITY), operating(true), explorerPaused(false), + updateInterval(500000), + distanceThreshold(0.5), angleThreshold(DEG2RAD(10)) + {} + + void setGoal(const Pose& goal) { + Lock lock(dataLock, true); + pauseExplorer(); + this->goal = goal; + } + + void setMap(const nav_msgs::OccupancyGridConstPtr& map) { + Lock lock(dataLock, true); + latestMap = map; + } + + void pauseExplorer() { + if (!explorerPaused && setMode(0)) + explorerPaused = true; + } + + bool setMode(int mode) { + if (!((void *)setModeSrv)) { + ros::NodeHandle nh; + setModeSrv = nh.serviceClient< crosbot_explore::SetMode >("/explore/set_mode", true); + } + + if ((void *)setModeSrv) { + crosbot_explore::SetMode::Request req; + crosbot_explore::SetMode::Response res; + + req.mode = mode; + return setModeSrv.call(req, res); + } + return false; + } + + void sendPath(const Plan& path, const std::string& frameId) { + if (!((void *)followPathSrv)) { + ros::NodeHandle nh; + followPathSrv = nh.serviceClient< crosbot_explore::FollowPath >("/explore/follow_path", true); + } + + if ((void *)followPathSrv) { + crosbot_explore::FollowPath::Request req; + crosbot_explore::FollowPath::Response res; + req.path.header.frame_id = frameId; + req.path.poses.resize(path.size()); + for (size_t i = 0; i < path.size(); ++i) { + req.path.poses[i].pose = path[i].toROS(); + } + + if (followPathSrv.call(req, res)) { + setMode(1); + } + } + } + +#define SLEEP_INTERVAL 50000 + void run() { + VoronoiGridPtr voronoi; + + ros::Time nextUpdate = ros::Time::now(); + Pose currentPose(INFINITY, INFINITY, INFINITY); + while (operating) { + ros::Time now = ros::Time::now(); + if (now < nextUpdate) { + usleep(SLEEP_INTERVAL); + continue; + } + + Pose goal; + nav_msgs::OccupancyGridConstPtr currentMap; + {{ + Lock lock(dataLock); + goal = this->goal; + if (currentMap != latestMap) { + voronoi = NULL; + currentMap = latestMap; + } + }} + + if (latestMap == NULL || !goal.isFinite()) { + pauseExplorer(); + usleep(SLEEP_INTERVAL); + continue; + } + + tf::StampedTransform transform; + try { + tfListener.lookupTransform(currentMap->header.frame_id, baseFrame, ros::Time(0), transform); + currentPose = transform; + } catch (tf::TransformException& te) { + ROS_WARN("Unable to determine current pose: %s", te.what()); + continue; + } + + ImagePtr debugImage; + if (voronoi == NULL) { + voronoi = new VoronoiGrid(*currentMap, voronoiConstraints, currentPose); + } + + if ((void *)debugPub) { + debugImage = voronoi->getImage(); + } + + if (currentPose.position.distanceTo(goal.position) < distanceThreshold) { + // At the position so just rotate. + ROS_INFO("Arrived at %.3lf, %.3lf, %.3lf.", goal.position.x, goal.position.y, goal.position.z); + + // TODO: Rotate into position + {{ + Lock lock(dataLock, true); + if (this->goal == goal) + this->goal = Pose(INFINITY, INFINITY, INFINITY); + }} + continue; + } + + // plan to goal + Plan plan = planner.getPath(voronoi, currentPose, goal, debugImage); + if (plan.size() > 0) { + // send path to explorer + sendPath(plan, voronoi->frame); + } else { + ROS_WARN("Unable to plan a path to %.3lf, %.3lf, %.3lf.", goal.position.x, goal.position.y, goal.position.z); + } + + if (((void *)debugPub) && debugImage != NULL) { + debugPub.publish(debugImage->toROS()); + } + + nextUpdate += ros::Duration(0, updateInterval*1000); + if (nextUpdate < now) { + nextUpdate = now; + } + } + + pauseExplorer(); + } + + + }; + PlanningThread thread; + +public: + void callbackGoal(const geometry_msgs::PoseStamped& goal) { + // set goal in planning thread + ROS_WARN("New goal received."); + thread.setGoal(goal); + } + + void callbackMap(const nav_msgs::OccupancyGridConstPtr& map) { + thread.setMap(map); + } + + void initalise(ros::NodeHandle& nh) { + // read configuration/parameters + ROS_INFO("Initialising"); + ros::NodeHandle paramNH("~"); // Because ROS's search order is buggered + paramNH.param("base_frame", thread.baseFrame, DEFAULT_BASEFRAME); + ConfigElementPtr cfg = new ROSConfigElement(paramNH); + + // read voronoi constraints + ConfigElementPtr voronoiCfg = cfg->getChild("voronoi"); + VoronoiGrid::Constraints& voronoiConstraints = thread.voronoiConstraints; + if (voronoiCfg != NULL) { + voronoiConstraints.restricted = voronoiCfg->getParamAsDouble("restrict", voronoiConstraints.restricted); + voronoiConstraints.partial = voronoiCfg->getParamAsDouble("partial", voronoiConstraints.partial); + voronoiConstraints.expand = voronoiCfg->getParamAsDouble("expand", voronoiConstraints.expand); + voronoiConstraints.orphanThreshold = voronoiCfg->getParamAsInt("orphan", voronoiConstraints.orphanThreshold); + } + + mapSub = nh.subscribe("map", 1, &MoveNode::callbackMap, this); + goalSub = nh.subscribe("goal", 1, &MoveNode::callbackGoal, this); + thread.debugPub = nh.advertise< sensor_msgs::Image >("debug", 1); + + thread.start(); + } + + void shutdown() { + thread.operating = false; + for (uint32_t i = 0; i < DEFAULT_WAIT_FOR_THREAD_CLOSE && thread.isAlive(); i + 100) { + usleep(100000); + } + if (thread.isAlive()) { + ROS_ERROR("Planning thread not shutting down."); + } + } +}; + +} // namespace crosbot + + +int main(int argc, char** argv) { + ros::init(argc, argv, "move"); + + ros::NodeHandle nh; + + crosbot::MoveNode node; + node.initalise(nh); + + while (ros::ok()) { + ros::spin(); + } + node.shutdown(); + + return 0; +} diff --git a/crosbot_explore/srv/SetMode.srv b/crosbot_explore/srv/SetMode.srv index 3a34117a8c0938db2a354b2af30e71b076244339..a4eceaa5026141855293cc5463a2d0b0ff6e221e 100644 --- a/crosbot_explore/srv/SetMode.srv +++ b/crosbot_explore/srv/SetMode.srv @@ -1,5 +1,6 @@ -std_msgs/Int32 mode # 0 paused - # 1 resume - # 2 left wall - # 3 right wall +int32 mode # 0 paused + # 1 resume + # 2 left wall + # 3 right wall + # 4 waypoint --- \ No newline at end of file diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp index 13500ae2496f557f7ab5cd594e09ffd77473e8b5..675aeda3cd4948ec3357c50c3d03f7fafd341d30 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp @@ -120,7 +120,23 @@ public: } } - // TODO: publish history + // publish history + if (historyPub.getNumSubscribers() > 0) { + nav_msgs::PathPtr rosPath(new nav_msgs::Path()); + rosPath->header.frame_id = mapFrame; + rosPath->header.stamp = newMean->getLatestUpdate()->timestamp.toROS(); + + PathPtr path = newMean->getPath(); + rosPath->poses.resize(path->path.size()); + for (size_t i = 0; i < path->path.size(); ++i) { + geometry_msgs::PoseStamped& pose = rosPath->poses[i]; + pose.pose = path->path[i].toROS(); + pose.header.frame_id = mapFrame; + pose.header.stamp = (i < path->timestamps.size())?(path->timestamps[i].toROS()):ros::Time(0); + } + + historyPub.publish(rosPath); + } } void tagAdded(MapPtr map, TagPtr tag) { @@ -139,8 +155,8 @@ public: scanSub = nh.subscribe("scan", 1, &FastSLAMModule::callbackScan, this); snapSub = nh.subscribe("snap", 1000, &FastSLAMModule::callbackSnap, this); - gridPub = nh.advertise("map", 1); - historyPub = nh.advertise("history", 1); + gridPub = nh.advertise< nav_msgs::OccupancyGrid >("map", 1, true); + historyPub = nh.advertise< nav_msgs::Path >("history", 1); // read configuration/parameters ConfigElementPtr config = new ROSConfigElement(paramNH); diff --git a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp index 05b7274f08d2f90a570421fa228927a3cd559a41..a1fa2d2270c175a4784ac580eba49a11f2c730b3 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp @@ -61,6 +61,10 @@ public: for (size_t i = 0; i < history.size(); i++) { rval->path.push_back(history[i].pose); + if (history[i].cloud != NULL) + rval->timestamps.push_back(history[i].cloud->timestamp); + else + rval->timestamps.push_back(Time(0,0)); } if (rval->path.size() == 0 || rval->path[rval->path.size()-1] != pose) diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp index f4d88746b12c00f4ab6bdf274ca62b549caf8259..47c56dded51cfb528f63b5900441674db16da825 100644 --- a/crosbot_fastslam/src/fastslam.cpp +++ b/crosbot_fastslam/src/fastslam.cpp @@ -468,7 +468,8 @@ void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool cal for (size_t i = 0; i < jobs.size(); i++) { delete jobs[i]; - } jobs.clear(); + } + jobs.clear(); } void FastSLAMMap::updateParticles(MapCloudPtr cloud) { diff --git a/crosbot_graphslam/CMakeLists.txt b/crosbot_graphslam/CMakeLists.txt index 45e0438caf3a1ef515dbe5e97db86fe83d613be3..f667732ec680081e667e3335df8dbff7ea57b201 100644 --- a/crosbot_graphslam/CMakeLists.txt +++ b/crosbot_graphslam/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(catkin REQUIRED COMPONENTS ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) - +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +find_package(SuiteSparse REQUIRED) +#find_package(Newmat REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -91,7 +93,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES graphslam CATKIN_DEPENDS crosbot crosbot_map nav_msgs roscpp sensor_msgs tf -# DEPENDS system_lib + DEPENDS suitesparse-dev ) ########### @@ -103,6 +105,7 @@ catkin_package( # include_directories(include) include_directories(include ${catkin_INCLUDE_DIRS} +# /usr/include/suitesparse ) ## Declare a cpp library @@ -121,8 +124,9 @@ add_dependencies(graphslam ${PROJECT_NAME}_gencpp) ## Specify libraries to link a library or executable target against target_link_libraries(crosbot_graphslam graphslam - ${catkin_LIBRARIES} - newmat + ${catkin_LIBRARIES} ${SuiteSparse_LIBRARIES} +# newmat +# cxsparse ) target_link_libraries(graphslam diff --git a/crosbot_graphslam/cmake/FindSuiteSparse.cmake b/crosbot_graphslam/cmake/FindSuiteSparse.cmake new file mode 100644 index 0000000000000000000000000000000000000000..0a1ebc14f456aeca535411bc67f86a5323d27493 --- /dev/null +++ b/crosbot_graphslam/cmake/FindSuiteSparse.cmake @@ -0,0 +1,45 @@ +# - Try to find SuiteSparse +# Once done, this will define +# +# SuiteSparse_FOUND - system has SuiteSparse +# SuiteSparse_INCLUDE_DIRS - the SuiteSparse include directories +# SuiteSparse_LIBRARIES - link these to use SuiteSparse + +if (SuiteSparse_INCLUDE_DIRS AND SuiteSparse_LIBRARIES) + # in cache already + set(SuiteSparse_FOUND TRUE) +else (SuiteSparse_INCLUDE_DIRS AND SuiteSparse_LIBRARIES) + include(LibFindMacros) + + # Dependencies + # NOTE: SuiteSparse has no explicit dependencies. + # libfind_package(SuiteSparse Dependencies) + + # Use pkg-config to get hints about paths + libfind_pkg_check_modules(SuiteSparse_PKGCONF newmat) + + # Include dir + find_path(SuiteSparse_INCLUDE_DIR + NAMES suitesparse/cs.h + PATHS + ${SuiteSparse_PKGCONF_INCLUDE_DIRS} + /usr/include + /usr/local/include + ) + + # Finally the library itself + find_library(SuiteSparse_LIBRARY + NAMES cxsparse + PATHS ${SuiteSparse_PKGCONF_LIBRARY_DIRS} + ) + + # Set the include dir variables and the libraries and let libfind_process do the rest. + # NOTE: Singular variables for this library, plural for libraries this this lib depends on. + set(SuiteSparse_PROCESS_INCLUDES SuiteSparse_INCLUDE_DIR) + set(SuiteSparse_PROCESS_LIBS SuiteSparse_LIBRARY) + libfind_process(SuiteSparse) + + if (SuiteSparse_INCLUDE_DIRS AND SuiteSparse_LIBRARIES) + set(SuiteSparse_FOUND TRUE) + endif (SuiteSparse_INCLUDE_DIRS AND SuiteSparse_LIBRARIES) +endif (SuiteSparse_INCLUDE_DIRS AND SuiteSparse_LIBRARIES) diff --git a/crosbot_graphslam/cmake/LibFindMacros.cmake b/crosbot_graphslam/cmake/LibFindMacros.cmake new file mode 100644 index 0000000000000000000000000000000000000000..69975c51be3f9c5e0ff372a4424f8de3b580ee88 --- /dev/null +++ b/crosbot_graphslam/cmake/LibFindMacros.cmake @@ -0,0 +1,99 @@ +# Works the same as find_package, but forwards the "REQUIRED" and "QUIET" arguments +# used for the current package. For this to work, the first parameter must be the +# prefix of the current package, then the prefix of the new package etc, which are +# passed to find_package. +macro (libfind_package PREFIX) + set (LIBFIND_PACKAGE_ARGS ${ARGN}) + if (${PREFIX}_FIND_QUIETLY) + set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} QUIET) + endif (${PREFIX}_FIND_QUIETLY) + if (${PREFIX}_FIND_REQUIRED) + set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} REQUIRED) + endif (${PREFIX}_FIND_REQUIRED) + find_package(${LIBFIND_PACKAGE_ARGS}) +endmacro (libfind_package) + +# CMake developers made the UsePkgConfig system deprecated in the same release (2.6) +# where they added pkg_check_modules. Consequently I need to support both in my scripts +# to avoid those deprecated warnings. Here's a helper that does just that. +# Works identically to pkg_check_modules, except that no checks are needed prior to use. +macro (libfind_pkg_check_modules PREFIX PKGNAME) + if (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) + include(UsePkgConfig) + pkgconfig(${PKGNAME} ${PREFIX}_INCLUDE_DIRS ${PREFIX}_LIBRARY_DIRS ${PREFIX}_LDFLAGS ${PREFIX}_CFLAGS) + else (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) + find_package(PkgConfig) + if (PKG_CONFIG_FOUND) + pkg_check_modules(${PREFIX} ${PKGNAME}) + endif (PKG_CONFIG_FOUND) + endif (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) +endmacro (libfind_pkg_check_modules) + +# Do the final processing once the paths have been detected. +# If include dirs are needed, ${PREFIX}_PROCESS_INCLUDES should be set to contain +# all the variables, each of which contain one include directory. +# Ditto for ${PREFIX}_PROCESS_LIBS and library files. +# Will set ${PREFIX}_FOUND, ${PREFIX}_INCLUDE_DIRS and ${PREFIX}_LIBRARIES. +# Also handles errors in case library detection was required, etc. +macro (libfind_process PREFIX) + # Skip processing if already processed during this run + if (NOT ${PREFIX}_FOUND) + # Start with the assumption that the library was found + set (${PREFIX}_FOUND TRUE) + + # Process all includes and set _FOUND to false if any are missing + foreach (i ${${PREFIX}_PROCESS_INCLUDES}) + if (${i}) + set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIRS} ${${i}}) + mark_as_advanced(${i}) + else (${i}) + set (${PREFIX}_FOUND FALSE) + endif (${i}) + endforeach (i) + + # Process all libraries and set _FOUND to false if any are missing + foreach (i ${${PREFIX}_PROCESS_LIBS}) + if (${i}) + set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARIES} ${${i}}) + mark_as_advanced(${i}) + else (${i}) + set (${PREFIX}_FOUND FALSE) + endif (${i}) + endforeach (i) + + # Print message and/or exit on fatal error + if (${PREFIX}_FOUND) + if (NOT ${PREFIX}_FIND_QUIETLY) + message (STATUS "Found ${PREFIX} ${${PREFIX}_VERSION}") + endif (NOT ${PREFIX}_FIND_QUIETLY) + else (${PREFIX}_FOUND) + if (${PREFIX}_FIND_REQUIRED) + foreach (i ${${PREFIX}_PROCESS_INCLUDES} ${${PREFIX}_PROCESS_LIBS}) + message("${i}=${${i}}") + endforeach (i) + message (FATAL_ERROR "Required library ${PREFIX} NOT FOUND.\nInstall the library (dev version) and try again. If the library is already installed, use ccmake to set the missing variables manually.") + endif (${PREFIX}_FIND_REQUIRED) + endif (${PREFIX}_FOUND) + endif (NOT ${PREFIX}_FOUND) +endmacro (libfind_process) + +macro(libfind_library PREFIX basename) + set(TMP "") + if(MSVC80) + set(TMP -vc80) + endif(MSVC80) + if(MSVC90) + set(TMP -vc90) + endif(MSVC90) + set(${PREFIX}_LIBNAMES ${basename}${TMP}) + if(${ARGC} GREATER 2) + set(${PREFIX}_LIBNAMES ${basename}${TMP}-${ARGV2}) + string(REGEX REPLACE "\\." "_" TMP ${${PREFIX}_LIBNAMES}) + set(${PREFIX}_LIBNAMES ${${PREFIX}_LIBNAMES} ${TMP}) + endif(${ARGC} GREATER 2) + find_library(${PREFIX}_LIBRARY + NAMES ${${PREFIX}_LIBNAMES} + PATHS ${${PREFIX}_PKGCONF_LIBRARY_DIRS} + ) +endmacro(libfind_library) + diff --git a/crosbot_graphslam/include/crosbot_graphslam/graphSlam.hpp b/crosbot_graphslam/include/crosbot_graphslam/graphSlam.hpp index 2db333adec4874844b661d480ef3f90cdbce5972..7560a0bb251d563cd08d8b01686e01d9c2e23796 100644 --- a/crosbot_graphslam/include/crosbot_graphslam/graphSlam.hpp +++ b/crosbot_graphslam/include/crosbot_graphslam/graphSlam.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -131,7 +132,7 @@ protected: //Total covariance at which a new local map will be created double LocalMapCovarianceThreshold; //Number of iterations of the graph optimiser - int NumOfOptimisationIts; + int MaxNumOfOptimisationIts; //Amount a local maps moves before it is evaluated for temp loop closures double LargeMovementThreshold; //amount two local maps have to overlap to be considered potentially matching @@ -145,6 +146,13 @@ protected: //Amount the last scan of a local map needs to be corrected before the local map is warped double LocalMapWarpThreshXY; double LocalMapWarpThreshTh; + //Maximum movement of a map for the graph optimsation to be considered + //to converge + double MaxOptMoveXY; + double MaxOptMoveTh; + //If should use a least squares optimiser. If false, will use gradient descent + bool UseLeastSquaresOptimisation; + //Kinect Params diff --git a/crosbot_graphslam/include/crosbot_graphslam/graphSlamCPU.hpp b/crosbot_graphslam/include/crosbot_graphslam/graphSlamCPU.hpp index a9b40549cf161132cb261809a40341aa68c1e89d..cb57894f05808e2a296646e98fd411dc79638361 100644 --- a/crosbot_graphslam/include/crosbot_graphslam/graphSlamCPU.hpp +++ b/crosbot_graphslam/include/crosbot_graphslam/graphSlamCPU.hpp @@ -10,6 +10,9 @@ #include +#include +//#include + #define NUM_ORIENTATION_BINS 64 #define NUM_PROJECTION_BINS 100 #define MAX_LOCAL_POINTS 2000 @@ -99,6 +102,7 @@ private: double warpPointsY[MAX_LOCAL_POINTS]; double warpPointsZ[MAX_LOCAL_POINTS]; int lastObserved[MAX_LOCAL_POINTS]; + int minOptNode; int numWarpPoints; bool isFeatureless; @@ -180,6 +184,7 @@ private: double previousScore; bool didOptimise; //bool tempO; + bool alreadyOutput; //debugging for timings ros::WallDuration totalTime; @@ -208,6 +213,8 @@ private: double getMetricValue(double pointX, double pointY, double ogPointX, double ogPointY); //Multiply two 3x3 matrices void mult3x3Matrix(double a[3][3], double b[3][3], double res[3][3]); + //Transpose a matrix + void transpose3x3Matrix(double a[3][3], double res[3][3]); // Nasty hack function to make covariance matrices acceptable in the occassional // case where not enough matching points were found when creating the // information matrix @@ -241,14 +248,19 @@ private: void calculateICPMatrix(int matchIndex, bool fullLoop, int currentMap); //Finalises the information matrix of a loop constraint void finaliseInformationMatrix(); + //Calculates the global hessian matrix for the map optimisation void getGlobalHessianMatrix(); - //Performs the optimisation of the graph + //Performs the optimisation of the graph using gradient descent //type of 0 is normal, 1 is just full loop closures, -1 is only since last //full loop closure void calculateOptimisationChange(int numIterations, int type); //Updates the global positions of all local maps void updateGlobalPositions(); + + //Optimises the graph using least squares + void optimiseGraph(int type); + //Updates the global covariances of each local map and updates //the global positions of all laser points void updateGlobalMap(); diff --git a/crosbot_graphslam/launch/crosbot_graphslam.launch b/crosbot_graphslam/launch/crosbot_graphslam.launch index 122af8c7aa3a1931dc8e87e3d72a7deb016f12d4..65029c8222fc2203ca822ef8cb053508cd743053 100644 --- a/crosbot_graphslam/launch/crosbot_graphslam.launch +++ b/crosbot_graphslam/launch/crosbot_graphslam.launch @@ -41,6 +41,9 @@ + + +