diff --git a/CMakeLists.txt b/CMakeLists.txt index 45374d65..243ba2ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,9 +159,6 @@ target_link_libraries(map_assembler rtabmap_ros ${Libraries}) add_executable(grid_map_assembler src/GridMapAssemblerNode.cpp) target_link_libraries(grid_map_assembler rtabmap_ros ${Libraries}) -add_executable(planner src/PlannerNode.cpp) -target_link_libraries(planner rtabmap_ros ${Libraries}) - add_executable(camera src/CameraNode.cpp) add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(camera ${Libraries}) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index a5ddb07e..4c6384fe 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -129,6 +131,18 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : mapData_ = nh.advertise("mapData", 1); mapGraph_ = nh.advertise("graph", 1); + // planning topics + goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this); + nextMetricGoalPub_ = nh.advertise("goal_pose", 1); + nextMetricGoalIdPub_ = nh.advertise("goal_pose_id", 1); + goalReachedPub_ = nh.advertise("goal_reached", 1); + pathPub_ = nh.advertise("path", 1); + pathIdsPub_ = nh.advertise("path_ids", 1); + + ros::Publisher nextMetricGoal_; + ros::Publisher goalReached_; + ros::Publisher path_; + configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir()); databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir()); @@ -916,6 +930,8 @@ void CoreWrapper::process( const Statistics & stats = rtabmap_.getStatistics(); this->publishStats(stats); + + this->updateGoal(); } } else if(!rtabmap_.isIDsGenerated()) @@ -933,6 +949,118 @@ void CoreWrapper::process( rtabmap_.getWMSize()+rtabmap_.getSTMSize()); } +void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg) +{ + currentMetricGoal_.setNull(); + int id = msg->data; + ROS_INFO("Planning: set goal %d", id); + + std::list > poses = rtabmap_.computePath(id); + if(poses.size()) + { + currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathGoalId()); + if(currentMetricGoal_.isNull()) + { + ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId()); + rtabmap_.clearPath(); + } + else + { + ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size()); + ros::Time now = ros::Time::now(); + if(pathPub_.getNumSubscribers()) + { + nav_msgs::Path path; + path.header.frame_id = mapFrameId_; + path.header.stamp = now; + path.poses.resize(poses.size()); + int oi = 0; + for(std::list >::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + path.poses[oi].header = path.header; + rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose); + ++oi; + } + pathPub_.publish(path); + } + if(pathIdsPub_.getNumSubscribers()) + { + std_msgs::Int32MultiArray array; + array.data.resize(poses.size()); + int oi = 0; + for(std::list >::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + array.data[oi++] = iter->first; + } + pathIdsPub_.publish(array); + } + + publishGoal(); + } + } + else + { + ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!"); + goalReachedPub_.publish(std_msgs::Empty()); + } +} + +void CoreWrapper::updateGoal() +{ + if(!currentMetricGoal_.isNull()) + { + if(rtabmap_.getPath().size() == 0) + { + // Goal reached + ROS_INFO("Planning: Publishing goal reached!"); + if(goalReachedPub_.getNumSubscribers()) + { + goalReachedPub_.publish(std_msgs::Empty()); + } + currentMetricGoal_.setNull(); + } + else + { + Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId()); + if(!updatedGoalPose.isNull()) + { + // detect if the goal has changed or local map + // has changed so much that current goal drifted + if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f) + { + currentMetricGoal_ = updatedGoalPose; + + publishGoal(); + } + } + else + { + ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId()); + } + } + } +} + +void CoreWrapper::publishGoal() +{ + ROS_INFO("Planning: Publishing next goal: Location %d pose=%s", + rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str()); + if(nextMetricGoalPub_.getNumSubscribers()) + { + geometry_msgs::PoseStamped goalMsg; + goalMsg.header.frame_id = mapFrameId_; + goalMsg.header.stamp = ros::Time::now(); + rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose); + nextMetricGoalPub_.publish(goalMsg); + } + if(nextMetricGoalIdPub_.getNumSubscribers()) + { + std_msgs::Int32 msg; + msg.data = rtabmap_.getPathGoalId(); + nextMetricGoalIdPub_.publish(msg); + } +} + bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); @@ -980,6 +1108,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt rtabmap_.resetMemory(); _variance = 0; lastPose_.setIdentity(); + currentMetricGoal_.setNull(); return true; } diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index b2e3c6d1..06d62cb7 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -92,6 +93,9 @@ private: const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg); + void goalNodeCallback(const std_msgs::Int32ConstPtr & msg); + void updateGoal(); + void publishGoal(); void process( int id, @@ -129,6 +133,7 @@ private: bool paused_; rtabmap::Transform lastPose_; float _variance; + rtabmap::Transform currentMetricGoal_; std::string frameId_; std::string mapFrameId_; @@ -144,6 +149,14 @@ private: ros::Publisher mapData_; ros::Publisher mapGraph_; + //Planning stuff + ros::Subscriber goalNodeSub_; + ros::Publisher nextMetricGoalPub_; + ros::Publisher nextMetricGoalIdPub_; + ros::Publisher goalReachedPub_; + ros::Publisher pathPub_; + ros::Publisher pathIdsPub_; + // for loop closure detection only image_transport::Subscriber defaultSub_; diff --git a/src/PlannerNode.cpp b/src/PlannerNode.cpp deleted file mode 100644 index 7ca6ca77..00000000 --- a/src/PlannerNode.cpp +++ /dev/null @@ -1,318 +0,0 @@ -/* -Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Universite de Sherbrooke nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY -DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include - -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include - -#include - -#include - -class Planner -{ - -public: - Planner() : - frameId_("base_link"), - mapFrameId_("map"), - plan2d_(true), - goalError_(1.0), - graphChangeError_(0.2), - currentGoalIndex_(0) - { - ros::NodeHandle pnh("~"); - pnh.param("frame_id", frameId_, frameId_); - pnh.param("map_frame_id", mapFrameId_, mapFrameId_); - pnh.param("plan_2d", plan2d_, plan2d_); - pnh.param("goal_error", goalError_, goalError_); - pnh.param("graph_change_error", graphChangeError_, graphChangeError_); - - - ros::NodeHandle nh; - goalTopic_ = nh.subscribe("goal", 1, &Planner::goalCallback, this); - mapDataTopic_ = nh.subscribe("mapData", 1, &Planner::mapDataCallback, this); - - pubNextGoal_ = nh.advertise("next_goal", 1); - pubGoalReached_ = nh.advertise("goal_reached", 1); - pubPath_ = nh.advertise("path", 1); - } - - ~Planner() - { - } - - void goalCallback(const std_msgs::Int32ConstPtr & msg) - { - int id = msg->data; - ROS_INFO("planner: set goal %d", id); - rtabmap_ros::GetMap getMapSrv; - getMapSrv.request.global = true; - getMapSrv.request.optimized = true; - getMapSrv.request.graphOnly = true; - if(!ros::service::call("get_map", getMapSrv)) - { - ROS_WARN("Can't call \"get_map\" service"); - return; - } - - std::map poses; - std::map mapIds; - std::multimap constraints; - rtabmap::Transform mapToOdom; - rtabmap_ros::mapGraphFromROS(getMapSrv.response.data.graph, poses, mapIds, constraints, mapToOdom); - std::multimap links; - for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) - { - links.insert(std::make_pair(iter->first, iter->second.to())); - links.insert(std::make_pair(iter->second.to(), iter->first)); // <-> - } - - if(!uContains(poses, id)) - { - ROS_WARN("Goal %d not found in the global graph!", id); - return; - } - - // find the nearest node of the current location - rtabmap::Transform currentPose; - try - { - tf::StampedTransform tmp; - tfListener_.lookupTransform(mapFrameId_, frameId_, ros::Time(0), tmp); - currentPose = rtabmap_ros::transformFromTF(tmp); - if(plan2d_) - { - currentPose.z() = 0; - } - } - catch(tf::TransformException & ex) - { - ROS_WARN("%s",ex.what()); - return; - } - - int nearestPose = -1; - float nearestDistance = -1.0f; - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - if(plan2d_) - { - iter->second.z() = 0; - } - float d = currentPose.getDistance(iter->second); - if(nearestPose == -1 || d < nearestDistance) - { - nearestPose = iter->first; - nearestDistance = d; - } - } - - if(nearestPose <= 0) - { - ROS_WARN("Nearest pose not found!?!?"); - return; - } - - ROS_INFO("Computing path from location %d to %d", nearestPose, id); - path_ = rtabmap::computePath(poses, links, nearestPose, id); - currentGoalIndex_ = 0; - currentGoal_.setNull(); - if(path_.size()<2) - { - path_.clear(); - ROS_WARN("Cannot compute a path (or goal is already reached)!"); - } - else - { - ROS_INFO("Path generated! Size=%d", (int)path_.size()); - if(pubPath_.getNumSubscribers()) - { - nav_msgs::Path path; - path.header.frame_id = mapFrameId_; - path.header.stamp = ros::Time::now(); - path.poses.resize(path_.size()); - for(unsigned int i=0; i poses; - std::map mapIds; - std::multimap constraints; - rtabmap::Transform mapToOdom; - rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, constraints, mapToOdom); - - // Post a new goal if: - // - no current goal is set, - // - the current goal is reached, - // - the current goal has moved (graph was optimized with new constraints) - if(!currentGoal_.isNull()) - { - std::map::iterator iter=poses.find(path_[currentGoalIndex_]); - if(iter != poses.end()) - { - rtabmap::Transform currentGoal = iter->second; - if(plan2d_) - { - currentGoal.z() = 0; - } - float d = currentGoal.getDistance(currentGoal_); - if(d > graphChangeError_) - { - // reset the goal - ROS_INFO("Goal reset because the local graph has changed (%f m) newCurent=%s oldCurrent=%s...", - d, currentGoal.prettyPrint().c_str(), currentGoal_.prettyPrint().c_str()); - currentGoal_.setNull(); - } - - if(!currentGoal_.isNull()) - { - float d = currentPose.getDistance(currentGoal_); - if(d < goalError_) - { - //Current goal reached! Reset it. - currentGoal_.setNull(); - if(currentGoalIndex_ == path_.size()-1) - { - // last goal reached! - ROS_INFO("Goal %d reached!", path_[currentGoalIndex_]); - pubGoalReached_.publish(std_msgs::Empty()); - path_.clear(); - return; // EXIT - } - - } - } - } - - } - if(currentGoal_.isNull()) - { - // find the farthest node that can be reached in the local map - for(unsigned int i=path_.size()-1; i>=currentGoalIndex_; --i) - { - std::map::iterator iter=poses.find(path_[i]); - if(iter != poses.end()) - { - currentGoalIndex_ = i; - currentGoal_ = iter->second; - if(plan2d_) - { - currentGoal_.z() = 0; - } - break; - } - } - if(currentGoal_.isNull()) - { - ROS_ERROR("Cannot update current goal!"); - } - else - { - ROS_INFO("Publishing next goal: Location %d (%d/%d)", - path_[currentGoalIndex_], currentGoalIndex_+1, (int)path_.size()); - geometry_msgs::PoseStamped goalMsg; - goalMsg.header = msg->header; - rtabmap_ros::transformToPoseMsg(currentGoal_, goalMsg.pose); - pubNextGoal_.publish(goalMsg); - } - } - } - } - -private: - std::string frameId_; - std::string mapFrameId_; - bool plan2d_; - double goalError_; - double graphChangeError_; - - tf::TransformListener tfListener_; - - ros::Subscriber goalTopic_; - ros::Subscriber mapDataTopic_; - - ros::Publisher pubNextGoal_; - ros::Publisher pubGoalReached_; - ros::Publisher pubPath_; - - std::vector path_; - int currentGoalIndex_; - rtabmap::Transform currentGoal_; -}; - - -int main(int argc, char** argv) -{ - ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kWarning); - ros::init(argc, argv, "planner"); - Planner planner; - ros::spin(); - return 0; -}