diff --git a/CMakeLists.txt b/CMakeLists.txt index fdf83deb..551c27c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,7 @@ add_message_files( PublishMap.srv ResetPose.srv SetGoal.srv + SetLabel.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 3646e3c4..828f3ada 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -96,6 +96,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); +inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;} + } #endif /* MSGCONVERSION_H_ */ diff --git a/msg/NodeData.msg b/msg/NodeData.msg index 150ce9df..7b8eb4d5 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -1,6 +1,9 @@ int32 id int32 mapId +int32 weight +float64 stamp +string label # Pose from odometry not corrected geometry_msgs/Pose pose diff --git a/package.xml b/package.xml index 906aca44..96d9819c 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.8.4 + 0.8.5 RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 98ce16a7..a7936ce1 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -353,6 +353,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this); + setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this); octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this); octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this); @@ -717,6 +718,7 @@ void CoreWrapper::commonDepthCallback( float cy = model.cy(); process(ptrImage->header.seq, + ptrImage->header.stamp, ptrImage->image, lastPose_, odomFrameId, @@ -798,6 +800,7 @@ void CoreWrapper::commonStereoCallback( float baseline = model.baseline(); process(leftImageMsg->header.seq, + leftImageMsg->header.stamp, ptrLeftImage->image, lastPose_, odomFrameId, @@ -926,6 +929,7 @@ void CoreWrapper::stereoScanTFCallback( void CoreWrapper::process( int id, + const ros::Time & stamp, const cv::Mat & image, const Transform & odom, const std::string & odomFrameId, @@ -990,7 +994,8 @@ void CoreWrapper::process( odom, odomRotationalVariance, odomTransitionalVariance, - id); + id, + rtabmap_ros::timestampFromROS(stamp)); if(rtabmap_.process(data)) { @@ -1001,15 +1006,14 @@ void CoreWrapper::process( mapToOdomMutex_.unlock(); // Publish local graph, info - ros::Time timeNow = ros::Time::now(); - this->publishStats(timeNow); + this->publishStats(stamp); std::map filteredPoses; filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), cloudMapPub_.getNumSubscribers() != 0, projMapPub_.getNumSubscribers() != 0, gridMapPub_.getNumSubscribers() != 0); - this->publishMaps(filteredPoses, timeNow); + this->publishMaps(filteredPoses, stamp); // clear memory if no one subscribed if(mapCacheCleanup_) @@ -1062,11 +1066,11 @@ void CoreWrapper::process( { currentMetricGoal_ = updatedGoalPose; - publishCurrentGoal(timeNow); + publishCurrentGoal(stamp); } // publish local path - publishLocalPath(timeNow); + publishLocalPath(stamp); } else { @@ -1509,7 +1513,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab if(poses.size() && poses.size() != mapIds.size()) { ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.size()); - return false; } ros::Time now = ros::Time::now(); @@ -1595,7 +1598,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab else { UWARN("No subscribers, don't need to publish!"); - return false; } return true; @@ -1603,13 +1605,60 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res) { - int id = req.target_node_id; - ROS_INFO("Planning: set goal %d", id); - UTimer timer; - rtabmap_.computePath(id, req.in_global_graph); - ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); - goalCommonCallback(rtabmap_.getPath()); - return !currentMetricGoal_.isNull(); + int id = req.node_id; + if(id == 0 && !req.node_label.empty() && rtabmap_.getMemory()) + { + id = rtabmap_.getMemory()->getSignatureIdByLabel(req.node_label); + } + + if(id > 0) + { + ROS_INFO("Planning: set goal %d", id); + UTimer timer; + rtabmap_.computePath(id, true); + ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); + goalCommonCallback(rtabmap_.getPath()); + if(currentMetricGoal_.isNull()) + { + ROS_ERROR("Planning: Node id %d not found or goal already reached!", id); + } + } + else if(!req.node_label.empty()) + { + ROS_ERROR("Planning: Node with label \"%s\" not found!", req.node_label.c_str()); + } + else + { + ROS_ERROR("Planning: Node id should be > 0 !"); + } + return true; +} + +bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res) +{ + if(rtabmap_.labelLocation(req.node_id, req.node_label)) + { + if(req.node_id > 0) + { + ROS_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); + } + else + { + ROS_INFO("Set label \"%s\" to last node", req.node_label.c_str()); + } + } + else + { + if(req.node_id > 0) + { + ROS_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id); + } + else + { + ROS_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str()); + } + } + return true; } void CoreWrapper::publishStats(const ros::Time & stamp) diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index a222f692..36cf536c 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/GetMap.h" #include "rtabmap_ros/PublishMap.h" #include "rtabmap_ros/SetGoal.h" +#include "rtabmap_ros/SetLabel.h" #include #include @@ -163,6 +164,7 @@ private: void process( int id, + const ros::Time & stamp, const cv::Mat & image, const rtabmap::Transform & odom = rtabmap::Transform(), const std::string & odomFrameId = "", @@ -188,6 +190,7 @@ private: bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res); + bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); @@ -372,6 +375,7 @@ private: ros::ServiceServer getGridMapSrv_; ros::ServiceServer publishMapDataSrv_; ros::ServiceServer setGoalSrv_; + ros::ServiceServer setLabelSrv_; ros::ServiceServer octomapBinarySrv_; ros::ServiceServer octomapFullSrv_; diff --git a/src/DataRecorderNode.cpp b/src/DataRecorderNode.cpp index 6032d9df..501bb5c2 100644 --- a/src/DataRecorderNode.cpp +++ b/src/DataRecorderNode.cpp @@ -290,7 +290,8 @@ private: Transform(), 1.0f, 1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(imageMsg->header.stamp)); recorder_.addData(data); } @@ -373,7 +374,8 @@ private: rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance>0?rotVariance:1.0f, transVariance>0?transVariance:1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(imageMsg->header.stamp)); recorder_.addData(data); } @@ -472,7 +474,8 @@ private: rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance>0?rotVariance:1.0f, transVariance>0?transVariance:1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(imageMsg->header.stamp)); recorder_.addData(data); } @@ -529,7 +532,8 @@ private: rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance>0?rotVariance:1.0f, transVariance>0?transVariance:1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp)); recorder_.addData(data); } @@ -583,7 +587,8 @@ private: Transform(), 1.0f, 1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp)); recorder_.addData(data); } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 4bed22e0..8433f6b4 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -429,7 +429,8 @@ void GuiWrapper::depthCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); this->post(new OdometryEvent(image)); } @@ -487,7 +488,8 @@ void GuiWrapper::depthOdomInfoCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg); this->post(new OdometryEvent(image, info)); } @@ -556,7 +558,8 @@ void GuiWrapper::depthScanCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); this->post(new OdometryEvent(image)); } @@ -648,7 +651,8 @@ void GuiWrapper::stereoScanCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); this->post(new OdometryEvent(image)); } @@ -730,7 +734,8 @@ void GuiWrapper::stereoOdomInfoCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg); this->post(new OdometryEvent(image, info)); } @@ -812,7 +817,8 @@ void GuiWrapper::stereoCallback( rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), rotVariance, transVariance, - odomMsg->header.seq); + odomMsg->header.seq, + rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); this->post(new OdometryEvent(image)); } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 591c5bb9..dd4bf7c0 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -337,8 +337,12 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size()); } - return rtabmap::Signature(msg.id, + return rtabmap::Signature( + msg.id, msg.mapId, + msg.weight, + msg.stamp, + msg.label, words, words3D, transformFromPoseMsg(msg.pose), @@ -356,6 +360,9 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & // add data msg.id = signature.id(); msg.mapId = signature.mapId(); + msg.weight = signature.getWeight(); + msg.stamp = signature.getStamp(); + msg.label = signature.getLabel(); transformToPoseMsg(signature.getPose(), msg.pose); compressedMatToBytes(signature.getImageCompressed(), msg.image); compressedMatToBytes(signature.getDepthCompressed(), msg.depth); diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index f7959248..95cf234c 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -149,7 +149,8 @@ public: rtabmap::Transform(), 1.0f, 1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(image->header.stamp)); this->processData(data, image->header); } diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index 4210b038..87cbd6b2 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -173,7 +173,8 @@ public: rtabmap::Transform(), 1.0f, 1.0f, - 0); + 0, + rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp)); this->processData(data, imageRectLeft->header); } diff --git a/srv/SetGoal.srv b/srv/SetGoal.srv index 7289d640..db883b56 100644 --- a/srv/SetGoal.srv +++ b/srv/SetGoal.srv @@ -1,5 +1,6 @@ #request -int32 target_node_id -bool in_global_graph +# Set either node_id or node_label +int32 node_id +string node_label --- #response \ No newline at end of file diff --git a/srv/SetLabel.srv b/srv/SetLabel.srv new file mode 100644 index 00000000..e5e740a2 --- /dev/null +++ b/srv/SetLabel.srv @@ -0,0 +1,6 @@ +#request +# Set node_id = 0 to set label to last node +int32 node_id +string node_label +--- +#response \ No newline at end of file