From 222937b65a57ccc6cda4c426e9171e5a055e7a13 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 21 Oct 2015 10:37:38 -0400 Subject: [PATCH 001/119] Odom: fixed Stereo parameters filter --- src/OdometryROS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 87167291..0b135526 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -276,7 +276,7 @@ rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) { std::string group = uSplit(iter->first, '/').front(); if(uStrContains(group, "Odom") || - group.compare("Stereo") || + (stereo && group.compare("Stereo") == 0) || group.compare("SURF") == 0 || group.compare("SIFT") == 0 || group.compare("ORB") == 0 || From 1cff33e3fa8d817c2b9b62db6bee7d1d0f538e53 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 24 Oct 2015 16:52:24 -0400 Subject: [PATCH 002/119] Updated for rtabmap 0.10.11 --- CMakeLists.txt | 2 +- package.xml | 2 +- src/rviz/MapGraphDisplay.cpp | 8 +++++++- src/rviz/MapGraphDisplay.h | 1 + 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 90cec1c9..60c75d8c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(octomap_ros) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.10.10 REQUIRED) +find_package(RTABMap 0.10.11 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/package.xml b/package.xml index e972177b..a784ad11 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.10.10 + 0.10.11 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/rviz/MapGraphDisplay.cpp b/src/rviz/MapGraphDisplay.cpp index 4b472e3d..84aa2d5e 100644 --- a/src/rviz/MapGraphDisplay.cpp +++ b/src/rviz/MapGraphDisplay.cpp @@ -52,7 +52,9 @@ namespace rtabmap_ros MapGraphDisplay::MapGraphDisplay() { color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue, - "Color to draw neighbor links.", this ); + "Color to draw neighbor links.", this ); + color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0), + "Color to draw merged neighbor links.", this ); color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red, "Color to draw global loop closure links.", this ); color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow, @@ -139,6 +141,10 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg { color = color_neighbor_property_->getOgreColor(); } + else if(iter->second.type() == rtabmap::Link::kNeighborMerged) + { + color = color_neighbor_merged_property_->getOgreColor(); + } else if(iter->second.type() == rtabmap::Link::kVirtualClosure) { color = color_virtual_property_->getOgreColor(); diff --git a/src/rviz/MapGraphDisplay.h b/src/rviz/MapGraphDisplay.h index da9a3e29..d499cc76 100644 --- a/src/rviz/MapGraphDisplay.h +++ b/src/rviz/MapGraphDisplay.h @@ -76,6 +76,7 @@ private: std::vector manual_objects_; ColorProperty* color_neighbor_property_; + ColorProperty* color_neighbor_merged_property_; ColorProperty* color_global_property_; ColorProperty* color_local_property_; ColorProperty* color_user_property_; From 94c7928321630fcf4e8639c6ab28f1e665a3e5dd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 27 Oct 2015 09:31:05 -0400 Subject: [PATCH 003/119] Fixed issue #40: RtabmapGlobalPathEvent build error --- src/CoreWrapper.cpp | 16 ++++++++++++++-- src/CoreWrapper.h | 2 +- src/GuiWrapper.cpp | 4 ++-- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index dd5e9a6a..37f29ce2 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1344,7 +1344,8 @@ void CoreWrapper::goalCommonCallback( int id, const std::string & label, const Transform & pose, - const ros::Time & stamp) + const ros::Time & stamp, + double * planningTime) { UTimer timer; @@ -1362,10 +1363,19 @@ void CoreWrapper::goalCommonCallback( ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str()); } + if(planningTime) + { + *planningTime = 0.0; + } + bool success = false; if((id > 0 && rtabmap_.computePath(id, true)) || (!pose.isNull() && rtabmap_.computePath(pose))) { + if(planningTime) + { + *planningTime = timer.elapsed(); + } ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); const std::vector > & poses = rtabmap_.getPath(); @@ -1905,10 +1915,12 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res) { - goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now()); + double planningTime = 0.0; + goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime); const std::vector > & path = rtabmap_.getPath(); res.path_ids.resize(path.size()); res.path_poses.resize(path.size()); + res.planning_time = planningTime; for(unsigned int i=0; iposes[i].pose); } - this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses)); + this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0)); } void GuiWrapper::goalReachedCallback( @@ -441,7 +441,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent) poses[i].first = setGoalSrv.response.path_ids[i]; poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]); } - this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses)); + this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time)); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal) From 470958c28e39a3bfe9284132e7ddf25906a69ee1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 27 Oct 2015 22:15:04 -0400 Subject: [PATCH 004/119] Added missing changes for issue #40 --- srv/SetGoal.srv | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/srv/SetGoal.srv b/srv/SetGoal.srv index 9de0acb4..8127f359 100644 --- a/srv/SetGoal.srv +++ b/srv/SetGoal.srv @@ -5,4 +5,5 @@ string node_label --- #response int32[] path_ids -geometry_msgs/Pose[] path_poses \ No newline at end of file +geometry_msgs/Pose[] path_poses +float32 planning_time \ No newline at end of file From fc9c3aa1623f77f5cafb95c216295ba9e81819a8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 15:56:30 -0400 Subject: [PATCH 005/119] data_player: fixed published TF stamp synchronization --- src/DbPlayerNode.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index 6135b2cc..818b2647 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -257,8 +257,6 @@ int main(int argc, char** argv) // publish transforms first if(publishTf) { - ros::Time tfExpiration = time + ros::Duration(rate>0?1.0/rate:acquisitionTime); - rtabmap::Transform localTransform; if(odom.data().cameraModels().size() == 1) { @@ -273,7 +271,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped baseToCamera; baseToCamera.child_frame_id = cameraFrameId; baseToCamera.header.frame_id = frameId; - baseToCamera.header.stamp = tfExpiration; + baseToCamera.header.stamp = time; rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); tfBroadcaster.sendTransform(baseToCamera); } @@ -283,7 +281,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped odomToBase; odomToBase.child_frame_id = frameId; odomToBase.header.frame_id = odomFrameId; - odomToBase.header.stamp = tfExpiration; + odomToBase.header.stamp = time; rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); tfBroadcaster.sendTransform(odomToBase); } @@ -293,7 +291,7 @@ int main(int argc, char** argv) geometry_msgs::TransformStamped baseToLaserScan; baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.header.frame_id = frameId; - baseToLaserScan.header.stamp = tfExpiration; + baseToLaserScan.header.stamp = time; rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform); tfBroadcaster.sendTransform(baseToLaserScan); } From d502991d804627f26f88e1a6bee404e76ea47a06 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 18:49:28 -0400 Subject: [PATCH 006/119] data_reocrder.launch: added stereo_approx_sync argument and desactivated LocalLoopClosureSpace --- launch/data_recorder.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index c95294fe..c52f0a90 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -5,6 +5,7 @@ + @@ -37,6 +38,7 @@ + @@ -47,6 +49,7 @@ + From 144a7f1e94100130d28fe28f6bda5624561fb993 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 20:05:04 -0400 Subject: [PATCH 007/119] Added rtabmap.launch --- launch/rtabmap.launch | 166 ++++++++++++++++++++++++++++++++++++++++++ src/CoreWrapper.cpp | 2 +- src/GuiWrapper.cpp | 4 +- 3 files changed, 170 insertions(+), 2 deletions(-) create mode 100644 launch/rtabmap.launch diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch new file mode 100644 index 00000000..3b224d57 --- /dev/null +++ b/launch/rtabmap.launch @@ -0,0 +1,166 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 37f29ce2..9d764a3e 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -124,7 +124,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeDepth && subscribeStereo) { - UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); + ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); subscribeDepth = false; } if(subscribeLaserScan) diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 216b7c7d..040e9f03 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -1212,7 +1212,9 @@ void GuiWrapper::setupCallbacks( if(subscribeDepth && subscribeStereo) { - ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\"."); + ROS_WARN("rtabmapviz: Parameters subscribe_depth and subscribe_stereo cannot be true at the " + "same time. Parameter subscribe_depth is set to false."); + subscribeDepth = false; } if(!subscribeDepth && !subscribeStereo && subscribeLaserScan) { From 6409033465da41ede49ae2d0b3ecdc405d3ccfcc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 20:07:18 -0400 Subject: [PATCH 008/119] Added rtabmap.launch to installation --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 60c75d8c..b2367744 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -278,6 +278,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES + launch/rtabmap.launch launch/rgbd_mapping.launch launch/stereo_mapping.launch launch/data_recorder.launch From 2b5317325b5c3ccda35e8c3340d712a0cd404e20 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 20:20:22 -0400 Subject: [PATCH 009/119] rtabmap.launch: fixed rtabmapviz config path --- launch/rtabmap.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 3b224d57..7337c65d 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -23,7 +23,7 @@ - + @@ -122,7 +122,7 @@ - + From df5d693a15a430534d5080fd1d6141734355b793 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 28 Oct 2015 21:11:36 -0400 Subject: [PATCH 010/119] rtabmap.launch set approx_sync argument only for stereo topics --- launch/rtabmap.launch | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 7337c65d..a494b72a 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -70,7 +70,6 @@ - @@ -160,7 +159,7 @@ - + From cb0ebecd5f5fff05496e4daed7829360918c9b66 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 31 Oct 2015 13:20:42 -0400 Subject: [PATCH 011/119] Fixed map disappearing with map_optimizer (see http://official-rtab-map-forum.67519.x6.nabble.com/Close-Loop-cause-Map-to-Disappear-td810.html) --- include/rtabmap_ros/MsgConversion.h | 3 ++ src/MapOptimizerNode.cpp | 63 +++++++++++++++++++++++------ src/MsgConversion.cpp | 22 ++++++++++ 3 files changed, 75 insertions(+), 13 deletions(-) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 4edfaacb..2ab3a009 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -110,6 +110,9 @@ void mapGraphToROS( rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg); void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); +rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg); +void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); + rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index 96a809a5..c32c09c3 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -137,8 +137,10 @@ public: if(iter->second.to() == link.to()) { edgeAlreadyAdded = true; - if(iter->second.transform() != link.transform()) + if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001) { + ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), + link.transform().prettyPrint().c_str()); dataChanged = true; } } @@ -149,16 +151,17 @@ public: } } - std::map newPoses; + std::map newNodeInfos; // add new odometry poses for(unsigned int i=0; inodes.size(); ++i) { int id = msg->nodes[i].id; Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose); - newPoses.insert(std::make_pair(id, pose)); + Signature s = rtabmap_ros::nodeInfoFromROS(msg->nodes[i]); + newNodeInfos.insert(std::make_pair(id, s)); - std::pair::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose)); - if(!p.second && pose != cachedPoses_.at(id)) + std::pair::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s)); + if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001) { dataChanged = true; } @@ -167,27 +170,27 @@ public: if(dataChanged) { ROS_WARN("Graph data has changed! Reset cache..."); - cachedPoses_ = newPoses; cachedConstraints_ = newConstraints; + cachedNodeInfos_ = newNodeInfos; } //match poses in the graph - std::map poses; std::multimap constraints; + std::map nodeInfos; if(globalOptimization_) { - poses = cachedPoses_; constraints = cachedConstraints_; + nodeInfos = cachedNodeInfos_; } else { constraints = newConstraints; for(unsigned int i=0; igraph.posesId.size(); ++i) { - std::map::iterator iter = cachedPoses_.find(msg->graph.posesId[i]); - if(iter != cachedPoses_.end()) + std::map::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]); + if(iter != cachedNodeInfos_.end()) { - poses.insert(*iter); + nodeInfos.insert(*iter); } else { @@ -196,18 +199,25 @@ public: } } } + + std::map poses; + for(std::map::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter) + { + poses.insert(std::make_pair(iter->first, iter->second.getPose())); + } + // Optimize only if there is a subscriber if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers()) { UTimer timer; std::map optimizedPoses; Transform mapCorrection = Transform::getIdentity(); + std::map posesOut; std::multimap linksOut; if(poses.size() > 1 && constraints.size() > 0) { graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_); int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first; - std::map posesOut; optimizer.getConnectedGraph( fromId, poses, @@ -249,6 +259,33 @@ public: outputDataMsg.header = msg->header; outputDataMsg.graph = outputGraphMsg; outputDataMsg.nodes = msg->nodes; + if(posesOut.size() > msg->nodes.size()) + { + std::set addedNodes; + for(unsigned int i=0; inodes.size(); ++i) + { + addedNodes.insert(msg->nodes[i].id); + } + std::list toAdd; + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + if(addedNodes.find(iter->first) == addedNodes.end()) + { + toAdd.push_back(iter->first); + } + } + if(toAdd.size()) + { + int oi = outputDataMsg.nodes.size(); + outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size()); + for(std::list::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter) + { + UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end()); + rtabmap_ros::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]); + ++oi; + } + } + } mapDataPub_.publish(outputDataMsg); } @@ -272,8 +309,8 @@ private: ros::Publisher mapDataPub_; ros::Publisher mapGraphPub_; - std::map cachedPoses_; std::multimap cachedConstraints_; + std::map cachedNodeInfos_; tf2_ros::TransformBroadcaster tfBroadcaster_; boost::thread* transformThread_; diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index c19a9e34..1b626454 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -555,6 +555,28 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & } } +rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg) +{ + rtabmap::Signature s( + msg.id, + msg.mapId, + msg.weight, + msg.stamp, + msg.label, + transformFromPoseMsg(msg.pose)); + return s; +} +void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) +{ + // 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); +} + rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) { rtabmap::OdometryInfo info; From 1dfa4e7f953bd63541ca04d4778aefe773e9eb19 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 31 Oct 2015 15:20:57 -0400 Subject: [PATCH 012/119] Removed grid_map_assembler, map_assembler is now using MapsManager (now use same published topics and mapping parameters than rtabmap), added all optimization parameters to map_optimizer (g2o and GTSAM can be selected) --- CMakeLists.txt | 8 +- src/CoreWrapper.cpp | 11 +- src/GridMapAssemblerNode.cpp | 199 ------------------------ src/MapAssemblerNode.cpp | 283 ++++------------------------------- src/MapOptimizerNode.cpp | 38 +++-- src/MapsManager.cpp | 155 ++++++++++++++++--- src/MapsManager.h | 9 +- 7 files changed, 212 insertions(+), 491 deletions(-) delete mode 100644 src/GridMapAssemblerNode.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b2367744..09581113 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -145,6 +145,7 @@ SET(rtabmap_ros_lib_src src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp + src/MapsManager.cpp ${MOC_FILES} ) @@ -186,7 +187,7 @@ SET(Libraries add_definitions(-DWITH_OCTOMAP) ENDIF(octomap_ros_FOUND) -add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp src/MapsManager.cpp) +add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp) target_link_libraries(rtabmap rtabmap_ros ${Libraries}) add_executable(rgbd_odometry src/RGBDOdometryNode.cpp) @@ -201,9 +202,6 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries}) add_executable(map_assembler src/MapAssemblerNode.cpp) 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(camera src/CameraNode.cpp) add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(camera ${Libraries}) @@ -244,7 +242,6 @@ install(TARGETS rgbd_odometry stereo_odometry map_assembler - grid_map_assembler map_optimizer data_player camera @@ -259,7 +256,6 @@ install(TARGETS rgbd_odometry stereo_odometry map_assembler - grid_map_assembler map_optimizer data_player camera diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 9d764a3e..25cb0ab7 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : genScan_(false), genScanMaxDepth_(4.0), mapToOdom_(rtabmap::Transform::getIdentity()), + mapsManager_(true), depthSync_(0), depthScanSync_(0), stereoScanSync_(0), @@ -1244,6 +1245,7 @@ void CoreWrapper::process( false, false, false, + false, tmpSignature); mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); @@ -1663,6 +1665,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: rtabmap_.getMemory(), false, true, + false, false); if(filteredPoses.size()) { @@ -1706,7 +1709,8 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs:: rtabmap_.getMemory(), false, false, - true); + true, + false); if(filteredPoses.size()) { // create the grid map @@ -1818,6 +1822,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab false, false, false, + false, signatures); } else @@ -2282,7 +2287,7 @@ bool CoreWrapper::octomapBinaryCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); octomap::OcTree * octree = mapsManager_.createOctomap(poses); bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map); @@ -2302,7 +2307,7 @@ bool CoreWrapper::octomapFullCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false); octomap::OcTree * octree = mapsManager_.createOctomap(poses); bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map); diff --git a/src/GridMapAssemblerNode.cpp b/src/GridMapAssemblerNode.cpp deleted file mode 100644 index 70c1b10b..00000000 --- a/src/GridMapAssemblerNode.cpp +++ /dev/null @@ -1,199 +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 "rtabmap_ros/MapData.h" -#include "rtabmap_ros/MsgConversion.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace rtabmap; - -class GridMapAssembler -{ - -public: - GridMapAssembler() : - gridCellSize_(0.05), // meters - mapSize_(0), // meters - eroded_(false), - filterRadius_(0.5), - filterAngle_(30.0) // degrees - { - ros::NodeHandle pnh("~"); - pnh.param("cell_size", gridCellSize_, gridCellSize_); // m - pnh.param("map_size", mapSize_, mapSize_); // m - pnh.param("filter_radius", filterRadius_, filterRadius_); - pnh.param("filter_angle", filterAngle_, filterAngle_); - pnh.param("eroded", eroded_, eroded_); - - UASSERT(gridCellSize_ > 0.0); - UASSERT(mapSize_ >= 0.0); - - ros::NodeHandle nh; - mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this); - - gridMap_ = nh.advertise("grid_map", 1); - - //private service - getMapService_ = pnh.advertiseService("get_map", &GridMapAssembler::getGridMapCallback, this); - resetService_ = pnh.advertiseService("reset", &GridMapAssembler::reset, this); - } - - ~GridMapAssembler() - { - } - - void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) - { - UTimer timer; - for(unsigned int i=0; inodes.size(); ++i) - { - if(!uContains(gridMaps_, msg->nodes[i].id) && msg->nodes[i].laserScan.size()) - { - cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan); - if(!laserScan.empty()) - { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, gridCellSize_); - - if(!ground.empty() || !obstacles.empty()) - { - gridMaps_.insert(std::make_pair(msg->nodes[i].id, std::make_pair(ground, obstacles))); - } - } - } - } - - std::map poses; - UASSERT(msg->graph.posesId.size() == msg->graph.poses.size()); - for(unsigned int i=0; igraph.posesId.size(); ++i) - { - poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); - } - - if(filterRadius_ > 0.0 && filterAngle_ > 0.0) - { - poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0); - } - - if(gridMap_.getNumSubscribers()) - { - // create the map - float xMin=0.0f, yMin=0.0f; - //cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_); - cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - mapSize_, - eroded_); - - if(!pixels.empty()) - { - //init - map_.info.resolution = gridCellSize_; - map_.info.origin.position.x = 0.0; - map_.info.origin.position.y = 0.0; - map_.info.origin.position.z = 0.0; - map_.info.origin.orientation.x = 0.0; - map_.info.origin.orientation.y = 0.0; - map_.info.origin.orientation.z = 0.0; - map_.info.origin.orientation.w = 1.0; - - map_.info.width = pixels.cols; - map_.info.height = pixels.rows; - map_.info.origin.position.x = xMin; - map_.info.origin.position.y = yMin; - map_.data.resize(map_.info.width * map_.info.height); - - memcpy(map_.data.data(), pixels.data, map_.info.width * map_.info.height); - - map_.header.frame_id = msg->header.frame_id; - map_.header.stamp = ros::Time::now(); - - gridMap_.publish(map_); - ROS_INFO("Grid Map published [%d,%d] (%fs)", pixels.cols, pixels.rows, timer.ticks()); - } - } - } - - bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) - { - if(map_.data.size()) - { - res.map = map_; - return true; - } - return false; - } - - bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) - { - ROS_INFO("grid_map_assembler: reset!"); - gridMaps_.clear(); - map_ = nav_msgs::OccupancyGrid(); - return true; - } - -private: - double gridCellSize_; - double mapSize_; - bool eroded_; - double filterRadius_; - double filterAngle_; - - ros::Subscriber mapDataTopic_; - - ros::Publisher gridMap_; - - ros::ServiceServer getMapService_; - ros::ServiceServer resetService_; - - std::map > gridMaps_; // - - nav_msgs::OccupancyGrid map_; -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "grid_map_assembler"); - GridMapAssembler assembler; - ros::spin(); - return 0; -} diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index d75b33bf..4cf3a406 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MsgConversion.h" +#include "MapsManager.h" #include #include #include @@ -49,54 +50,13 @@ class MapAssembler public: MapAssembler() : - cloudDecimation_(4), - cloudMaxDepth_(4.0), - cloudVoxelSize_(0.02), - scanVoxelSize_(0.01), - nodeFilteringAngle_(30), // degrees - nodeFilteringRadius_(0.5), - noiseFilterRadius_(0.0), - noiseFilterMinNeighbors_(5), - computeOccupancyGrid_(false), - gridCellSize_(0.05), - groundMaxAngle_(M_PI_4), - clusterMinSize_(20), - maxHeight_(0), - occupancyMapSize_(0.0) + mapsManager_(false) { ros::NodeHandle pnh("~"); - pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); - pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); - pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - - pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_); - pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_); - - pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); - pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - - pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_); - pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_); - pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_); - pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_); - pnh.param("occupancy_max_height", maxHeight_, maxHeight_); - pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_); - - UASSERT(gridCellSize_ > 0); - UASSERT(maxHeight_ >= 0); - UASSERT(occupancyMapSize_ >=0.0); ros::NodeHandle nh; mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); - assembledMapClouds_ = nh.advertise("assembled_clouds", 1); - assembledMapScans_ = nh.advertise("assembled_scans", 1); - if(computeOccupancyGrid_) - { - occupancyMapPub_ = nh.advertise("grid_projection_map", 1); - } - // private service resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this); } @@ -108,239 +68,60 @@ public: void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) { UTimer timer; + + std::map poses; + std::multimap constraints; + Transform mapOdom; + rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom); for(unsigned int i=0; inodes.size(); ++i) { - int id = msg->nodes[i].id; - if(!uContains(rgbClouds_, id)) + if(msg->nodes[i].image.size() || + msg->nodes[i].depth.size() || + msg->nodes[i].laserScan.size()) { - rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]); - if(!s.sensorData().imageCompressed().empty() && - !s.sensorData().depthOrRightCompressed().empty() && - (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid())) - { - cv::Mat image, depth; - s.sensorData().uncompressData(&image, &depth, 0); - - - if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty()) - { - pcl::PointCloud::Ptr cloud; - cloud = rtabmap::util3d::cloudRGBFromSensorData( - s.sensorData(), - cloudDecimation_, - cloudMaxDepth_); - - if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloud, *indices, *tmp); - cloud = tmp; - } - if(cloud->size() && cloudVoxelSize_ > 0) - { - cloud = util3d::voxelize(cloud, cloudVoxelSize_); - } - - if(cloud->size()) - { - rgbClouds_.insert(std::make_pair(id, cloud)); - - if(computeOccupancyGrid_) - { - pcl::PointCloud::Ptr cloudClipped = cloud; - if(cloudClipped->size() && maxHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), maxHeight_); - } - if(cloudClipped->size()) - { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - - cv::Mat ground, obstacles; - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_); - if(!ground.empty() || !obstacles.empty()) - { - occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles))); - } - } - - } - } - } - } - } - - if(!uContains(scans_, id) && msg->nodes[i].laserScan.size()) - { - cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan); - if(!laserScan.empty()) - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(laserScan); - if(cloud->size() && scanVoxelSize_ > 0) - { - cloud = util3d::voxelize(cloud, scanVoxelSize_); - } - if(cloud->size()) - { - scans_.insert(std::make_pair(id, cloud)); - } - } + uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i]))); } } - // filter poses - std::map poses; - UASSERT(msg->graph.posesId.size() == msg->graph.poses.size()); - for(unsigned int i=0; igraph.posesId.size(); ++i) + // create a tmp signature with latest sensory data + if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end()) { - poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); - } - if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0) - { - poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0); + Signature tmpS = nodes_.at(poses.rbegin()->first); + SensorData tmpData = tmpS.sensorData(); + tmpData.setId(-1); + uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpData))); + poses.insert(std::make_pair(-1, poses.rbegin()->second)); } - if(assembledMapClouds_.getNumSubscribers()) - { - // generate the assembled cloud! - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + // Update maps + poses = mapsManager_.updateMapCaches( + poses, + 0, + false, + false, + false, + false, + nodes_); - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - std::map::Ptr >::iterator jter = rgbClouds_.find(iter->first); - if(jter != rgbClouds_.end()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - } - } + mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); - if(assembledCloud->size()) - { - if(cloudVoxelSize_ > 0) - { - assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_); - } - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = ros::Time::now(); - cloudMsg->header.frame_id = msg->header.frame_id; - assembledMapClouds_.publish(cloudMsg); - } - } - - if(assembledMapScans_.getNumSubscribers()) - { - // generate the assembled scan! - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - - for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - std::map::Ptr >::iterator jter = scans_.find(iter->first); - if(jter != scans_.end()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - } - } - - if(assembledCloud->size()) - { - if(scanVoxelSize_ > 0) - { - assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); - } - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = ros::Time::now(); - cloudMsg->header.frame_id = msg->header.frame_id; - assembledMapScans_.publish(cloudMsg); - } - } - - if(occupancyMapPub_.getNumSubscribers()) - { - // create the map - float xMin=0.0f, yMin=0.0f; - cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps( - poses, - occupancyLocalMaps_, - gridCellSize_, xMin, yMin, - occupancyMapSize_); - - if(!pixels.empty()) - { - //init - nav_msgs::OccupancyGrid map; - map.info.resolution = gridCellSize_; - map.info.origin.position.x = 0.0; - map.info.origin.position.y = 0.0; - map.info.origin.position.z = 0.0; - map.info.origin.orientation.x = 0.0; - map.info.origin.orientation.y = 0.0; - map.info.origin.orientation.z = 0.0; - map.info.origin.orientation.w = 1.0; - - map.info.width = pixels.cols; - map.info.height = pixels.rows; - map.info.origin.position.x = xMin; - map.info.origin.position.y = yMin; - map.data.resize(map.info.width * map.info.height); - - memcpy(map.data.data(), pixels.data, map.info.width * map.info.height); - - map.header.frame_id = msg->header.frame_id; - map.header.stamp = ros::Time::now(); - - occupancyMapPub_.publish(map); - } - } - ROS_INFO("Processing data %fs", timer.ticks()); + ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks()); } bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { ROS_INFO("map_assembler: reset!"); - occupancyLocalMaps_.clear(); - rgbClouds_.clear(); - scans_.clear(); + mapsManager_.clear(); return true; } private: - int cloudDecimation_; - double cloudMaxDepth_; - double cloudVoxelSize_; - double scanVoxelSize_; - - double nodeFilteringAngle_; - double nodeFilteringRadius_; - - double noiseFilterRadius_; - double noiseFilterMinNeighbors_; - - bool computeOccupancyGrid_; - double gridCellSize_; - double groundMaxAngle_; - int clusterMinSize_; - double maxHeight_; - double occupancyMapSize_; - - std::map > occupancyLocalMaps_; // + MapsManager mapsManager_; + std::map nodes_; ros::Subscriber mapDataTopic_; - ros::Publisher assembledMapClouds_; - ros::Publisher assembledMapScans_; - ros::Publisher occupancyMapPub_; - ros::ServiceServer resetService_; - - std::map::Ptr > rgbClouds_; - std::map::Ptr > scans_; }; diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index c32c09c3..33740d04 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -48,8 +49,6 @@ public: MapOptimizer() : mapFrameId_("map"), odomFrameId_("odom"), - iterations_(100), - ignoreVariance_(false), globalOptimization_(true), optimizeFromLastNode_(false), mapToOdom_(rtabmap::Transform::getIdentity()), @@ -58,14 +57,35 @@ public: ros::NodeHandle nh; ros::NodeHandle pnh("~"); + double epsilon = 0.0; + bool robust = true; + bool slam2d =false; + int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM + int iterations = 100; + bool ignoreVariance = false; + pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); - pnh.param("iterations", iterations_, iterations_); - pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_); + pnh.param("iterations", iterations, iterations); + pnh.param("ignore_variance", ignoreVariance, ignoreVariance); pnh.param("global_optimization", globalOptimization_, globalOptimization_); pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_); + pnh.param("epsilon", epsilon, epsilon); + pnh.param("robust", robust, robust); + pnh.param("slam_2d", slam2d, slam2d); + pnh.param("strategy", strategy, strategy); - UASSERT(iterations_ > 0); + + UASSERT(iterations > 0); + + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeStrategy(), uNumber2Str(strategy))); + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeEpsilon(), uNumber2Str(epsilon))); + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(iterations))); + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeRobust(), uBool2Str(robust))); + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeSlam2D(), uBool2Str(slam2d))); + parameters.insert(ParametersPair(Parameters::kRGBDOptimizeVarianceIgnored(), uBool2Str(ignoreVariance))); + optimizer_ = graph::Optimizer::create(parameters); double tfDelay = 0.05; // 20 Hz bool publishTf = true; @@ -216,15 +236,14 @@ public: std::multimap linksOut; if(poses.size() > 1 && constraints.size() > 0) { - graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_); int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first; - optimizer.getConnectedGraph( + optimizer_->getConnectedGraph( fromId, poses, constraints, posesOut, linksOut); - optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut); + optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut); mapToOdomMutex_.lock(); mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse(); mapToOdom_ = mapCorrection; @@ -296,10 +315,9 @@ public: private: std::string mapFrameId_; std::string odomFrameId_; - int iterations_; - bool ignoreVariance_; bool globalOptimization_; bool optimizeFromLastNode_; + graph::Optimizer * optimizer_; rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index c29785ab..43dc4eeb 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -29,13 +29,15 @@ using namespace rtabmap; -MapsManager::MapsManager() : +MapsManager::MapsManager(bool usePublicNamespace) : cloudDecimation_(4), cloudMaxDepth_(4.0), // meters cloudVoxelSize_(0.05), // meters cloudFloorCullingHeight_(0.0), cloudOutputVoxelized_(false), cloudFrustumCulling_(false), + scanVoxelSize_(0.0), + scanOutputVoxelized_(false), projMaxGroundAngle_(45.0), // degrees projMinClusterSize_(20), projMaxHeight_(2.0), // meters @@ -58,6 +60,12 @@ MapsManager::MapsManager() : pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); + pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_); + pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_); + + // scan map stuff + pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); + pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_); //projection map stuff pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); @@ -75,10 +83,27 @@ MapsManager::MapsManager() : pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_); pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); + // If true, the last message published on + // the map topics will be saved and sent to new subscribers when they + // connect + bool latch = true; + pnh.param("latch", latch, latch); + // mapping topics - cloudMapPub_ = nh.advertise("cloud_map", 1); - projMapPub_ = nh.advertise("proj_map", 1); - gridMapPub_ = nh.advertise("grid_map", 1); + if(usePublicNamespace) + { + cloudMapPub_ = nh.advertise("cloud_map", 1, latch); + projMapPub_ = nh.advertise("proj_map", 1, latch); + gridMapPub_ = nh.advertise("grid_map", 1, latch); + scanMapPub_ = nh.advertise("scan_map", 1, latch); + } + else + { + cloudMapPub_ = pnh.advertise("cloud_map", 1, latch); + projMapPub_ = pnh.advertise("proj_map", 1, latch); + gridMapPub_ = pnh.advertise("grid_map", 1, latch); + scanMapPub_ = pnh.advertise("scan_map", 1, latch); + } } MapsManager::~MapsManager() { @@ -97,7 +122,8 @@ bool MapsManager::hasSubscribers() const { return cloudMapPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || - gridMapPub_.getNumSubscribers() != 0; + gridMapPub_.getNumSubscribers() != 0 || + scanMapPub_.getNumSubscribers() != 0; } std::map MapsManager::getFilteredPoses(const std::map & poses) @@ -117,28 +143,30 @@ std::map MapsManager::updateMapCaches( bool updateCloud, bool updateProj, bool updateGrid, + bool updateScan, const std::map & signatures) { - if(!updateCloud && !updateProj && !updateGrid) + if(!updateCloud && !updateProj && !updateGrid && !updateScan) { // all false, udpate only those where we have subscribers updateCloud = cloudMapPub_.getNumSubscribers() != 0; updateProj = projMapPub_.getNumSubscribers() != 0; updateGrid = gridMapPub_.getNumSubscribers() != 0; + updateScan = scanMapPub_.getNumSubscribers() != 0; } UDEBUG("Updating map caches..."); if(!memory && signatures.size() == 0) { - ROS_FATAL("Memory should not be null!?"); + ROS_ERROR("Memory and signatures should not be both null!?"); return std::map(); } std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid) + if(updateCloud || updateProj || updateGrid || updateScan) { // filter nodes if(mapFilterRadius_ > 0.0) @@ -170,11 +198,13 @@ std::map MapsManager::updateMapCaches( rtabmap::SensorData data; bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first)); - bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); + bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); + bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); if(rgbDepthRequired || depthRequired || - scanRequired) + scanRequired || + gridRequired) { std::map::const_iterator findIter = signatures.find(iter->first); if(findIter != signatures.end()) @@ -198,7 +228,7 @@ std::map MapsManager::updateMapCaches( data.uncompressData( (rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0, (rgbDepthRequired||depthRequired) ? &depth:0, - scanRequired?&scan:0); + scanRequired||gridRequired?&scan:0); pcl::PointCloud::Ptr cloudRGB; pcl::PointCloud::Ptr cloudXYZ; @@ -211,6 +241,13 @@ std::map MapsManager::updateMapCaches( cloudDecimation_, cloudMaxDepth_, cloudVoxelSize_); + if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) + { + pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*cloudRGB, *indices, *tmp); + cloudRGB = tmp; + } } else { @@ -226,6 +263,13 @@ std::map MapsManager::updateMapCaches( cloudDecimation_, cloudMaxDepth_, gridCellSize_); // use gridCellSize since this cloud is only for the projection map + if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) + { + pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::copyPointCloud(*cloudXYZ, *indices, *tmp); + cloudXYZ = tmp; + } } else { @@ -273,7 +317,7 @@ std::map MapsManager::updateMapCaches( { cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxHeight_); } - if(cloudClipped->size()) + if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) { cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); @@ -294,11 +338,30 @@ std::map MapsManager::updateMapCaches( uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); } - if(scanRequired) + if(scanRequired || gridRequired) { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0)) + { + pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); + if(scanVoxelSize_ > 0.0) + { + scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); + if(gridRequired) + { + scan = util3d::laserScanFromPointCloud(*scanCloud); + } + } + if(scanRequired) + { + uInsert(scans_, std::make_pair(iter->first, scanCloud)); + } + } + if(gridRequired) + { + cv::Mat ground, obstacles; + util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); + uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); + } } } else @@ -428,20 +491,23 @@ void MapsManager::publishMaps( cloudMaxDepth_>0.0?cloudMaxDepth_:999999., true); //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; + if(jter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); + *assembledCloud+=*transformed; + } } } } } } - if(cloudFloorCullingHeight_ > 0.0) + if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0) { assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f); } - if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_) + if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) { assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); } @@ -456,7 +522,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size()); + ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); } } else if(mapCacheCleanup_) @@ -465,6 +531,53 @@ void MapsManager::publishMaps( cameraModels_.clear(); } + if(scanMapPub_.getNumSubscribers()) + { + // generate the assembled scan cloud! + UTimer time; + pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); + int count = 0; + std::list > negativePoses; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(iter->first > 0) + { + std::map::Ptr >::iterator jter = scans_.find(iter->first); + if(jter != scans_.end() && jter->second->size()) + { + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); + *assembledCloud+=*transformed; + ++count; + } + } + // negative poses are not used + } + + if(assembledCloud->size()) + { + if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_) + { + assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); + } + + ROS_INFO("Assembled %d scans (%fs)", count, time.ticks()); + + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*assembledCloud, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + scanMapPub_.publish(cloudMsg); + } + else if(poses.size()) + { + ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size()); + } + } + else if(mapCacheCleanup_) + { + scans_.clear(); + } + if(projMapPub_.getNumSubscribers()) { // create the projection map diff --git a/src/MapsManager.h b/src/MapsManager.h index 9e24816d..2261b6ac 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -26,7 +26,7 @@ class Memory; class MapsManager { public: - MapsManager(); + MapsManager(bool usePublicNamespace); virtual ~MapsManager(); void clear(); bool hasSubscribers() const; @@ -40,6 +40,7 @@ public: bool updateCloud, bool updateProj, bool updateGrid, + bool updateScan, const std::map & signatures = std::map()); void publishMaps( @@ -71,6 +72,10 @@ private: double cloudFloorCullingHeight_; bool cloudOutputVoxelized_; bool cloudFrustumCulling_; + double cloudNoiseFilteringRadius_; + int cloudNoiseFilteringMinNeighbors_; + double scanVoxelSize_; + bool scanOutputVoxelized_; double projMaxGroundAngle_; int projMinClusterSize_; double projMaxHeight_; @@ -85,8 +90,10 @@ private: ros::Publisher cloudMapPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; + ros::Publisher scanMapPub_; std::map::Ptr > clouds_; + std::map::Ptr > scans_; std::map > cameraModels_; std::map > projMaps_; // std::map > gridMaps_; // From d50652cbc5b922070b506560a2bb07f79dbebf31 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 31 Oct 2015 16:37:39 -0400 Subject: [PATCH 013/119] Updated demo_stereo_outdoor.launch (removed map_optimizer, updated parameters) --- launch/config/demo_stereo_outdoor.rviz | 25 +++++++++++-------- launch/demo/demo_stereo_outdoor.launch | 34 +++++++++----------------- 2 files changed, 27 insertions(+), 32 deletions(-) diff --git a/launch/config/demo_stereo_outdoor.rviz b/launch/config/demo_stereo_outdoor.rviz index 1b8076c3..8c27f14d 100644 --- a/launch/config/demo_stereo_outdoor.rviz +++ b/launch/config/demo_stereo_outdoor.rviz @@ -147,17 +147,22 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Points - Topic: /rtabmap/mapData_optimized + Topic: /rtabmap/mapData Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Class: rtabmap_ros/MapGraph - Color: 25; 255; 0 Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 Name: MapGraph + Neighbor: 0; 0; 255 Topic: /rtabmap/mapDataGraph_optimized + User: 255; 0; 0 Value: true + Virtual: 255; 0; 255 - Class: rviz/Image Enabled: true Image Topic: /stereo_camera/left/image_rect_color @@ -173,10 +178,10 @@ Visualization Manager: Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: false + Enabled: true Name: Map - Topic: /map - Value: false + Topic: /rtabmap/proj_map + Value: true - Class: rtabmap_ros/Info Enabled: true Name: Info @@ -206,7 +211,7 @@ Visualization Manager: Views: Current: Class: rtabmap_ros/OrbitOriented - Distance: 6.60197 + Distance: 8.28384 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 @@ -218,10 +223,10 @@ Visualization Manager: Z: 0.113349 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.455398 + Pitch: 0.635398 Target Frame: base_footprint Value: OrbitOriented (rtabmap) - Yaw: 3.1304 + Yaw: 3.0704 Saved: ~ Window Geometry: Displays: @@ -241,5 +246,5 @@ Window Geometry: Views: collapsed: false Width: 1341 - X: 147 - Y: 48 + X: 117 + Y: 18 diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index ed05a852..dc959a19 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -43,19 +43,21 @@ - + - + + - - + + + @@ -70,7 +72,7 @@ - + @@ -81,28 +83,16 @@ - + - - + - - - - - - - - - - - - + @@ -116,8 +106,8 @@ - - + + From facfbbb9dc253c56a1b00db86cf27c9f2763f652 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Nov 2015 21:27:36 -0500 Subject: [PATCH 014/119] MapsManager: added cloudNoiseFitleringXXX variables init --- src/MapsManager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 43dc4eeb..8ef295fb 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -36,6 +36,8 @@ MapsManager::MapsManager(bool usePublicNamespace) : cloudFloorCullingHeight_(0.0), cloudOutputVoxelized_(false), cloudFrustumCulling_(false), + cloudNoiseFilteringRadius_(0.0), + cloudNoiseFilteringMinNeighbors_(5), scanVoxelSize_(0.0), scanOutputVoxelized_(false), projMaxGroundAngle_(45.0), // degrees From 1cba7053d047d504160fe37fe7f95410d4fbf79c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 6 Nov 2015 14:52:37 -0500 Subject: [PATCH 015/119] Updated az3_mapping_robot_kinect_scan.launch demo --- .../az3_mapping_robot_kinect_scan.launch | 169 ++++++++++++------ .../config/costmap_common_params_2d.yaml | 8 +- .../azimut3/config/local_costmap_params.yaml | 4 +- 3 files changed, 120 insertions(+), 61 deletions(-) diff --git a/launch/azimut3/az3_mapping_robot_kinect_scan.launch b/launch/azimut3/az3_mapping_robot_kinect_scan.launch index bb5406dd..546ea967 100644 --- a/launch/azimut3/az3_mapping_robot_kinect_scan.launch +++ b/launch/azimut3/az3_mapping_robot_kinect_scan.launch @@ -1,69 +1,128 @@ - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml index 7c7e531d..9e7325f8 100644 --- a/launch/azimut3/config/costmap_common_params_2d.yaml +++ b/launch/azimut3/config/costmap_common_params_2d.yaml @@ -17,7 +17,7 @@ obstacle_layer: laser_scan_sensor: { data_type: LaserScan, topic: base_scan, - expected_update_rate: 0.2, + expected_update_rate: 0.1, marking: true, clearing: true } @@ -26,7 +26,7 @@ obstacle_layer: sensor_frame: base_footprint, data_type: PointCloud2, topic: obstacles_cloud, - expected_update_rate: 0.5, + expected_update_rate: 0.2, marking: true, clearing: true, min_obstacle_height: 0.04 @@ -36,10 +36,10 @@ obstacle_layer: sensor_frame: base_footprint, data_type: PointCloud2, topic: ground_cloud, - expected_update_rate: 0.5, + expected_update_rate: 0.2, marking: false, clearing: true, - min_obstacle_height: -1.0 # make usre the ground is not filtered + min_obstacle_height: -1.0 # make sure the ground is not filtered } diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml index 87950d24..1d7cf928 100644 --- a/launch/azimut3/config/local_costmap_params.yaml +++ b/launch/azimut3/config/local_costmap_params.yaml @@ -1,7 +1,7 @@ global_frame: odom robot_base_frame: base_footprint -update_frequency: 2 -publish_frequency: 1 +update_frequency: 5 +publish_frequency: 2 rolling_window: true width: 3.0 height: 3.0 From 80addaf9264b3057f2fb45ec60117d65b0067247 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 6 Nov 2015 16:12:28 -0500 Subject: [PATCH 016/119] Updated az3_mapping_robot_kinect_scan.launch (fixed openni rate) --- .../az3_mapping_robot_kinect_scan.launch | 19 +++++++++++-------- launch/azimut3/az3_openni.launch | 3 ++- .../config/costmap_common_params_2d.yaml | 10 ++++------ .../azimut3/config/global_costmap_params.yaml | 2 +- .../azimut3/config/local_costmap_params.yaml | 4 ++-- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/launch/azimut3/az3_mapping_robot_kinect_scan.launch b/launch/azimut3/az3_mapping_robot_kinect_scan.launch index 546ea967..1c5158b8 100644 --- a/launch/azimut3/az3_mapping_robot_kinect_scan.launch +++ b/launch/azimut3/az3_mapping_robot_kinect_scan.launch @@ -2,7 +2,7 @@ - + @@ -28,8 +28,9 @@ - - + + + @@ -39,7 +40,8 @@ - + + @@ -53,7 +55,7 @@ - + @@ -83,7 +85,7 @@ - + @@ -95,7 +97,8 @@ - + + @@ -104,7 +107,7 @@ - + diff --git a/launch/azimut3/az3_openni.launch b/launch/azimut3/az3_openni.launch index 8982436d..aae74211 100644 --- a/launch/azimut3/az3_openni.launch +++ b/launch/azimut3/az3_openni.launch @@ -2,6 +2,7 @@ + - \ No newline at end of file + diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml index 9e7325f8..9084d0d8 100644 --- a/launch/azimut3/config/costmap_common_params_2d.yaml +++ b/launch/azimut3/config/costmap_common_params_2d.yaml @@ -1,7 +1,5 @@ footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]] -footprint_padding: 0.02 -#robot_radius: 0.38 -#robot_radius: ir_of_robot +footprint_padding: 0.04 inflation_layer: inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles transform_tolerance: 2 @@ -16,7 +14,7 @@ obstacle_layer: laser_scan_sensor: { data_type: LaserScan, - topic: base_scan, + topic: scan, expected_update_rate: 0.1, marking: true, clearing: true @@ -26,7 +24,7 @@ obstacle_layer: sensor_frame: base_footprint, data_type: PointCloud2, topic: obstacles_cloud, - expected_update_rate: 0.2, + expected_update_rate: 0.5, marking: true, clearing: true, min_obstacle_height: 0.04 @@ -36,7 +34,7 @@ obstacle_layer: sensor_frame: base_footprint, data_type: PointCloud2, topic: ground_cloud, - expected_update_rate: 0.2, + expected_update_rate: 0.5, marking: false, clearing: true, min_obstacle_height: -1.0 # make sure the ground is not filtered diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml index 643ec7e6..12895d12 100644 --- a/launch/azimut3/config/global_costmap_params.yaml +++ b/launch/azimut3/config/global_costmap_params.yaml @@ -2,7 +2,7 @@ global_frame: map robot_base_frame: base_footprint update_frequency: 1 -publish_frequency: 2 +publish_frequency: 1 always_send_full_costmap: false plugins: - {name: static_layer, type: "rtabmap_ros::StaticLayer"} diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml index 1d7cf928..87950d24 100644 --- a/launch/azimut3/config/local_costmap_params.yaml +++ b/launch/azimut3/config/local_costmap_params.yaml @@ -1,7 +1,7 @@ global_frame: odom robot_base_frame: base_footprint -update_frequency: 5 -publish_frequency: 2 +update_frequency: 2 +publish_frequency: 1 rolling_window: true width: 3.0 height: 3.0 From 51cb5ed452655ed0b7c650e5c98db394ca5abf29 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 11 Nov 2015 14:44:21 -0500 Subject: [PATCH 017/119] updated azimu3_nav.rviz --- launch/azimut3/config/azimut3_nav.rviz | 176 ++++++++++++++++++++++--- 1 file changed, 160 insertions(+), 16 deletions(-) diff --git a/launch/azimut3/config/azimut3_nav.rviz b/launch/azimut3/config/azimut3_nav.rviz index b5f7631a..061978da 100644 --- a/launch/azimut3/config/azimut3_nav.rviz +++ b/launch/azimut3/config/azimut3_nav.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /TF1/Frames1 Splitter Ratio: 0.601881 - Tree Height: 187 + Tree Height: 353 - Class: rviz/Selection Name: Selection - Class: rviz/Views @@ -19,7 +19,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Info - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 @@ -52,13 +52,73 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false + base_footprint: + Value: true + base_laser_link: + Value: true + base_link: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + camera_rgb_frame: + Value: true + camera_rgb_optical_frame: + Value: true + map: + Value: true + odom: + Value: true + wheelLB_linkWheel_link: + Value: true + wheelLB_wheel_link: + Value: true + wheelLF_linkWheel_link: + Value: true + wheelLF_wheel_link: + Value: true + wheelRB_linkWheel_link: + Value: true + wheelRB_wheel_link: + Value: true + wheelRF_linkWheel_link: + Value: true + wheelRF_wheel_link: + Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + map: + odom: + base_footprint: + base_link: + base_laser_link: + {} + camera_link: + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_rgb_frame: + camera_rgb_optical_frame: + {} + wheelLB_linkWheel_link: + wheelLB_wheel_link: + {} + wheelLF_linkWheel_link: + wheelLF_wheel_link: + {} + wheelRB_linkWheel_link: + wheelRB_wheel_link: + {} + wheelRF_linkWheel_link: + wheelRF_wheel_link: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -102,18 +162,18 @@ Visualization Manager: Class: rviz/Map Color Scheme: costmap Draw Behind: false - Enabled: false + Enabled: true Name: Global costmap Topic: /planner/move_base/global_costmap/costmap - Value: false + Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: costmap Draw Behind: false - Enabled: true + Enabled: false Name: Local costmap Topic: /planner/move_base/local_costmap/costmap - Value: true + Value: false - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -124,6 +184,61 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLB_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLB_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLF_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelLF_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRB_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRB_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRF_linkWheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheelRF_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -132,7 +247,7 @@ Visualization Manager: Visual Enabled: true - Class: rviz/Image Enabled: true - Image Topic: /camera/data_resized_image_relay + Image Topic: /camera/throttled_image Max Value: 1 Median window: 5 Min Value: 0 @@ -171,7 +286,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Points - Topic: /rtabmap/mapData_relay + Topic: /rtabmap/mapData Use Fixed Frame: true Use rainbow: true Value: true @@ -185,7 +300,13 @@ Visualization Manager: Class: rviz/Path Color: 255; 149; 57 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: move_base global plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /planner/move_base/NavfnROS/plan Value: true - Alpha: 1 @@ -193,7 +314,13 @@ Visualization Manager: Class: rviz/Path Color: 2; 14; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: move_base local plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /planner/move_base/TrajectoryPlannerROS/local_plan Value: true - Alpha: 1 @@ -227,17 +354,28 @@ Visualization Manager: Value: true - Alpha: 1 Class: rtabmap_ros/MapGraph - Color: 0; 0; 255 Enabled: true + Global loop closure: 255; 0; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 Name: MapGraph - Topic: /rtabmap/mapData_relay + Neighbor: 0; 0; 255 + Topic: /rtabmap/mapGraph + User: 255; 0; 0 Value: true + Virtual: 255; 0; 255 - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Rtabmap global path + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /rtabmap/global_path Value: true - Alpha: 1 @@ -245,7 +383,13 @@ Visualization Manager: Class: rviz/Path Color: 85; 255; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Rtabmap local path + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /rtabmap/local_path Value: true - Alpha: 1 @@ -308,10 +452,10 @@ Visualization Manager: Z: -0.0856586 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.884797 + Pitch: 1.2448 Target Frame: base_footprint Value: Orbit (rviz) - Yaw: 3.81544 + Yaw: 3.76044 Saved: ~ Window Geometry: Displays: @@ -321,7 +465,7 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000dd00fffffffb0000000a0049006d00610067006501000001d0000000870000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -331,5 +475,5 @@ Window Geometry: Views: collapsed: false Width: 1228 - X: 406 - Y: 154 + X: 396 + Y: 144 From 4f49425eac73bddf60d11c1350955f4aba10a8f9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 18 Nov 2015 15:18:45 -0500 Subject: [PATCH 018/119] Simplified how linking to rtabmap_ros using catkin so that only rtabmap_ros dependency is required: added CATKIN_DEPENDS optional costmap_2d and octomap_ros, added DEPENDS RTABMap and OpenCV. --- CMakeLists.txt | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 09581113..c74742dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,13 +86,23 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg) ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need + +SET(optional_dependencies "") +IF(costmap_2d_FOUND) +SET(optional_dependencies ${optional_dependencies} costmap_2d ) +ENDIF(costmap_2d_FOUND) +IF(octomap_ros_FOUND) +SET(optional_dependencies ${optional_dependencies} octomap_ros) +ENDIF(octomap_ros_FOUND) + catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_ros CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader - stereo_msgs move_base_msgs + stereo_msgs move_base_msgs ${optional_dependencies} + DEPENDS RTABMap OpenCV ) ########### From aa05bc4672de8376bfc0b3e559e0170099d081fd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 19 Nov 2015 12:26:29 -0500 Subject: [PATCH 019/119] Added "log_debug", "log_info", "log_warning" and "log_error" services --- src/CoreNode.cpp | 4 ++++ src/CoreWrapper.cpp | 30 ++++++++++++++++++++++++++++++ src/CoreWrapper.h | 8 ++++++++ src/OdometryROS.cpp | 31 +++++++++++++++++++++++++++++++ src/OdometryROS.h | 8 ++++++++ 5 files changed, 81 insertions(+) diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 66f53b27..31fff376 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -56,6 +56,10 @@ int main(int argc, char** argv) { ULogger::setLevel(ULogger::kInfo); } + else if(strcmp(argv[i], "--uwarn") == 0) + { + ULogger::setLevel(ULogger::kWarning); + } else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 25cb0ab7..eee467f4 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -414,6 +414,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this); octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this); #endif + //private services + setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this); + setLogInfoSrv_ = pnh.advertiseService("log_info", &CoreWrapper::setLogInfo, this); + setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); + setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras); @@ -1615,6 +1620,31 @@ bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Em return true; } +bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Debug"); + ULogger::setLevel(ULogger::kDebug); + return true; +} +bool CoreWrapper::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Info"); + ULogger::setLevel(ULogger::kInfo); + return true; +} +bool CoreWrapper::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Warning"); + ULogger::setLevel(ULogger::kWarning); + return true; +} +bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("rtabmap: Set log level to Error"); + ULogger::setLevel(ULogger::kError); + return true; +} + bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) { ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 82ecd099..a5b861d9 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -194,6 +194,10 @@ private: bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); @@ -374,6 +378,10 @@ private: ros::ServiceServer backupDatabase_; ros::ServiceServer setModeLocalizationSrv_; ros::ServiceServer setModeMappingSrv_; + ros::ServiceServer setLogDebugSrv_; + ros::ServiceServer setLogInfoSrv_; + ros::ServiceServer setLogWarnSrv_; + ros::ServiceServer setLogErrorSrv_; ros::ServiceServer getMapDataSrv_; ros::ServiceServer getProjMapSrv_; ros::ServiceServer getGridMapSrv_; diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 0b135526..86601451 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -255,6 +255,11 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this); pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this); resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this); + + setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this); + setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this); + setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this); + setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this); } OdometryROS::~OdometryROS() @@ -550,4 +555,30 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&) return true; } +bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("visual_odometry: Set log level to Debug"); + ULogger::setLevel(ULogger::kDebug); + return true; +} +bool OdometryROS::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("visual_odometry: Set log level to Info"); + ULogger::setLevel(ULogger::kInfo); + return true; +} +bool OdometryROS::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("visual_odometry: Set log level to Warning"); + ULogger::setLevel(ULogger::kWarning); + return true; +} +bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + ROS_INFO("visual_odometry: Set log level to Error"); + ULogger::setLevel(ULogger::kError); + return true; +} + + } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 5d37cc9b..0e95542b 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -61,6 +61,10 @@ public: bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&); bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); const std::string & frameId() const {return frameId_;} const std::string & odomFrameId() const {return odomFrameId_;} @@ -90,6 +94,10 @@ private: ros::ServiceServer resetToPoseSrv_; ros::ServiceServer pauseSrv_; ros::ServiceServer resumeSrv_; + ros::ServiceServer setLogDebugSrv_; + ros::ServiceServer setLogInfoSrv_; + ros::ServiceServer setLogWarnSrv_; + ros::ServiceServer setLogErrorSrv_; tf2_ros::TransformBroadcaster tfBroadcaster_; tf::TransformListener tfListener_; From 78b061eb2bc13e72ac49cd52dc95f381717d4797 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 25 Nov 2015 12:16:59 -0500 Subject: [PATCH 020/119] Added parameters backward compatibility approach from 0.11.0 --- CMakeLists.txt | 2 +- src/CoreWrapper.cpp | 91 ++++++++++----------------------------------- src/OdometryROS.cpp | 74 +++++++++++++----------------------- 3 files changed, 46 insertions(+), 121 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c74742dd..3bc31f84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(octomap_ros) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.10.11 REQUIRED) +find_package(RTABMap 0.11.0 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index eee467f4..3b972ea8 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -258,83 +258,32 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("LccReextract/LoopClosureFeatures"); - oldParameterNames.push_back("Rtabmap/DetectorStrategy"); - oldParameterNames.push_back("RGBD/ScanMatchingSize"); - oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius"); - oldParameterNames.push_back("RGBD/ToroIterations"); - oldParameterNames.push_back("Mem/RehearsedNodesKept"); - oldParameterNames.push_back("Odom/PnPEstimation"); - oldParameterNames.push_back("LccBow/MaxDepth"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("GFTT/MaxCorners") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: GFTT/MaxCorners -> %s. Please update your launch file accordingly.", - Parameters::kKpWordsPerImage().c_str()); + // can be migrated + parameters_.at(iter->second.second)= vStr; + ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } - else if(iter->compare("LccBow/MaxDepth") == 0) + else { - ROS_WARN("Parameter name changed: LccBow/MaxDepth -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractMaxDepth().c_str()); - parameters_.at(Parameters::kLccReextractMaxDepth())= vStr; - } - else if(iter->compare("LccReextract/LoopClosureFeatures") == 0) - { - ROS_WARN("Parameter name changed: LccReextract/LoopClosureFeatures -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractActivated().c_str()); - parameters_.at(Parameters::kLccReextractActivated())= vStr; - } - else if(iter->compare("Rtabmap/DetectorStrategy") == 0) - { - ROS_WARN("Parameter name changed: Rtabmap/DetectorStrategy -> %s. Please update your launch file accordingly.", - Parameters::kKpDetectorStrategy().c_str()); - parameters_.at(Parameters::kKpDetectorStrategy())= vStr; - } - else if(iter->compare("RGBD/ScanMatchingSize") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ScanMatchingSize -> %s. Please update your launch file accordingly.", - Parameters::kRGBDPoseScanMatching().c_str()); - parameters_.at(Parameters::kRGBDPoseScanMatching())= std::atoi(vStr.c_str()) > 0?"true":"false"; - } - else if(iter->compare("RGBD/LocalLoopDetectionRadius") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionRadius -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalRadius().c_str()); - parameters_.at(Parameters::kRGBDLocalRadius())= vStr; - } - else if(iter->compare("RGBD/ToroIterations") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ToroIterations -> %s. Please update your launch file accordingly.", - Parameters::kRGBDOptimizeIterations().c_str()); - parameters_.at(Parameters::kRGBDOptimizeIterations())= vStr; - } - else if(iter->compare("Mem/RehearsedNodesKept") == 0) - { - ROS_WARN("Parameter name changed: Mem/RehearsedNodesKept -> %s. Please update your launch file accordingly.", - Parameters::kMemNotLinkedNodesKept().c_str()); - parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr; - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionMaxDiffID -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str()); - parameters_.at(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth())= vStr; - } - else if(iter->compare("RGBD/PlanVirtualLinksMaxDiffID") == 0) - { - ROS_WARN("Parameter \"RGBD/PlanVirtualLinksMaxDiffID\" doesn't exist anymore."); - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: Odom/PnPEstimation -> %s. Please update your launch file accordingly.", - Parameters::kOdomEstimationType().c_str()); - parameters_.at(Parameters::kOdomEstimationType())= uNumber2Str(1); + if(iter->second.second.empty()) + { + ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 86601451..7b69e3c7 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -174,7 +174,7 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : iter->second = uNumber2Str(vInt); } - if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) + if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) { ROS_WARN("Parameter min_inliers must be >= 8, setting to 8..."); iter->second = uNumber2Str(8); @@ -182,54 +182,32 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("Odom/Type"); - oldParameterNames.push_back("Odom/MaxWords"); - oldParameterNames.push_back("Odom/WordsRatio"); - oldParameterNames.push_back("Odom/LocalHistory"); - oldParameterNames.push_back("Odom/NearestNeighbor"); - oldParameterNames.push_back("Odom/NNDR"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("Odom/Type") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.", - Parameters::kOdomFeatureType().c_str()); - parameters_.at(Parameters::kOdomFeatureType())= vStr; + // can be migrated + parameters_.at(iter->second.second)= vStr; + ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } - else if(iter->compare("Odom/MaxWords") == 0) + else { - ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; - } - else if(iter->compare("Odom/LocalHistory") == 0) - { - ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowLocalHistorySize().c_str()); - parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr; - } - else if(iter->compare("Odom/NearestNeighbor") == 0) - { - ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNType().c_str()); - parameters_.at(Parameters::kOdomBowNNType())= vStr; - } - else if(iter->compare("Odom/NNDR") == 0) - { - ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNDR().c_str()); - parameters_.at(Parameters::kOdomBowNNDR())= vStr; - } - else if(iter->compare("GFTT/MaxCorners") == 0) - { - ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; + if(iter->second.second.empty()) + { + ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } @@ -289,15 +267,13 @@ rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) group.compare("FREAK") == 0 || group.compare("BRIEF") == 0 || group.compare("GFTT") == 0 || - group.compare("BRISK") == 0) + group.compare("BRISK") == 0 || + group.compare("Reg") == 0 || + group.compare("Vis") == 0) { if(stereo) { - if(iter->first.compare(Parameters::kOdomMaxDepth()) == 0) - { - iter->second = "0"; // infinity - } - else if(iter->first.compare(Parameters::kOdomEstimationType()) == 0) + if(iter->first.compare(Parameters::kVisEstimationType()) == 0) { iter->second = "1"; // 3D->2D (PNP) } From b44b8ca73204a91fae754a938f895a5c59a5a814 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 25 Nov 2015 15:46:56 -0500 Subject: [PATCH 021/119] fixed last goal not reached in localization mode --- CMakeLists.txt | 2 +- src/CoreWrapper.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c74742dd..60878203 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(octomap_ros) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.10.11 REQUIRED) +find_package(RTABMap 0.10.12 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index eee467f4..84341e19 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1291,7 +1291,7 @@ void CoreWrapper::process( if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) { if(latestNodeWasReached_ || - rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || + rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) { latestNodeWasReached_ = true; @@ -1411,7 +1411,7 @@ void CoreWrapper::goalCommonCallback( // Adjust the target pose relative to last node if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) { - if(rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) + if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) { latestNodeWasReached_ = true; currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); From 542437c135a79698f847f25e84a4c8b3d7aea2f0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 26 Nov 2015 10:42:04 -0500 Subject: [PATCH 022/119] Updated variance check for Inf too --- src/CoreWrapper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 84341e19..94ba6404 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -929,8 +929,8 @@ void CoreWrapper::commonDepthCallback( rtabmap_ros::timestampFromROS(stamp)), lastPose_, odomFrameId, - rotVariance_>0?rotVariance_:1.0, - transVariance_>0?transVariance_:1.0); + uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, + uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0); rotVariance_ = 0; transVariance_ = 0; } @@ -1067,8 +1067,8 @@ void CoreWrapper::commonStereoCallback( rtabmap_ros::timestampFromROS(stamp)), lastPose_, odomFrameId, - rotVariance_>0?rotVariance_:1.0, - transVariance_>0?transVariance_:1.0); + uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, + uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0); rotVariance_ = 0; transVariance_ = 0; From 9979421b935ee50abafff3a243b599d01ec8656a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 26 Nov 2015 13:16:56 -0500 Subject: [PATCH 023/119] Updated OdometryROS with rtabmap library updates --- src/OdometryROS.cpp | 39 +++------------------------------------ src/OdometryROS.h | 1 - 2 files changed, 3 insertions(+), 37 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 7b69e3c7..52154181 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -124,14 +124,14 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : //parameters - parameters_ = this->getDefaultOdometryParameters(stereo); + parameters_ = Parameters::getDefaultOdometryParameters(stereo); if(!configPath.empty()) { if(UFile::exists(configPath.c_str())) { ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str()); rtabmap::ParametersMap allParameters; - Rtabmap::readParameters(configPath.c_str(), allParameters); + Parameters::readINI(configPath.c_str(), allParameters); // only update odometry parameters for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { @@ -251,46 +251,13 @@ OdometryROS::~OdometryROS() delete odometry_; } -rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) -{ - rtabmap::ParametersMap odomParameters; - rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter) - { - std::string group = uSplit(iter->first, '/').front(); - if(uStrContains(group, "Odom") || - (stereo && group.compare("Stereo") == 0) || - group.compare("SURF") == 0 || - group.compare("SIFT") == 0 || - group.compare("ORB") == 0 || - group.compare("FAST") == 0 || - group.compare("FREAK") == 0 || - group.compare("BRIEF") == 0 || - group.compare("GFTT") == 0 || - group.compare("BRISK") == 0 || - group.compare("Reg") == 0 || - group.compare("Vis") == 0) - { - if(stereo) - { - if(iter->first.compare(Parameters::kVisEstimationType()) == 0) - { - iter->second = "1"; // 3D->2D (PNP) - } - } - odomParameters.insert(*iter); - } - } - return odomParameters; -} - void OdometryROS::processArguments(int argc, char * argv[], bool stereo) { for(int i=1;ifirst + " = \"" + iter->second + "\""; diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 0e95542b..2813ff5b 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -49,7 +49,6 @@ namespace rtabmap_ros { class OdometryROS { public: - static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false); static void processArguments(int argc, char * argv[], bool stereo = false); public: From 82e9e9c80604a0829b4ffeb89a103bfb94afb5d5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 25 Nov 2015 12:16:59 -0500 Subject: [PATCH 024/119] Added parameters backward compatibility approach from 0.11.0 --- CMakeLists.txt | 2 +- src/CoreWrapper.cpp | 91 ++++++++++----------------------------------- src/OdometryROS.cpp | 74 +++++++++++++----------------------- 3 files changed, 46 insertions(+), 121 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 60878203..3bc31f84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(octomap_ros) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.10.12 REQUIRED) +find_package(RTABMap 0.11.0 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 94ba6404..1b13d3f4 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -258,83 +258,32 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("LccReextract/LoopClosureFeatures"); - oldParameterNames.push_back("Rtabmap/DetectorStrategy"); - oldParameterNames.push_back("RGBD/ScanMatchingSize"); - oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius"); - oldParameterNames.push_back("RGBD/ToroIterations"); - oldParameterNames.push_back("Mem/RehearsedNodesKept"); - oldParameterNames.push_back("Odom/PnPEstimation"); - oldParameterNames.push_back("LccBow/MaxDepth"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("GFTT/MaxCorners") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: GFTT/MaxCorners -> %s. Please update your launch file accordingly.", - Parameters::kKpWordsPerImage().c_str()); + // can be migrated + parameters_.at(iter->second.second)= vStr; + ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } - else if(iter->compare("LccBow/MaxDepth") == 0) + else { - ROS_WARN("Parameter name changed: LccBow/MaxDepth -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractMaxDepth().c_str()); - parameters_.at(Parameters::kLccReextractMaxDepth())= vStr; - } - else if(iter->compare("LccReextract/LoopClosureFeatures") == 0) - { - ROS_WARN("Parameter name changed: LccReextract/LoopClosureFeatures -> %s. Please update your launch file accordingly.", - Parameters::kLccReextractActivated().c_str()); - parameters_.at(Parameters::kLccReextractActivated())= vStr; - } - else if(iter->compare("Rtabmap/DetectorStrategy") == 0) - { - ROS_WARN("Parameter name changed: Rtabmap/DetectorStrategy -> %s. Please update your launch file accordingly.", - Parameters::kKpDetectorStrategy().c_str()); - parameters_.at(Parameters::kKpDetectorStrategy())= vStr; - } - else if(iter->compare("RGBD/ScanMatchingSize") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ScanMatchingSize -> %s. Please update your launch file accordingly.", - Parameters::kRGBDPoseScanMatching().c_str()); - parameters_.at(Parameters::kRGBDPoseScanMatching())= std::atoi(vStr.c_str()) > 0?"true":"false"; - } - else if(iter->compare("RGBD/LocalLoopDetectionRadius") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionRadius -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalRadius().c_str()); - parameters_.at(Parameters::kRGBDLocalRadius())= vStr; - } - else if(iter->compare("RGBD/ToroIterations") == 0) - { - ROS_WARN("Parameter name changed: RGBD/ToroIterations -> %s. Please update your launch file accordingly.", - Parameters::kRGBDOptimizeIterations().c_str()); - parameters_.at(Parameters::kRGBDOptimizeIterations())= vStr; - } - else if(iter->compare("Mem/RehearsedNodesKept") == 0) - { - ROS_WARN("Parameter name changed: Mem/RehearsedNodesKept -> %s. Please update your launch file accordingly.", - Parameters::kMemNotLinkedNodesKept().c_str()); - parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr; - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionMaxDiffID -> %s. Please update your launch file accordingly.", - Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str()); - parameters_.at(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth())= vStr; - } - else if(iter->compare("RGBD/PlanVirtualLinksMaxDiffID") == 0) - { - ROS_WARN("Parameter \"RGBD/PlanVirtualLinksMaxDiffID\" doesn't exist anymore."); - } - else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0) - { - ROS_WARN("Parameter name changed: Odom/PnPEstimation -> %s. Please update your launch file accordingly.", - Parameters::kOdomEstimationType().c_str()); - parameters_.at(Parameters::kOdomEstimationType())= uNumber2Str(1); + if(iter->second.second.empty()) + { + ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 86601451..7b69e3c7 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -174,7 +174,7 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : iter->second = uNumber2Str(vInt); } - if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) + if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) { ROS_WARN("Parameter min_inliers must be >= 8, setting to 8..."); iter->second = uNumber2Str(8); @@ -182,54 +182,32 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } // Backward compatibility - std::list oldParameterNames; - oldParameterNames.push_back("Odom/Type"); - oldParameterNames.push_back("Odom/MaxWords"); - oldParameterNames.push_back("Odom/WordsRatio"); - oldParameterNames.push_back("Odom/LocalHistory"); - oldParameterNames.push_back("Odom/NearestNeighbor"); - oldParameterNames.push_back("Odom/NNDR"); - oldParameterNames.push_back("GFTT/MaxCorners"); - for(std::list::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) { std::string vStr; - if(pnh.getParam(*iter, vStr)) + if(pnh.getParam(iter->first, vStr)) { - if(iter->compare("Odom/Type") == 0) + if(iter->second.first) { - ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.", - Parameters::kOdomFeatureType().c_str()); - parameters_.at(Parameters::kOdomFeatureType())= vStr; + // can be migrated + parameters_.at(iter->second.second)= vStr; + ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } - else if(iter->compare("Odom/MaxWords") == 0) + else { - ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; - } - else if(iter->compare("Odom/LocalHistory") == 0) - { - ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowLocalHistorySize().c_str()); - parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr; - } - else if(iter->compare("Odom/NearestNeighbor") == 0) - { - ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNType().c_str()); - parameters_.at(Parameters::kOdomBowNNType())= vStr; - } - else if(iter->compare("Odom/NNDR") == 0) - { - ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.", - Parameters::kOdomBowNNDR().c_str()); - parameters_.at(Parameters::kOdomBowNNDR())= vStr; - } - else if(iter->compare("GFTT/MaxCorners") == 0) - { - ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.", - Parameters::kOdomMaxFeatures().c_str()); - parameters_.at(Parameters::kOdomMaxFeatures())= vStr; + if(iter->second.second.empty()) + { + ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore!", + iter->first.c_str()); + } + else + { + ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + iter->first.c_str(), iter->second.second.c_str()); + } } } } @@ -289,15 +267,13 @@ rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) group.compare("FREAK") == 0 || group.compare("BRIEF") == 0 || group.compare("GFTT") == 0 || - group.compare("BRISK") == 0) + group.compare("BRISK") == 0 || + group.compare("Reg") == 0 || + group.compare("Vis") == 0) { if(stereo) { - if(iter->first.compare(Parameters::kOdomMaxDepth()) == 0) - { - iter->second = "0"; // infinity - } - else if(iter->first.compare(Parameters::kOdomEstimationType()) == 0) + if(iter->first.compare(Parameters::kVisEstimationType()) == 0) { iter->second = "1"; // 3D->2D (PNP) } From 50580d59e1cf3f5308d635d9f2042ea578e5ed70 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 26 Nov 2015 13:16:56 -0500 Subject: [PATCH 025/119] Updated OdometryROS with rtabmap library updates --- src/OdometryROS.cpp | 39 +++------------------------------------ src/OdometryROS.h | 1 - 2 files changed, 3 insertions(+), 37 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 7b69e3c7..52154181 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -124,14 +124,14 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : //parameters - parameters_ = this->getDefaultOdometryParameters(stereo); + parameters_ = Parameters::getDefaultOdometryParameters(stereo); if(!configPath.empty()) { if(UFile::exists(configPath.c_str())) { ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str()); rtabmap::ParametersMap allParameters; - Rtabmap::readParameters(configPath.c_str(), allParameters); + Parameters::readINI(configPath.c_str(), allParameters); // only update odometry parameters for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { @@ -251,46 +251,13 @@ OdometryROS::~OdometryROS() delete odometry_; } -rtabmap::ParametersMap OdometryROS::getDefaultOdometryParameters(bool stereo) -{ - rtabmap::ParametersMap odomParameters; - rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter) - { - std::string group = uSplit(iter->first, '/').front(); - if(uStrContains(group, "Odom") || - (stereo && group.compare("Stereo") == 0) || - group.compare("SURF") == 0 || - group.compare("SIFT") == 0 || - group.compare("ORB") == 0 || - group.compare("FAST") == 0 || - group.compare("FREAK") == 0 || - group.compare("BRIEF") == 0 || - group.compare("GFTT") == 0 || - group.compare("BRISK") == 0 || - group.compare("Reg") == 0 || - group.compare("Vis") == 0) - { - if(stereo) - { - if(iter->first.compare(Parameters::kVisEstimationType()) == 0) - { - iter->second = "1"; // 3D->2D (PNP) - } - } - odomParameters.insert(*iter); - } - } - return odomParameters; -} - void OdometryROS::processArguments(int argc, char * argv[], bool stereo) { for(int i=1;ifirst + " = \"" + iter->second + "\""; diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 0e95542b..2813ff5b 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -49,7 +49,6 @@ namespace rtabmap_ros { class OdometryROS { public: - static rtabmap::ParametersMap getDefaultOdometryParameters(bool stereo = false); static void processArguments(int argc, char * argv[], bool stereo = false); public: From 27d40dee2b96e36ee34595ff7ef8718889ddd8f2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 26 Nov 2015 16:06:42 -0500 Subject: [PATCH 026/119] Added "scan_cloud" topic (PointCloud2 type) with "subscribe_scan_cloud" parameter to rtabmap and rtabmapviz nodes. --- src/CoreWrapper.cpp | 273 ++++++++++++++++++++++++++++++++++++-------- src/CoreWrapper.h | 69 ++++++++++- src/GuiWrapper.cpp | 260 +++++++++++++++++++++++++++++++++++------ src/GuiWrapper.h | 69 ++++++++++- src/MapsManager.cpp | 28 +++-- src/MapsManager.h | 1 + 6 files changed, 603 insertions(+), 97 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 1b13d3f4..ede6bd81 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -109,7 +109,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : ros::NodeHandle nh; ros::NodeHandle pnh("~"); - bool subscribeLaserScan = false; + bool subscribeScan2d = false; + bool subscribeScan3d = false; bool subscribeDepth = true; bool subscribeStereo = false; int depthCameras = 1; @@ -121,14 +122,24 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : // ROS related parameters (private) pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan); + if(pnh.getParam("subscribe_laserScan", subscribeScan2d)) + { + ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); + } + pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d); + pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeDepth && subscribeStereo) { ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false."); subscribeDepth = false; } - if(subscribeLaserScan) + if(subscribeScan2d && subscribeScan3d) + { + ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false."); + subscribeDepth = false; + } + if(subscribeScan2d || subscribeScan3d) { if(!subscribeDepth && !subscribeStereo) { @@ -369,7 +380,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); - setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras); + setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations); @@ -453,7 +464,7 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile) { ROS_WARN("Config file doesn't exist! It will be generated..."); } - Rtabmap::readParameters(configFile.c_str(), parameters); + Parameters::readINI(configFile.c_str(), parameters); } // otherwise take default parameters @@ -470,7 +481,7 @@ void CoreWrapper::saveParameters(const std::string & configFile) { printf("Config file doesn't exist, a new one will be created.\n"); } - Rtabmap::writeParameters(configFile.c_str(), parameters_); + Parameters::writeINI(configFile.c_str(), parameters_); } else { @@ -670,7 +681,8 @@ void CoreWrapper::commonDepthCallback( const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { std::vector imageMsgs; std::vector depthMsgs; @@ -678,14 +690,15 @@ void CoreWrapper::commonDepthCallback( imageMsgs.push_back(imageMsg); depthMsgs.push_back(depthMsg); cameraInfoMsgs.push_back(cameraInfoMsg); - commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg); + commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg); } void CoreWrapper::commonDepthCallback( const std::string & odomFrameId, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { UASSERT(imageMsgs.size()>0 && imageMsgs.size() == depthMsgs.size() && @@ -704,7 +717,7 @@ void CoreWrapper::commonDepthCallback( int cameraCount = imageMsgs.size(); cv::Mat rgb; cv::Mat depth; - pcl::PointCloud scanCloud; + pcl::PointCloud scanCloud2d; std::vector cameraModels; int genMaxScanPts = 0; for(unsigned int i=0; iheader.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -839,16 +852,16 @@ void CoreWrapper::commonDepthCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(lastPoseStamp_ != scanMsg->header.stamp) + if(lastPoseStamp_ != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -858,19 +871,27 @@ void CoreWrapper::commonDepthCallback( } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } - else if(scanCloud.size()) + else if(scanCloud2d.size()) { - scan = util3d::laserScanFromPointCloud(scanCloud); + scan = util3d::laserScan2dFromPointCloud(scanCloud2d); } - ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:depthMsgs[0]->header.stamp; + ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: + scan3dMsg.get() != 0?scan3dMsg->header.stamp: + depthMsgs[0]->header.stamp; process(stamp, SensorData(scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():genMaxScanPts, - scanMsg.get() != 0?scanMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts, + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), rgb, depth, cameraModels, @@ -890,7 +911,8 @@ void CoreWrapper::commonStereoCallback( const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg) + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg) { if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 || @@ -934,10 +956,10 @@ void CoreWrapper::commonStereoCallback( } cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -946,16 +968,16 @@ void CoreWrapper::commonStereoCallback( sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; //projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_); - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(lastPoseStamp_ != scanMsg->header.stamp) + if(lastPoseStamp_ != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -966,6 +988,12 @@ void CoreWrapper::commonStereoCallback( } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } @@ -1004,11 +1032,13 @@ void CoreWrapper::commonStereoCallback( } } - ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp; + ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: + scan3dMsg.get() != 0?scan3dMsg->header.stamp: + leftImageMsg->header.stamp; process(stamp, SensorData(scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():0, - scanMsg.get() != 0?scanMsg->range_max:0, + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, ptrLeftImage->image, ptrRightImage->image, stereoModel, @@ -1035,7 +1065,8 @@ void CoreWrapper::depthCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::depthScanCallback( const sensor_msgs::ImageConstPtr& imageMsg, @@ -1048,7 +1079,22 @@ void CoreWrapper::depthScanCallback( { return; } - commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::depthScan3dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomUpdate(odomMsg)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::stereoCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1063,7 +1109,8 @@ void CoreWrapper::stereoCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::stereoScanCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1077,7 +1124,23 @@ void CoreWrapper::stereoScanCallback( { return; } - commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::stereoScan3dCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg) +{ + if(!commonOdomUpdate(odomMsg)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::depth2Callback( @@ -1105,7 +1168,8 @@ void CoreWrapper::depth2Callback( cameraInfoMsgs.push_back(cameraInfo2Msg); sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg); } @@ -1119,7 +1183,8 @@ void CoreWrapper::depthTFCallback( return; } sensor_msgs::LaserScanConstPtr scanMsg; // Null - commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::depthScanTFCallback( const sensor_msgs::ImageConstPtr& imageMsg, @@ -1131,7 +1196,21 @@ void CoreWrapper::depthScanTFCallback( { return; } - commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg); +} +void CoreWrapper::depthScan3dTFCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomTFUpdate(scanMsg->header.stamp)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // Null + commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::stereoTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1145,7 +1224,8 @@ void CoreWrapper::stereoTFCallback( } sensor_msgs::LaserScanConstPtr scanMsg; // null - commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); } void CoreWrapper::stereoScanTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -1158,7 +1238,23 @@ void CoreWrapper::stereoScanTFCallback( { return; } - commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg); + sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg); +} + +void CoreWrapper::stereoScan3dTFCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg) +{ + if(!commonOdomTFUpdate(leftImageMsg->header.stamp)) + { + return; + } + sensor_msgs::LaserScanConstPtr scan2dMsg; // null + commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg); } void CoreWrapper::process( @@ -2311,7 +2407,8 @@ bool CoreWrapper::octomapFullCallback( */ void CoreWrapper::setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeScan2d, + bool subscribeScan3d, bool subscribeStereo, int queueSize, bool stereoApproxSync, @@ -2323,7 +2420,7 @@ void CoreWrapper::setupCallbacks( if(subscribeDepth) { UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!"); + UASSERT_MSG(depthCameras == 1 || !(subscribeScan2d || subscribeScan3d || !odomFrameId_.empty()), "Not yet supported!"); imageSubs_.resize(depthCameras); imageDepthSubs_.resize(depthCameras); @@ -2357,7 +2454,7 @@ void CoreWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeScan2d) { ROS_INFO("Registering Depth+LaserScan callback..."); scanSub_.subscribe(nh, "scan", 1); @@ -2378,6 +2475,27 @@ void CoreWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + ROS_INFO("Registering Depth+LaserScan3d callback..."); + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + *imageSubs_[0], + odomSub_, + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(depthCameras > 1) @@ -2427,7 +2545,7 @@ void CoreWrapper::setupCallbacks( else { // use odom from TF, so subscribe to sensors only - if(subscribeLaserScan) + if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); depthScanTFSync_ = new message_filters::Synchronizer( @@ -2445,6 +2563,24 @@ void CoreWrapper::setupCallbacks( cameraInfoSubs_[0]->getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dTFSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFSyncPolicy(queueSize), + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0], + scan3dSub_); + depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { depthTFSync_ = new message_filters::Synchronizer( @@ -2481,7 +2617,7 @@ void CoreWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeScan2d) { scanSub_.subscribe(nh, "scan", 1); stereoScanSync_ = new message_filters::Synchronizer( @@ -2503,6 +2639,28 @@ void CoreWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dSync_ = new message_filters::Synchronizer( + MyStereoScan3dSyncPolicy(queueSize), + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_, + scan3dSub_, + odomSub_); + stereoScan3dSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(stereoApproxSync) @@ -2542,9 +2700,9 @@ void CoreWrapper::setupCallbacks( else { // use odom from TF, so subscribe to sensors only - if(subscribeLaserScan) + if(subscribeScan2d) { - ROS_INFO("Registering Stereo+LaserScan+OdomTF callback..."); + ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback..."); scanSub_.subscribe(nh, "scan", 1); stereoScanTFSync_ = new message_filters::Synchronizer( MyStereoScanTFSyncPolicy(queueSize), @@ -2563,6 +2721,27 @@ void CoreWrapper::setupCallbacks( cameraInfoRight_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeScan3d) + { + ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback..."); + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dTFSync_ = new message_filters::Synchronizer( + MyStereoScan3dTFSyncPolicy(queueSize), + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_, + scan3dSub_); + stereoScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else //!subscribeLaserScan { if(stereoApproxSync) diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index a5b861d9..03924b99 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -86,7 +86,8 @@ public: private: void setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeScan2d, + bool subscribeScan3d, bool subscribeStereo, int queueSize, bool stereoApproxSync, @@ -102,20 +103,23 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); void commonDepthCallback( const std::string & odomFrameId, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); void commonStereoCallback( const std::string & odomFrameId, const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg); + const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg); // with odom msg void depthCallback( @@ -129,6 +133,12 @@ private: const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void depthScan3dCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); void stereoCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, @@ -142,6 +152,13 @@ private: const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg); + void stereoScan3dCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg); void depth2Callback( const nav_msgs::OdometryConstPtr & odomMsg, const sensor_msgs::ImageConstPtr& image1Msg, @@ -161,6 +178,11 @@ private: const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void depthScan3dTFCallback( + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); void stereoTFCallback( const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg, @@ -172,6 +194,12 @@ private: const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr& scanMsg); + void stereoScan3dTFCallback( + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg); void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0); void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg); @@ -280,6 +308,7 @@ private: message_filters::Subscriber odomSub_; message_filters::Subscriber scanSub_; + message_filters::Subscriber scan3dSub_; typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, @@ -289,6 +318,14 @@ private: sensor_msgs::LaserScan> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy; + message_filters::Synchronizer * depthScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, nav_msgs::Odometry, @@ -305,6 +342,15 @@ private: nav_msgs::Odometry> MyStereoScanSyncPolicy; message_filters::Synchronizer * stereoScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry> MyStereoScan3dSyncPolicy; + message_filters::Synchronizer * stereoScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -339,6 +385,13 @@ private: sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy; message_filters::Synchronizer * depthScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy; + message_filters::Synchronizer * depthScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -353,6 +406,14 @@ private: sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy; message_filters::Synchronizer * stereoScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo, + sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy; + message_filters::Synchronizer * stereoScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 040e9f03..93e40d38 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -119,7 +119,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : ros::NodeHandle pnh("~"); // To receive odometry events - bool subscribeLaserScan = false; + bool subscribeLaserScan2d = false; + bool subscribeLaserScan3d = false; bool subscribeDepth = false; bool subscribeOdomInfo = false; bool subscribeStereo = false; @@ -130,7 +131,12 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : pnh.param("frame_id", frameId_, frameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan); + if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d)) + { + ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); + } + pnh.param("subscribe_scan", subscribeLaserScan2d, subscribeLaserScan2d); + pnh.param("subscribe_scan_cloud", subscribeLaserScan3d, subscribeLaserScan3d); pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); pnh.param("depth_cameras", depthCameras, depthCameras); @@ -181,7 +187,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : this->setupCallbacks( subscribeDepth, - subscribeLaserScan, + subscribeLaserScan2d, + subscribeLaserScan3d, subscribeOdomInfo, subscribeStereo, queueSize, @@ -510,7 +517,8 @@ void GuiWrapper::commonDepthCallback( const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { std::vector imageMsgs; @@ -519,7 +527,7 @@ void GuiWrapper::commonDepthCallback( imageMsgs.push_back(imageMsg); depthMsgs.push_back(depthMsg); cameraInfoMsgs.push_back(cameraInfoMsg); - commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg); + commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg); } void GuiWrapper::commonDepthCallback( @@ -527,7 +535,8 @@ void GuiWrapper::commonDepthCallback( const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 && @@ -547,9 +556,13 @@ void GuiWrapper::commonDepthCallback( } else { - if(scanMsg.get()) + if(scan2dMsg.get()) { - odomHeader = scanMsg->header; + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; } else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get()) { @@ -691,10 +704,10 @@ void GuiWrapper::commonDepthCallback( } cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -702,16 +715,16 @@ void GuiWrapper::commonDepthCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(odomHeader.stamp != scanMsg->header.stamp) + if(odomHeader.stamp != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -723,6 +736,12 @@ void GuiWrapper::commonDepthCallback( } scan = util3d::laserScanFromPointCloud(*pclScan); } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + scan = util3d::laserScanFromPointCloud(*pclScan); + } rtabmap::OdometryInfo info; if(odomInfoMsg.get()) @@ -733,8 +752,8 @@ void GuiWrapper::commonDepthCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scanMsg.get()?(int)scanMsg->ranges.size():0, - scanMsg.get()?(int)scanMsg->range_max:0, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, rgb, depth, cameraModels, @@ -754,7 +773,8 @@ void GuiWrapper::commonStereoCallback( const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) { // limit 10 Hz max @@ -787,9 +807,13 @@ void GuiWrapper::commonStereoCallback( } else { - if(scanMsg.get()) + if(scan2dMsg.get()) { - odomHeader = scanMsg->header; + odomHeader = scan2dMsg->header; + } + else if(scan3dMsg.get()) + { + odomHeader = scan3dMsg->header; } else { @@ -878,10 +902,10 @@ void GuiWrapper::commonStereoCallback( cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image; cv::Mat scan; - if(scanMsg.get() != 0) + if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) { return; } @@ -889,16 +913,16 @@ void GuiWrapper::commonStereoCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); // sync with odometry stamp - if(odomHeader.stamp != scanMsg->header.stamp) + if(odomHeader.stamp != scan2dMsg->header.stamp) { if(!odomT.isNull()) { - Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scanMsg->header.stamp); + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan2dMsg->header.stamp); if(sensorT.isNull()) { return; @@ -908,6 +932,12 @@ void GuiWrapper::commonStereoCallback( } } + scan = util3d::laserScan2dFromPointCloud(*pclScan); + } + else if(scan3dMsg.get() != 0) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); scan = util3d::laserScanFromPointCloud(*pclScan); } @@ -920,8 +950,8 @@ void GuiWrapper::commonStereoCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scanMsg.get()?(int)scanMsg->ranges.size():0, - scanMsg.get()?(int)scanMsg->range_max:0, + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, left, right, stereoModel, @@ -944,6 +974,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg) sensor_msgs::ImageConstPtr(), sensor_msgs::CameraInfoConstPtr(), sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -959,6 +990,7 @@ void GuiWrapper::depthCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -987,6 +1019,7 @@ void GuiWrapper::depth2Callback( depthMsgs, cameraInfoMsgs, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1003,6 +1036,7 @@ void GuiWrapper::depthOdomInfoCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1032,6 +1066,7 @@ void GuiWrapper::depthOdomInfo2Callback( depthMsgs, cameraInfoMsgs, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1048,6 +1083,24 @@ void GuiWrapper::depthScanCallback( depthMsg, cameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::depthScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1066,6 +1119,26 @@ void GuiWrapper::stereoScanCallback( leftCameraInfoMsg, rightCameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::stereoScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1084,6 +1157,7 @@ void GuiWrapper::stereoOdomInfoCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1101,6 +1175,7 @@ void GuiWrapper::stereoCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1116,6 +1191,7 @@ void GuiWrapper::depthTFCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } @@ -1131,6 +1207,7 @@ void GuiWrapper::depthOdomInfoTFCallback( depthMsg, cameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1146,6 +1223,23 @@ void GuiWrapper::depthScanTFCallback( depthMsg, cameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::depthScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + nav_msgs::OdometryConstPtr(), + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1163,6 +1257,25 @@ void GuiWrapper::stereoScanTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + rtabmap_ros::OdomInfoConstPtr()); +} + +void GuiWrapper::stereoScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + nav_msgs::OdometryConstPtr(), + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, rtabmap_ros::OdomInfoConstPtr()); } @@ -1180,6 +1293,7 @@ void GuiWrapper::stereoOdomInfoTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), odomInfoMsg); } @@ -1196,12 +1310,14 @@ void GuiWrapper::stereoTFCallback( leftCameraInfoMsg, rightCameraInfoMsg, sensor_msgs::LaserScanConstPtr(), + sensor_msgs::PointCloud2ConstPtr(), rtabmap_ros::OdomInfoConstPtr()); } void GuiWrapper::setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeLaserScan2d, + bool subscribeLaserScan3d, bool subscribeOdomInfo, bool subscribeStereo, int queueSize, @@ -1216,7 +1332,7 @@ void GuiWrapper::setupCallbacks( "same time. Parameter subscribe_depth is set to false."); subscribeDepth = false; } - if(!subscribeDepth && !subscribeStereo && subscribeLaserScan) + if(!subscribeDepth && !subscribeStereo && (subscribeLaserScan2d || subscribeLaserScan3d)) { ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription..."); } @@ -1233,7 +1349,7 @@ void GuiWrapper::setupCallbacks( if(subscribeDepth) { UASSERT(depthCameras >= 1 && depthCameras <= 2); - UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!"); + UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan2d || subscribeLaserScan3d || !odomFrameId_.empty()), "Not yet supported!"); imageSubs_.resize(depthCameras); imageDepthSubs_.resize(depthCameras); @@ -1267,7 +1383,7 @@ void GuiWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); depthScanSync_ = new message_filters::Synchronizer( @@ -1287,6 +1403,26 @@ void GuiWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); @@ -1382,7 +1518,7 @@ void GuiWrapper::setupCallbacks( else { // use TF as odom - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); depthScanTFSync_ = new message_filters::Synchronizer( @@ -1400,6 +1536,24 @@ void GuiWrapper::setupCallbacks( cameraInfoSubs_[0]->getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + depthScan3dTFSync_ = new message_filters::Synchronizer( + MyDepthScan3dTFSyncPolicy(queueSize), + scan3dSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); @@ -1454,7 +1608,7 @@ void GuiWrapper::setupCallbacks( if(odomFrameId_.empty()) { odomSub_.subscribe(nh, "odom", 1); - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); stereoScanSync_ = new message_filters::Synchronizer( @@ -1476,6 +1630,28 @@ void GuiWrapper::setupCallbacks( odomSub_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dSync_ = new message_filters::Synchronizer( + MyStereoScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); @@ -1521,7 +1697,7 @@ void GuiWrapper::setupCallbacks( else { //use odom TF - if(subscribeLaserScan) + if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); stereoScanTFSync_ = new message_filters::Synchronizer( @@ -1541,6 +1717,26 @@ void GuiWrapper::setupCallbacks( cameraInfoRight_.getTopic().c_str(), scanSub_.getTopic().c_str()); } + else if(subscribeLaserScan3d) + { + scan3dSub_.subscribe(nh, "scan_cloud", 1); + stereoScan3dTFSync_ = new message_filters::Synchronizer( + MyStereoScan3dTFSyncPolicy(queueSize), + scan3dSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } else if(subscribeOdomInfo) { odomInfoSub_.subscribe(nh, "odom_info", 1); diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index bb93a46d..f1d1b761 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -80,7 +80,8 @@ private: void setupCallbacks( bool subscribeDepth, - bool subscribeLaserScan, + bool subscribeLaserScan2d, + bool subscribeLaserScan3d, bool subscribeOdomInfo, bool subscribeStereo, int queueSize, @@ -91,14 +92,16 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void commonDepthCallback( const nav_msgs::OdometryConstPtr & odomMsg, const std::vector & imageMsgs, const std::vector & depthMsgs, const std::vector & cameraInfoMsgs, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void commonStereoCallback( const nav_msgs::OdometryConstPtr & odomMsg, @@ -106,7 +109,8 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, - const sensor_msgs::LaserScanConstPtr& scanMsg, + const sensor_msgs::LaserScanConstPtr& scan2dMsg, + const sensor_msgs::PointCloud2ConstPtr& scan3dMsg, const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg); void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); @@ -146,6 +150,12 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); void stereoScanCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, @@ -154,6 +164,13 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoOdomInfoCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -182,6 +199,11 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs:: CameraInfoConstPtr& camInfoMsg); + void depthScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs:: CameraInfoConstPtr& camInfoMsg); void stereoScanTFCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, @@ -189,6 +211,12 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dTFCallback( + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoOdomInfoTFCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const sensor_msgs::ImageConstPtr& leftImageMsg, @@ -231,6 +259,7 @@ private: message_filters::Subscriber odomSub_; message_filters::Subscriber odomInfoSub_; message_filters::Subscriber scanSub_; + message_filters::Subscriber scan3dSub_; image_transport::SubscriberFilter imageRectLeft_; image_transport::SubscriberFilter imageRectRight_; @@ -256,6 +285,14 @@ private: sensor_msgs::CameraInfo> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy; + message_filters::Synchronizer * depthScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< nav_msgs::Odometry, sensor_msgs::Image, @@ -288,6 +325,15 @@ private: sensor_msgs::CameraInfo> MyStereoScanSyncPolicy; message_filters::Synchronizer * stereoScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy; + message_filters::Synchronizer * stereoScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< rtabmap_ros::OdomInfo, nav_msgs::Odometry, @@ -326,6 +372,13 @@ private: sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy; message_filters::Synchronizer * depthScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy; + message_filters::Synchronizer * depthScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, @@ -354,6 +407,14 @@ private: sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy; message_filters::Synchronizer * stereoScanTFSync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy; + message_filters::Synchronizer * stereoScan3dTFSync_; + typedef message_filters::sync_policies::ApproximateTime< rtabmap_ros::OdomInfo, sensor_msgs::Image, diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 8ef295fb..322f2bc6 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -66,6 +66,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_); // scan map stuff + pnh.param("scan_decimation", scanDecimation_, scanDecimation_); pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_); @@ -344,21 +345,28 @@ std::map MapsManager::updateMapCaches( { if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0)) { - pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); - if(scanVoxelSize_ > 0.0) + if(scanDecimation_ > 1) { - scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); - if(gridRequired) + scan = util3d::downsample(scan, scanDecimation_); + } + if(scanRequired || scanVoxelSize_ > 0.0) + { + pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); + if(scanVoxelSize_ > 0.0) { - scan = util3d::laserScanFromPointCloud(*scanCloud); + scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); + if(gridRequired && scan.type() == CV_32FC2) + { + scan = util3d::laserScan2dFromPointCloud(*scanCloud); + } + } + if(scanRequired) + { + uInsert(scans_, std::make_pair(iter->first, scanCloud)); } } - if(scanRequired) - { - uInsert(scans_, std::make_pair(iter->first, scanCloud)); - } } - if(gridRequired) + if(gridRequired && scan.type() == CV_32FC2) { cv::Mat ground, obstacles; util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); diff --git a/src/MapsManager.h b/src/MapsManager.h index 2261b6ac..f495db7f 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -74,6 +74,7 @@ private: bool cloudFrustumCulling_; double cloudNoiseFilteringRadius_; int cloudNoiseFilteringMinNeighbors_; + int scanDecimation_; double scanVoxelSize_; bool scanOutputVoxelized_; double projMaxGroundAngle_; From b8307449bd2e3eb3dbefc7755763dc6381117099 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 27 Nov 2015 11:10:53 -0500 Subject: [PATCH 027/119] Updated for rtabmap library changes https://github.com/introlab/rtabmap/commit/7d35906398ba07c17c905d9bd263e0aa5eb5d2d8 --- src/CoreWrapper.cpp | 4 ++-- src/MapOptimizerNode.cpp | 17 +++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index ede6bd81..d9372dc5 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -383,7 +383,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras); int optimizeIterations = 0; - Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations); + Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); if(publishTf && optimizeIterations != 0) { transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay)); @@ -391,7 +391,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : else if(publishTf) { UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.", - Parameters::kRGBDOptimizeIterations().c_str(), mapFrameId_.c_str()); + Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str()); } } diff --git a/src/MapOptimizerNode.cpp b/src/MapOptimizerNode.cpp index 33740d04..ae18bfbd 100644 --- a/src/MapOptimizerNode.cpp +++ b/src/MapOptimizerNode.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_ros/MsgConversion.h" #include #include +#include #include #include #include @@ -79,13 +80,13 @@ public: UASSERT(iterations > 0); ParametersMap parameters; - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeStrategy(), uNumber2Str(strategy))); - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeEpsilon(), uNumber2Str(epsilon))); - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(iterations))); - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeRobust(), uBool2Str(robust))); - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeSlam2D(), uBool2Str(slam2d))); - parameters.insert(ParametersPair(Parameters::kRGBDOptimizeVarianceIgnored(), uBool2Str(ignoreVariance))); - optimizer_ = graph::Optimizer::create(parameters); + parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy))); + parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon))); + parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations))); + parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust))); + parameters.insert(ParametersPair(Parameters::kOptimizerSlam2D(), uBool2Str(slam2d))); + parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance))); + optimizer_ = Optimizer::create(parameters); double tfDelay = 0.05; // 20 Hz bool publishTf = true; @@ -317,7 +318,7 @@ private: std::string odomFrameId_; bool globalOptimization_; bool optimizeFromLastNode_; - graph::Optimizer * optimizer_; + Optimizer * optimizer_; rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; From 8eea06c32beeb254a540c0f4afc11b87291afe8b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 27 Nov 2015 11:54:18 -0500 Subject: [PATCH 028/119] Fixed variance conversion from double to float with large numbers (over float max but not inf) (ref: http://answers.ros.org/question/221550/problem-running-rtabmap_ros-against-a-bag-file/) --- src/CoreWrapper.cpp | 8 ++++---- src/CoreWrapper.h | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 94ba6404..f7459ceb 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -629,8 +629,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); - double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); + float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); + float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); if(uIsFinite(rotVariance) && rotVariance > rotVariance_) { rotVariance_ = rotVariance; @@ -1217,8 +1217,8 @@ void CoreWrapper::process( const SensorData & data, const Transform & odom, const std::string & odomFrameId, - double odomRotationalVariance, - double odomTransitionalVariance) + float odomRotationalVariance, + float odomTransitionalVariance) { UTimer timer; if(rtabmap_.isIDsGenerated() || data.id() > 0) diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index a5b861d9..f4839c53 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -183,8 +183,8 @@ private: const rtabmap::SensorData & data, const rtabmap::Transform & odom = rtabmap::Transform(), const std::string & odomFrameId = "", - double odomRotationalVariance = 1.0, - double odomTransitionalVariance = 1.0); + float odomRotationalVariance = 1.0, + float odomTransitionalVariance = 1.0); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); @@ -229,8 +229,8 @@ private: bool paused_; rtabmap::Transform lastPose_; ros::Time lastPoseStamp_; - double rotVariance_; - double transVariance_; + float rotVariance_; + float transVariance_; rtabmap::Transform currentMetricGoal_; bool latestNodeWasReached_; rtabmap::ParametersMap parameters_; From 736a909725317b92a6bb44c3c4a6eab583d00843 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 27 Nov 2015 14:11:11 -0500 Subject: [PATCH 029/119] Added "cloud_ceiling_culling_height" parameter to MapsManager and MapCloud rviz plugin --- src/MapsManager.cpp | 15 +++++++++++++-- src/MapsManager.h | 1 + src/rviz/MapCloudDisplay.cpp | 13 +++++++++++-- src/rviz/MapCloudDisplay.h | 1 + 4 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 8ef295fb..7c1e455a 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -34,6 +34,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : cloudMaxDepth_(4.0), // meters cloudVoxelSize_(0.05), // meters cloudFloorCullingHeight_(0.0), + cloudCeilingCullingHeight_(0.0), cloudOutputVoxelized_(false), cloudFrustumCulling_(false), cloudNoiseFilteringRadius_(0.0), @@ -60,6 +61,14 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); + pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_); + if(cloudFloorCullingHeight_ > 0 && + cloudCeilingCullingHeight_ > 0 && + cloudCeilingCullingHeight_ < cloudFloorCullingHeight_) + { + ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled)."); + cloudCeilingCullingHeight_ = 0; + } pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_); @@ -504,9 +513,11 @@ void MapsManager::publishMaps( } } - if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0) + if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) { - assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f); + assembledCloud = util3d::passThrough(assembledCloud, "z", + cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, + cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); } if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) diff --git a/src/MapsManager.h b/src/MapsManager.h index 2261b6ac..8a554fe5 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -70,6 +70,7 @@ private: double cloudMaxDepth_; double cloudVoxelSize_; double cloudFloorCullingHeight_; + double cloudCeilingCullingHeight_; bool cloudOutputVoxelized_; bool cloudFrustumCulling_; double cloudNoiseFilteringRadius_; diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 0bf52c0f..9171924c 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -156,6 +156,13 @@ MapCloudDisplay::MapCloudDisplay() cloud_filter_floor_height_->setMin( 0.0f ); cloud_filter_floor_height_->setMax( 999.0f ); + cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f, + "Filter the ceiling at the specified height set here " + "(only appropriate for 2D mapping).", + this, SLOT( updateCloudParameters() ), this ); + cloud_filter_ceiling_height_->setMin( 0.0f ); + cloud_filter_ceiling_height_->setMax( 999.0f ); + node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f, "(Disabled=0) Only keep one node in the specified radius.", this, SLOT( updateCloudParameters() ), this ); @@ -276,9 +283,11 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) if(cloud->size()) { - if(cloud_filter_floor_height_->getFloat() > 0.0f) + if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f) { - cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f); + cloud = rtabmap::util3d::passThrough(cloud, "z", + cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f, + cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f); } sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index db289a23..688ded8f 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -108,6 +108,7 @@ public: rviz::FloatProperty* cloud_max_depth_; rviz::FloatProperty* cloud_voxel_size_; rviz::FloatProperty* cloud_filter_floor_height_; + rviz::FloatProperty* cloud_filter_ceiling_height_; rviz::FloatProperty* node_filtering_radius_; rviz::FloatProperty* node_filtering_angle_; rviz::BoolProperty* download_map_; From a2f7275267fecc02e76e0452b957ee3dc97c7e7a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 27 Nov 2015 14:58:23 -0500 Subject: [PATCH 030/119] Fixed data_recorder.launch FATAL error on label which already exists --- launch/data_recorder.launch | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index c52f0a90..310e6f17 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -35,10 +35,13 @@ - + + - + + + From 7def30380734ba8c40bc3f272d4a4ddeb6983f52 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 27 Nov 2015 18:14:13 -0500 Subject: [PATCH 031/119] Updated az3_mapping_robot_nav.launch move_base config --- launch/azimut3/az3_mapping_robot_nav.launch | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch index c1dec6a7..01c8dc9d 100644 --- a/launch/azimut3/az3_mapping_robot_nav.launch +++ b/launch/azimut3/az3_mapping_robot_nav.launch @@ -45,6 +45,8 @@ + + @@ -79,7 +81,7 @@ - + @@ -87,9 +89,9 @@ - - - + + + @@ -109,7 +111,7 @@ - + From 085bd6cf368049554b365c2c677726113dfe162d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 1 Dec 2015 10:54:30 -0500 Subject: [PATCH 032/119] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 149e6479..6ae8c454 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ source ~/catkin_ws/devel/setup.bash 0. Optional dependencies * If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3. - * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster. + * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev`). ```bash $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge ``` From fd74706fbd6cf8a53f06d1fbd61aecc40124bb7e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 1 Dec 2015 11:00:26 -0500 Subject: [PATCH 033/119] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6ae8c454..ccd5ad73 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ source ~/catkin_ws/devel/setup.bash 0. Optional dependencies * If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3. - * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev`). + * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`). ```bash $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge ``` From 97b7b0ccf4c426c79658252beba3e9e54b30cb04 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Dec 2015 10:48:37 -0500 Subject: [PATCH 034/119] Added twist values to Odometry messages --- src/OdometryROS.cpp | 17 +++++++++++++++++ src/OdometryROS.h | 1 + 2 files changed, 18 insertions(+) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 86601451..7bfc8e5c 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -422,6 +422,21 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.covariance.at(28) = info.variance; // pp odom.pose.covariance.at(35) = info.variance; // yawyaw + //set velocity + if(previousStamp_.isValid()) + { + float dt = 1.0f/(stamp - previousStamp_).toSec(); + float x,y,z,roll,pitch,yaw; + odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + odom.twist.twist.linear.x = x*dt; + odom.twist.twist.linear.y = y*dt; + odom.twist.twist.linear.z = z*dt; + odom.twist.twist.angular.x = roll*dt; + odom.twist.twist.angular.y = pitch*dt; + odom.twist.twist.angular.z = yaw*dt; + } + previousStamp_ = stamp; + //publish the message odomPub_.publish(odom); } @@ -516,6 +531,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { ROS_INFO("visual_odometry: reset odom!"); odometry_->reset(); + previousStamp_ = ros::Time(); return true; } @@ -524,6 +540,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros: Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw); ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); odometry_->reset(pose); + previousStamp_ = ros::Time(); return true; } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 0e95542b..3f6c985e 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -102,6 +102,7 @@ private: tf::TransformListener tfListener_; bool paused_; + ros::Time previousStamp_; }; } From 8355431160b0cea867b9a0588f4dbadaacb1346d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Dec 2015 12:13:31 -0500 Subject: [PATCH 035/119] Fixed mapToOdom transform not set on getMap and publishMap services --- src/CoreWrapper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index f7459ceb..f8826201 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1678,7 +1678,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros: rtabmap_ros::mapDataToROS(poses, constraints, signatures, - Transform::getIdentity(), + mapToOdom_, res.data); res.data.header.stamp = ros::Time::now(); @@ -1821,7 +1821,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_ros::mapDataToROS(poses, constraints, signatures, - Transform::getIdentity(), + mapToOdom_, *msg); mapDataPub_.publish(msg); @@ -1835,7 +1835,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_ros::mapGraphToROS(poses, constraints, - Transform::getIdentity(), + mapToOdom_, *msg); mapGraphPub_.publish(msg); From 451fdd1ec8af9403683bb52ebbd268e29278b9f3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 4 Jan 2016 19:04:19 -0500 Subject: [PATCH 036/119] Fixed build with latest rtabmap 0.11 --- include/rtabmap_ros/MsgConversion.h | 10 +++++++ src/CameraNode.cpp | 2 +- src/CoreWrapper.cpp | 32 ++++++---------------- src/GuiWrapper.cpp | 26 +++--------------- src/MsgConversion.cpp | 41 ++++++++++++++++++++++++++--- src/OdometryROS.cpp | 36 ++++++++++++++----------- src/nodelets/point_cloud_xyz.cpp | 14 ++++------ src/nodelets/point_cloud_xyzrgb.cpp | 15 +++-------- 8 files changed, 89 insertions(+), 87 deletions(-) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 2ab3a009..c6c80b5c 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -40,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -83,6 +85,14 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg); std::vector points2fFromROS(const std::vector & msg); void points2fToROS(const std::vector & kpts, std::vector & msg); +rtabmap::CameraModel cameraModelFromROS( + const sensor_msgs::CameraInfo & camInfo, + const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); +rtabmap::StereoCameraModel stereoCameraModelFromROS( + const sensor_msgs::CameraInfo & leftCamInfo, + const sensor_msgs::CameraInfo & rightCamInfo, + const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); + void mapDataFromROS( const rtabmap_ros::MapData & msg, std::map & poses, diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index 98b67868..21e2dc3f 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -193,7 +193,7 @@ public: if(!path.empty() && UDirectory::exists(path)) { //images - camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate); + camera_ = new rtabmap::CameraImages(path, frameRate); } else if(!path.empty() && UFile::exists(path)) { diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index d9372dc5..f18ab590 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -54,7 +54,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #ifdef WITH_OCTOMAP #include @@ -817,23 +816,16 @@ void CoreWrapper::commonDepthCallback( return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); if(scan2dMsg.get() == 0 && genScan_) { scanCloud2d += util3d::laserScanFromDepthImage( subDepth, - model.fx(), - model.fy(), - model.cx(), - model.cy(), + cameraModels.back().fx(), + cameraModels.back().fy(), + cameraModels.back().cx(), + cameraModels.back().cy(), genScanMaxDepth_, localTransform); genMaxScanPts += subDepth.cols; @@ -1009,17 +1001,9 @@ void CoreWrapper::commonStereoCallback( } ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8"); - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg); - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -1027,7 +1011,7 @@ void CoreWrapper::commonStereoCallback( ROS_WARN("Detected baseline (%f m) is quite large! Is your " "right camera_info P(0,3) correctly set? Note that " "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - model.baseline()); + stereoModel.baseline()); shown = true; } } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 93e40d38..57b95675 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -40,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include - #include #include #include @@ -692,14 +689,7 @@ void GuiWrapper::commonDepthCallback( return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform)); } } @@ -862,17 +852,9 @@ void GuiWrapper::commonStereoCallback( } } - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg); - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -880,7 +862,7 @@ void GuiWrapper::commonStereoCallback( ROS_WARN("Detected baseline (%f m) is quite large! Is your " "right camera_info P(0,3) correctly set? Note that " "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - model.baseline()); + stereoModel.baseline()); shown = true; } } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 1b626454..86fa3f0f 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include namespace rtabmap_ros { @@ -293,6 +295,35 @@ void points2fToROS(const std::vector & kpts, std::vector & poses, @@ -384,7 +415,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) { //Features stuff... std::multimap words; - std::multimap words3D; + std::multimap words3D; pcl::PointCloud cloud; if(msg.wordPts.data.size() && msg.wordPts.data.size() == msg.wordIds.size()) @@ -398,7 +429,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) words.insert(std::make_pair(wordId, pt)); if(i< cloud.size()) { - words3D.insert(std::make_pair(wordId, cloud[i])); + words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z))); } } @@ -539,11 +570,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & pcl::PointCloud cloud; cloud.resize(signature.getWords3().size()); index = 0; - for(std::multimap::const_iterator jter=signature.getWords3().begin(); + for(std::multimap::const_iterator jter=signature.getWords3().begin(); jter!=signature.getWords3().end(); ++jter) { - cloud[index++] = jter->second; + cloud[index].x = jter->second.x; + cloud[index].y = jter->second.y; + cloud[index++].z = jter->second.z; } pcl::toROSMsg(cloud, msg.wordPts); } diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 52154181..99bf31fc 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -216,8 +216,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); if(odomStrategy == 1) { - ROS_INFO("Using OdometryOpticalFlow"); - odometry_ = new rtabmap::OdometryOpticalFlow(parameters_); + ROS_INFO("Using OdometryF2F"); + odometry_ = new rtabmap::OdometryF2F(parameters_); } else { @@ -369,13 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomPub_.publish(odom); } + // local map / reference frame if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) { - const std::map & map = ((OdometryBOW*)odometry_)->getLocalMap(); pcl::PointCloud cloud; - for(std::map::const_iterator iter=map.begin(); iter!=map.end(); ++iter) + const std::map & map = ((OdometryBOW*)odometry_)->getLocalMap(); + for(std::map::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { - cloud.push_back(iter->second); + cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z)); } sensor_msgs::PointCloud2 cloudMsg; pcl::toROSMsg(cloud, cloudMsg); @@ -391,13 +392,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature(); if(s) { - const std::multimap & words3 = s->getWords3(); + const std::multimap & words3 = s->getWords3(); pcl::PointCloud cloud; - for(std::multimap::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) + for(std::multimap::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) { // transform to odom frame - pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose); - cloud.push_back(pt); + cv::Point3f pt = util3d::transformPoint(iter->second, pose); + cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z)); } sensor_msgs::PointCloud2 cloudMsg; @@ -409,14 +410,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } else { - //Optical flow - const pcl::PointCloud::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D(); - if(cloud->size()) + //Frame to Frame + const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame(); + if(refFrame.getWords3().size()) { - pcl::PointCloud::Ptr cloudTransformed; - cloudTransformed = util3d::transformPointCloud(cloud, pose); + pcl::PointCloud cloud; + for(std::multimap::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter) + { + // transform to odom frame + cv::Point3f pt = util3d::transformPoint(iter->second, pose); + cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z)); + } sensor_msgs::PointCloud2 cloudMsg; - pcl::toROSMsg(*cloudTransformed, cloudMsg); + pcl::toROSMsg(cloud, cloudMsg); cloudMsg.header.stamp = stamp; // use corresponding time stamp to image cloudMsg.header.frame_id = odomFrameId_; odomLastFrame_.publish(cloudMsg); diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 6bed0638..082c3789 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include #include @@ -238,18 +240,12 @@ private: if(cloudPub_.getNumSubscribers()) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - float cx = model.cx(); - float cy = model.cy(); - pcl::PointCloud::Ptr pclCloud; + rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx(), leftModel.cy(), disparityMsg->T); pclCloud = rtabmap::util3d::cloudFromDisparity( disparity, - cx, - cy, - disparityMsg->f, - disparityMsg->T, + stereoModel, decimation_); processAndPublish(pclCloud, disparityMsg->header); diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index f58c9cfb..146068ef 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + #include #include #include @@ -228,22 +230,11 @@ private: } ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*camInfoLeft, *camInfoRight); - - float fx = model.left().fx(); - float cx = model.left().cx(); - float cy = model.left().cy(); - float baseline = model.baseline(); - pcl::PointCloud::Ptr pclCloud; pclCloud = rtabmap::util3d::cloudFromStereoImages( ptrLeftImage->image, ptrRightImage->image, - cx, - cy, - fx, - baseline, + rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), decimation_); processAndPublish(pclCloud, imageLeft->header); From d8a24687f47460d9f0b8c1294adc377d71bed8b9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 Jan 2016 11:03:49 -0500 Subject: [PATCH 037/119] Update CoreWrapper.cpp --- src/CoreWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index f18ab590..313ffea2 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -136,7 +136,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : if(subscribeScan2d && subscribeScan3d) { ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false."); - subscribeDepth = false; + subscribeScan3d = false; } if(subscribeScan2d || subscribeScan3d) { From ef724530a1045872f915bf2c7e2bc9825e700527 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 Jan 2016 16:44:53 -0500 Subject: [PATCH 038/119] Updated with new Odometry headers --- src/OdometryROS.cpp | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 99bf31fc..e324e655 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include +#include #include #include #include @@ -214,16 +215,7 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : int odomStrategy = 0; // BOW Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); - if(odomStrategy == 1) - { - ROS_INFO("Using OdometryF2F"); - odometry_ = new rtabmap::OdometryF2F(parameters_); - } - else - { - ROS_INFO("Using OdometryBOW"); - odometry_ = new rtabmap::OdometryBOW(parameters_); - } + odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { odometry_->reset(initialPose); @@ -370,10 +362,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } // local map / reference frame - if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) + if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) { pcl::PointCloud cloud; - const std::map & map = ((OdometryBOW*)odometry_)->getLocalMap(); + const std::map & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); for(std::map::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z)); @@ -387,9 +379,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(odomLastFrame_.getNumSubscribers()) { - if(dynamic_cast(odometry_)) + if(dynamic_cast(odometry_)) { - const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature(); + const rtabmap::Signature * s = ((OdometryLocalMap*)odometry_)->getMemory()->getLastWorkingSignature(); if(s) { const std::multimap & words3 = s->getWords3(); @@ -458,7 +450,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) bool OdometryROS::isOdometryBOW() const { - return dynamic_cast(odometry_) != 0; + return dynamic_cast(odometry_) != 0; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) From e877c543ac8fc8a485e7c76fe0627a1e69677084 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 7 Jan 2016 14:01:00 -0500 Subject: [PATCH 039/119] Fixed localMap (map->multimap) build error --- src/OdometryROS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index e324e655..e762303f 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -365,8 +365,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) { pcl::PointCloud cloud; - const std::map & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); - for(std::map::const_iterator iter=map.begin(); iter!=map.end(); ++iter) + const std::multimap & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); + for(std::multimap::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z)); } From 8dacddb1108a0fc00a240313b3d29d95e01a1902 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 12:39:23 -0500 Subject: [PATCH 040/119] Added groundTruthPose field to NodeData msg --- msg/NodeData.msg | 3 +++ src/CoreWrapper.cpp | 2 +- src/MapAssemblerNode.cpp | 2 +- src/MsgConversion.cpp | 6 +++++- 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/msg/NodeData.msg b/msg/NodeData.msg index de26a52d..2265fd71 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -8,6 +8,9 @@ string label # Pose from odometry not corrected geometry_msgs/Pose pose +# Ground truth (optional) +geometry_msgs/Pose groundTruthPose + # compressed image in /camera_link frame # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h" uint8[] image diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 313ffea2..2a333a9e 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1269,7 +1269,7 @@ void CoreWrapper::process( std::map tmpSignature; SensorData tmpData = data; tmpData.setId(-1); - tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, tmpData))); + tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom)); // Update maps diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 4cf3a406..3395376e 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -89,7 +89,7 @@ public: Signature tmpS = nodes_.at(poses.rbegin()->first); SensorData tmpData = tmpS.sensorData(); tmpData.setId(-1); - uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpData))); + uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpS.getGroundTruthPose(), tmpData))); poses.insert(std::make_pair(-1, poses.rbegin()->second)); } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 86fa3f0f..9a517525 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -486,6 +486,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) msg.stamp, msg.label, transformFromPoseMsg(msg.pose), + transformFromPoseMsg(msg.groundTruthPose), stereoModel.isValid()? rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), @@ -520,6 +521,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg.stamp = signature.getStamp(); msg.label = signature.getLabel(); transformToPoseMsg(signature.getPose(), msg.pose); + transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image); compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth); compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan); @@ -596,7 +598,8 @@ rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg) msg.weight, msg.stamp, msg.label, - transformFromPoseMsg(msg.pose)); + transformFromPoseMsg(msg.pose), + transformFromPoseMsg(msg.groundTruthPose)); return s; } void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) @@ -608,6 +611,7 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg.stamp = signature.getStamp(); msg.label = signature.getLabel(); transformToPoseMsg(signature.getPose(), msg.pose); + transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); } rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) From 76eb994d0c31b8e3843c1cff5ff97ae96658fadc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 14:06:35 -0500 Subject: [PATCH 041/119] Updated warnings for parameters that are deprecated --- src/CoreWrapper.cpp | 6 +++--- src/GuiWrapper.cpp | 2 +- src/OdometryROS.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 2a333a9e..35a81b50 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -121,7 +121,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : // ROS related parameters (private) pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - if(pnh.getParam("subscribe_laserScan", subscribeScan2d)) + if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d) { ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); } @@ -286,12 +286,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : { if(iter->second.second.empty()) { - ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore!", + ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { - ROS_WARN("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 57b95675..d8427bf2 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -128,7 +128,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : pnh.param("frame_id", frameId_, frameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); - if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d)) + if(pnh.getParam("subscribe_laserScan", subscribeLaserScan2d) && subscribeLaserScan2d) { ROS_WARN("rtabmapviz: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed."); } diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index e762303f..67008fac 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -201,12 +201,12 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : { if(iter->second.second.empty()) { - ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore!", + ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { - ROS_WARN("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } From 5793430009fb8b96e390f7a1f4a3a867f9f4d86f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 14:22:15 -0500 Subject: [PATCH 042/119] Fixed bug where odom's local map appeared in 2D --- src/OdometryROS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 67008fac..cce59ccd 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -368,7 +368,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) const std::multimap & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); for(std::multimap::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { - cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z)); + cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z)); } sensor_msgs::PointCloud2 cloudMsg; pcl::toROSMsg(cloud, cloudMsg); From dc81f2446d9db2535e3eafe285ebf1a60a496f9a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 15:42:57 -0500 Subject: [PATCH 043/119] Fixed PreferencesDialog::getAllParameters() fatal error --- src/PreferencesDialogROS.cpp | 22 ---------------------- src/PreferencesDialogROS.h | 3 ++- 2 files changed, 2 insertions(+), 23 deletions(-) diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index baf8807b..9025697a 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -122,25 +122,3 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) } -void PreferencesDialogROS::writeSettings(const QString & filePath) -{ - writeGuiSettings(filePath); - - // This will tell the MainWindow that the - //parameters are updated. The MainWindow will send an Event that - // will be handled by the GuiWrapper where we will write - // parameters in ROS and the rtabmap_node will be notified. - if(_parameters.size()) - { - emit settingsChanged(_parameters); - } - - if(_obsoletePanels) - { - emit settingsChanged(_obsoletePanels); - } - - _parameters = rtabmap::ParametersMap(); - _obsoletePanels = kPanelDummy; -} - diff --git a/src/PreferencesDialogROS.h b/src/PreferencesDialogROS.h index 2453f290..977f3d9e 100644 --- a/src/PreferencesDialogROS.h +++ b/src/PreferencesDialogROS.h @@ -46,7 +46,8 @@ protected: virtual void readCameraSettings(const QString & filePath); virtual bool readCoreSettings(const QString & filePath); - virtual void writeSettings(const QString & filePath); + virtual void writeCameraSettings(const QString & filePath) const {} + virtual void writeCoreSettings(const QString & filePath) const {} virtual QString getTmpIniFilePath() const; From 352010cd2dfe6b15c1e17cac89f78b90c9466146 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 16:07:15 -0500 Subject: [PATCH 044/119] Added ground_truth_frame_id to rtabmap node (to save ground truth in nodes) --- src/CoreWrapper.cpp | 62 +++++++++++++++++++++++++++++----------- src/CoreWrapper.h | 1 + src/MapAssemblerNode.cpp | 2 +- 3 files changed, 48 insertions(+), 17 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 35a81b50..4012f0cd 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -80,6 +80,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : frameId_("base_link"), mapFrameId_("map"), odomFrameId_(""), + groundTruthFrameId_(""), // e.g., "world" configPath_(""), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), waitForTransform_(true), @@ -153,6 +154,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : pnh.param("frame_id", frameId_, frameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF + pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("queue_size", queueSize, queueSize); pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync); @@ -180,6 +182,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : { odomFrameId_ = tfPrefix+"/"+odomFrameId_; } + if(!groundTruthFrameId_.empty()) + { + groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_; + } + // keep worldFrameId_ without prefix as it should be global } if(depthCameras <= 0 && subscribeDepth) @@ -192,6 +199,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : { ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); } + if(!groundTruthFrameId_.empty()) + { + ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str()); + } ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("rtabmap: queue_size = %d", queueSize); ROS_INFO("rtabmap: tf_delay = %f", tfDelay); @@ -880,15 +891,24 @@ void CoreWrapper::commonDepthCallback( scan3dMsg.get() != 0?scan3dMsg->header.stamp: depthMsgs[0]->header.stamp; + Transform groundTruthPose; + if(!groundTruthFrameId_.empty()) + { + groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); + } + + SensorData data(scan, + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts, + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + rgb, + depth, + cameraModels, + imageMsgs[0]->header.seq, + rtabmap_ros::timestampFromROS(stamp)); + data.setGroundTruth(groundTruthPose); + process(stamp, - SensorData(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():genMaxScanPts, - scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), - rgb, - depth, - cameraModels, - imageMsgs[0]->header.seq, - rtabmap_ros::timestampFromROS(stamp)), + data, lastPose_, odomFrameId, uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, @@ -1019,15 +1039,25 @@ void CoreWrapper::commonStereoCallback( ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: scan3dMsg.get() != 0?scan3dMsg->header.stamp: leftImageMsg->header.stamp; + + Transform groundTruthPose; + if(!groundTruthFrameId_.empty()) + { + groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp); + } + + SensorData data(scan, + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, + ptrLeftImage->image, + ptrRightImage->image, + stereoModel, + leftImageMsg->header.seq, + rtabmap_ros::timestampFromROS(stamp)); + data.setGroundTruth(groundTruthPose); + process(stamp, - SensorData(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get() != 0?scan2dMsg->range_max:0, - ptrLeftImage->image, - ptrRightImage->image, - stereoModel, - leftImageMsg->header.seq, - rtabmap_ros::timestampFromROS(stamp)), + data, lastPose_, odomFrameId, uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0, diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 03924b99..8ee66689 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -266,6 +266,7 @@ private: std::string frameId_; std::string mapFrameId_; std::string odomFrameId_; + std::string groundTruthFrameId_; std::string configPath_; std::string databasePath_; bool waitForTransform_; diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 3395376e..b6c2ca20 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -89,7 +89,7 @@ public: Signature tmpS = nodes_.at(poses.rbegin()->first); SensorData tmpData = tmpS.sensorData(); tmpData.setId(-1); - uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpS.getGroundTruthPose(), tmpData))); + uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData))); poses.insert(std::make_pair(-1, poses.rbegin()->second)); } From ae8dfee188daa26478ad21323f756e46e215a6e3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 16:08:20 -0500 Subject: [PATCH 045/119] Updated rgbdslam_datasets.launch --- launch/tests/rgbdslam_datasets.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index a026a383..a25bd080 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -65,6 +65,7 @@ + From 697b3bae7b9cfdbb64d84a57a1fadff202a82778 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Jan 2016 17:08:49 -0500 Subject: [PATCH 046/119] Updated PreferencesDialogROS::getTmpIniFilePath() to be public --- src/PreferencesDialogROS.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/PreferencesDialogROS.h b/src/PreferencesDialogROS.h index 977f3d9e..18dced84 100644 --- a/src/PreferencesDialogROS.h +++ b/src/PreferencesDialogROS.h @@ -40,6 +40,7 @@ public: virtual ~PreferencesDialogROS(); virtual QString getIniFilePath() const; + virtual QString getTmpIniFilePath() const; protected: virtual QString getParamMessage(); @@ -49,8 +50,6 @@ protected: virtual void writeCameraSettings(const QString & filePath) const {} virtual void writeCoreSettings(const QString & filePath) const {} - virtual QString getTmpIniFilePath() const; - private: QString configFile_; }; From 2250eb1f011fe256190b2eebe0ab36c169c1a13c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 13 Jan 2016 13:47:56 -0500 Subject: [PATCH 047/119] Updated rgbdslam_datasets.launch --- launch/tests/rgbdslam_datasets.launch | 37 ++++++++++----------------- 1 file changed, 13 insertions(+), 24 deletions(-) diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index a25bd080..3b9deea0 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -10,20 +10,7 @@ --> - - - - - - - + @@ -40,13 +27,18 @@ + - - - - - + + + + + + + + + @@ -70,10 +62,7 @@ - - - - + @@ -90,7 +79,7 @@ - + From edd107f889c586ecfcafb97fdd97f47967db5dd6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 27 Jan 2016 10:46:20 -0500 Subject: [PATCH 048/119] Updated rtabmap.launch to fix depth_registered_topic not defined when compressed=true. Also added relay topic renaming to avoid subscribing to raw published from kinect and the one published from the relays (when used). --- launch/rtabmap.launch | 53 +++++++++++++++++++++++++++---------------- 1 file changed, 33 insertions(+), 20 deletions(-) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index a494b72a..6a7b6444 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -48,6 +48,9 @@ + + + @@ -55,17 +58,27 @@ + + + + + + + + + + - - + + - - + + @@ -77,12 +90,12 @@ - - + + - - + + @@ -107,12 +120,12 @@ - - + + - - + + @@ -130,12 +143,12 @@ - - + + - - + + @@ -148,12 +161,12 @@ - - + + - - + + From 622d15ea6ae09ea5b8da2e7055fd447a1c7017cb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 2 Feb 2016 12:22:23 -0500 Subject: [PATCH 049/119] rtabmap: Added warnings when scan/depth cannot be synchronized to odom TF, instead of aborting update. --- src/CoreWrapper.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index f8826201..a9562e76 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -778,6 +778,7 @@ void CoreWrapper::commonDepthCallback( Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); if(localTransform.isNull()) { + ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec()); return; } // sync with odometry stamp @@ -788,9 +789,13 @@ void CoreWrapper::commonDepthCallback( Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp); if(sensorT.isNull()) { - return; + ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry " + "stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec()); + } + else + { + localTransform = odomT.inverse() * sensorT * localTransform; } - localTransform = odomT.inverse() * sensorT * localTransform; } } @@ -884,6 +889,7 @@ void CoreWrapper::commonDepthCallback( // make sure the frame of the laser is updated too if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec()); return; } @@ -902,10 +908,14 @@ void CoreWrapper::commonDepthCallback( Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); if(sensorT.isNull()) { - return; + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec()); + } + else + { + Transform t = odomT.inverse() * sensorT; + pclScan = util3d::transformPointCloud(pclScan, t); } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); } } From 36f283851257eb3d65dc3b4b918a2a4efdfe26d6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 8 Feb 2016 11:24:06 -0500 Subject: [PATCH 050/119] rtabmap.launch: added missing launch_prefix --- launch/rtabmap.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 6a7b6444..e1593bb6 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -93,7 +93,7 @@ - + @@ -109,7 +109,7 @@ - + @@ -134,7 +134,7 @@ - + From b4c0224f0668c5c6e472eddbddf984305b7e62ce Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 17 Feb 2016 16:05:35 -0500 Subject: [PATCH 051/119] Fixed build errors against 0.11.2 --- src/DbPlayerNode.cpp | 4 ++-- src/MapsManager.cpp | 10 +++++----- src/MsgConversion.cpp | 4 ++-- src/rviz/MapCloudDisplay.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index 818b2647..de4fab77 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -214,7 +214,7 @@ int main(int argc, char** argv) else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) { //stereo - if(odom.data().stereoCameraModel().isValid()) + if(odom.data().stereoCameraModel().isValidForProjection()) { camInfoA.D.resize(8,0); @@ -262,7 +262,7 @@ int main(int argc, char** argv) { localTransform = odom.data().cameraModels()[0].localTransform(); } - else if(odom.data().stereoCameraModel().isValid()) + else if(odom.data().stereoCameraModel().isValidForProjection()) { localTransform = odom.data().stereoCameraModel().left().localTransform(); } diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 322f2bc6..501036ef 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -224,12 +224,12 @@ std::map MapsManager::updateMapCaches( { if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && - (data.cameraModels().size() || data.stereoCameraModel().isValid())) + (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())) { // Which data should we decompress? cv::Mat image, depth, scan; data.uncompressData( - (rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0, + (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0, (rgbDepthRequired||depthRequired) ? &depth:0, scanRequired||gridRequired?&scan:0); @@ -287,7 +287,7 @@ std::map MapsManager::updateMapCaches( // Make sure that image size is set in camera models. // The camera models are used when cloud_frustum_culling=true. std::vector models; - if(data.stereoCameraModel().isValid()) + if(data.stereoCameraModel().isValidForProjection()) { //insert only the left camera model rtabmap::CameraModel model = data.stereoCameraModel().left(); @@ -380,7 +380,7 @@ std::map MapsManager::updateMapCaches( iter->first, !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0, - (data.cameraModels().size() || data.stereoCameraModel().isValid())?1:0); + (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0); } } } @@ -489,7 +489,7 @@ void MapsManager::publishMaps( { for(unsigned int i=0; isecond.size(); ++i) { - if(kter->second[i].isValid()) + if(kter->second[i].isValidForProjection()) { int size = assembledCloud->size(); assembledCloud = util3d::frustumFiltering( diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 9a517525..306bea79 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -487,7 +487,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) msg.label, transformFromPoseMsg(msg.pose), transformFromPoseMsg(msg.groundTruthPose), - stereoModel.isValid()? + stereoModel.isValidForProjection()? rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), msg.laserScanMaxPts, @@ -545,7 +545,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]); } } - else if(signature.sensorData().stereoCameraModel().isValid()) + else if(signature.sensorData().stereoCameraModel().isValidForProjection()) { msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx()); msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy()); diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 0bf52c0f..54c5bf28 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -259,7 +259,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]); if(!s.sensorData().imageCompressed().empty() && !s.sensorData().depthOrRightCompressed().empty() && - (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid())) + (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) { cv::Mat image, depth; s.sensorData().uncompressData(&image, &depth, 0); From 35639248b95c1e1826b4e1cf33add272f3e96d0f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 18 Feb 2016 11:19:35 -0500 Subject: [PATCH 052/119] Fixed crash with odometryF2F by cloning input images, creating a new map when variance >=9999 is detected, implemented intermediate nodes in ROS --- src/CoreWrapper.cpp | 43 +++++++++++++++++++++++++++++++++------- src/CoreWrapper.h | 2 ++ src/RGBDOdometryNode.cpp | 4 ++-- 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 4012f0cd..a0571a02 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -74,6 +74,7 @@ using namespace rtabmap; CoreWrapper::CoreWrapper(bool deleteDbOnStart) : paused_(false), lastPose_(Transform::getIdentity()), + lastPoseIntermediate_(false), rotVariance_(0), transVariance_(0), latestNodeWasReached_(false), @@ -103,6 +104,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : stereoExactTFSync_(0), transformThread_(0), rate_(Parameters::defaultRtabmapDetectionRate()), + createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), mbClient_("move_base", true) { @@ -317,8 +319,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { - rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate())); - ROS_INFO("RTAB-Map rate detection = %f Hz", rate_); + Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_); + ROS_INFO("RTAB-Map detection rate = %f Hz", rate_); + } + if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end()) + { + Parameters::parse(parameters_, Parameters::kRtabmapCreateIntermediateNodes(), createIntermediateNodes_); + if(createIntermediateNodes_) + { + ROS_INFO("Create intermediate nodes"); + } } bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); if(isRGBD) @@ -589,14 +599,15 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && odom.isIdentity()) + if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= 9999)) { - UWARN("Odometry is reset (identity pose detected). Increment map id!"); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; } + lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); @@ -611,14 +622,30 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) } // Throttle + bool ignoreFrame = false; if(rate_>0.0f) { if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_)) + { + ignoreFrame = true; + } + } + if(ignoreFrame) + { + if(createIntermediateNodes_) + { + lastPoseIntermediate_ = true; + } + else { return false; } } - time_ = ros::Time::now(); + else if(!ignoreFrame) + { + time_ = ros::Time::now(); + } + return true; } return false; @@ -643,6 +670,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp) transVariance_ = 0; } + lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = stamp; // Throttle @@ -903,7 +931,7 @@ void CoreWrapper::commonDepthCallback( rgb, depth, cameraModels, - imageMsgs[0]->header.seq, + lastPoseIntermediate_?-1:imageMsgs[0]->header.seq, rtabmap_ros::timestampFromROS(stamp)); data.setGroundTruth(groundTruthPose); @@ -1052,7 +1080,7 @@ void CoreWrapper::commonStereoCallback( ptrLeftImage->image, ptrRightImage->image, stereoModel, - leftImageMsg->header.seq, + lastPoseIntermediate_?-1:leftImageMsg->header.seq, rtabmap_ros::timestampFromROS(stamp)); data.setGroundTruth(groundTruthPose); @@ -1593,6 +1621,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt rotVariance_ = 0; transVariance_ = 0; lastPose_.setIdentity(); + lastPoseIntermediate_ = false; currentMetricGoal_.setNull(); latestNodeWasReached_ = false; mapsManager_.clear(); diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 8ee66689..e8a9ba10 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -257,6 +257,7 @@ private: bool paused_; rtabmap::Transform lastPose_; ros::Time lastPoseStamp_; + bool lastPoseIntermediate_; double rotVariance_; double transVariance_; rtabmap::Transform currentMetricGoal_; @@ -462,6 +463,7 @@ private: boost::thread* transformThread_; float rate_; + bool createIntermediateNodes_; ros::Time time_; }; diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index 9874ea89..abd08b0c 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -197,8 +197,8 @@ public: model.cx(), model.cy(), localTransform); - cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); - cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth); + cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); + cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); rtabmap::SensorData data( ptrImage->image, From caaf81731e89732b2a6b0b6d6661d26c6f01ffd7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 18 Feb 2016 12:53:26 -0500 Subject: [PATCH 053/119] Avoid publishing data when an intermediate node is detected (fixed also stamp based detection rate) --- src/CoreWrapper.cpp | 183 +++++++++++++++++++++++++------------------- src/CoreWrapper.h | 1 + 2 files changed, 107 insertions(+), 77 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index a0571a02..de175f4b 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -106,6 +106,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : rate_(Parameters::defaultRtabmapDetectionRate()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), + previousStamp_(0), mbClient_("move_base", true) { ros::NodeHandle nh; @@ -625,7 +626,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) bool ignoreFrame = false; if(rate_>0.0f) { - if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_)) + if((previousStamp_.toSec() > 0.0 && odomMsg->header.stamp.toSec() > previousStamp_.toSec() && odomMsg->header.stamp - previousStamp_ < ros::Duration(1.0f/rate_)) || + ((previousStamp_.toSec() <= 0.0 || odomMsg->header.stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_))) { ignoreFrame = true; } @@ -644,6 +646,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) else if(!ignoreFrame) { time_ = ros::Time::now(); + previousStamp_ = odomMsg->header.stamp; } return true; @@ -673,15 +676,33 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp) lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = stamp; - // Throttle + + bool ignoreFrame = false; if(rate_>0.0f) { - if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_)) + if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) || + ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_))) + { + ignoreFrame = true; + } + } + if(ignoreFrame) + { + if(createIntermediateNodes_) + { + lastPoseIntermediate_ = true; + } + else { return false; } } - time_ = ros::Time::now(); + else if(!ignoreFrame) + { + time_ = ros::Time::now(); + previousStamp_ = stamp; + } + return true; } return false; @@ -1319,96 +1340,103 @@ void CoreWrapper::process( odomFrameId_ = odomFrameId; mapToOdomMutex_.unlock(); - // Publish local graph, info - this->publishStats(stamp); - std::map filteredPoses = rtabmap_.getLocalOptimizedPoses(); - - // create a tmp signature with latest sensory data - std::map tmpSignature; - SensorData tmpData = data; - tmpData.setId(-1); - tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); - filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom)); - - // Update maps - filteredPoses = mapsManager_.updateMapCaches( - filteredPoses, - rtabmap_.getMemory(), - false, - false, - false, - false, - tmpSignature); - - mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); - - // update goal if planning is enabled - if(!currentMetricGoal_.isNull()) + if(data.id() < 0) { - if(rtabmap_.getPath().size() == 0) + ROS_INFO("Intermediate node added"); + } + else + { + // Publish local graph, info + this->publishStats(stamp); + std::map filteredPoses = rtabmap_.getLocalOptimizedPoses(); + + // create a tmp signature with latest sensory data + std::map tmpSignature; + SensorData tmpData = data; + tmpData.setId(-1); + tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); + filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom)); + + // Update maps + filteredPoses = mapsManager_.updateMapCaches( + filteredPoses, + rtabmap_.getMemory(), + false, + false, + false, + false, + tmpSignature); + + mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_); + + // update goal if planning is enabled + if(!currentMetricGoal_.isNull()) { - if(rtabmap_.getPathStatus() > 0) + if(rtabmap_.getPath().size() == 0) { - // Goal reached - ROS_INFO("Planning: Publishing goal reached!"); - } - else - { - ROS_WARN("Planning: Plan failed!"); - if(mbClient_.isServerConnected()) + if(rtabmap_.getPathStatus() > 0) { - mbClient_.cancelGoal(); + // Goal reached + ROS_INFO("Planning: Publishing goal reached!"); } - } - if(goalReachedPub_.getNumSubscribers()) - { - std_msgs::Bool result; - result.data = rtabmap_.getPathStatus() > 0; - goalReachedPub_.publish(result); - } - currentMetricGoal_.setNull(); - latestNodeWasReached_ = false; - } - else - { - currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId()); - if(!currentMetricGoal_.isNull()) - { - // Adjust the target pose relative to last node - if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) + else { - if(latestNodeWasReached_ || - rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || - rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) + ROS_WARN("Planning: Plan failed!"); + if(mbClient_.isServerConnected()) { - latestNodeWasReached_ = true; - currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); + mbClient_.cancelGoal(); } } - - // publish next goal with updated currentMetricGoal_ - publishCurrentGoal(stamp); - - // publish local path - publishLocalPath(stamp); - - // publish global path - publishGlobalPath(stamp); - } - else - { - ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", - rtabmap_.getPathCurrentGoalId()); - rtabmap_.clearPath(-1); if(goalReachedPub_.getNumSubscribers()) { std_msgs::Bool result; - result.data = false; + result.data = rtabmap_.getPathStatus() > 0; goalReachedPub_.publish(result); } currentMetricGoal_.setNull(); latestNodeWasReached_ = false; } + else + { + currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId()); + if(!currentMetricGoal_.isNull()) + { + // Adjust the target pose relative to last node + if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) + { + if(latestNodeWasReached_ || + rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() || + rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius()) + { + latestNodeWasReached_ = true; + currentMetricGoal_ *= rtabmap_.getPathTransformToGoal(); + } + } + + // publish next goal with updated currentMetricGoal_ + publishCurrentGoal(stamp); + + // publish local path + publishLocalPath(stamp); + + // publish global path + publishGlobalPath(stamp); + } + else + { + ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)", + rtabmap_.getPathCurrentGoalId()); + rtabmap_.clearPath(-1); + if(goalReachedPub_.getNumSubscribers()) + { + std_msgs::Bool result; + result.data = false; + goalReachedPub_.publish(result); + } + currentMetricGoal_.setNull(); + latestNodeWasReached_ = false; + } + } } } } @@ -1625,6 +1653,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt currentMetricGoal_.setNull(); latestNodeWasReached_ = false; mapsManager_.clear(); + previousStamp_ = ros::Time(0); return true; } diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index e8a9ba10..f611eff6 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -465,6 +465,7 @@ private: float rate_; bool createIntermediateNodes_; ros::Time time_; + ros::Time previousStamp_; }; #endif /* COREWRAPPER_H_ */ From d25c77ebba9f3d1cb82cbb38f1e340aaba143a4e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 19 Feb 2016 15:12:13 -0500 Subject: [PATCH 054/119] Updated demo_robot_mapping.launch with 0.11 --- launch/demo/demo_robot_mapping.launch | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch index a8fc6c13..59a7ae1c 100644 --- a/launch/demo/demo_robot_mapping.launch +++ b/launch/demo/demo_robot_mapping.launch @@ -19,7 +19,7 @@ - + @@ -32,19 +32,21 @@ - - - + + + - - - - + + + + + + - + From 0bd147cfcfc32aab45a5a5732cbe7e4da7a3f347 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 23 Feb 2016 11:47:31 -0500 Subject: [PATCH 055/119] Updated package with latest changes from lib 0.11.2. Updated demo_stereo_outdoor.launch --- launch/demo/demo_stereo_outdoor.launch | 25 +++++++++++++------------ src/OdometryROS.cpp | 20 ++++++++++---------- src/OdometryROS.h | 2 +- 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index dc959a19..fc3de495 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -49,12 +49,13 @@ - - - - - - + + + + + + + @@ -80,19 +81,19 @@ - + - - + + - - - + + + diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index cce59ccd..6130eb57 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include #include @@ -318,7 +318,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) // process data ros::WallTime time = ros::WallTime::now(); rtabmap::OdometryInfo info; - rtabmap::Transform pose = odometry_->process(data, &info); + SensorData dataCpy = data; + rtabmap::Transform pose = odometry_->process(dataCpy, &info); if(!pose.isNull()) { //********************* @@ -362,10 +363,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } // local map / reference frame - if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) + if(odomLocalMap_.getNumSubscribers() && dynamic_cast(odometry_)) { pcl::PointCloud cloud; - const std::multimap & map = ((OdometryLocalMap*)odometry_)->getLocalMap(); + const std::multimap & map = ((OdometryF2M*)odometry_)->getMap().getWords3(); for(std::multimap::const_iterator iter=map.begin(); iter!=map.end(); ++iter) { cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z)); @@ -379,12 +380,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) if(odomLastFrame_.getNumSubscribers()) { - if(dynamic_cast(odometry_)) + if(dynamic_cast(odometry_)) { - const rtabmap::Signature * s = ((OdometryLocalMap*)odometry_)->getMemory()->getLastWorkingSignature(); - if(s) + const std::multimap & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3(); + if(words3.size()) { - const std::multimap & words3 = s->getWords3(); pcl::PointCloud cloud; for(std::multimap::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) { @@ -448,9 +448,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec()); } -bool OdometryROS::isOdometryBOW() const +bool OdometryROS::isOdometryF2M() const { - return dynamic_cast(odometry_) != 0; + return dynamic_cast(odometry_) != 0; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 2813ff5b..6b5de5ed 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -70,7 +70,7 @@ public: const rtabmap::ParametersMap & parameters() const {return parameters_;} const tf::TransformListener & tfListener() const {return tfListener_;} bool isPaused() const {return paused_;} - bool isOdometryBOW() const; + bool isOdometryF2M() const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; private: From ca0f72a3ce9207f9d603bf35eadf823c7035cff1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Feb 2016 12:45:54 -0500 Subject: [PATCH 056/119] Updated rgbdslam_datasets.launch with 0.11.2 --- launch/tests/rgbdslam_datasets.launch | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index 3b9deea0..d13f9dc5 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -29,21 +29,21 @@ - + - + - - - + + + - + @@ -56,8 +56,17 @@ + + + + + + + + + - + From f89ea11c655460f477025005b9e72a00690dd114 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 29 Feb 2016 14:42:28 -0500 Subject: [PATCH 057/119] rgbdslam_datasets.launch: Use 3D->3D estimation by default --- launch/tests/rgbdslam_datasets.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index d13f9dc5..4eb0765e 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -31,7 +31,7 @@ - + From b424e97d5fcb38a4134375e952a1fb1d96acaa20 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 29 Feb 2016 16:43:45 -0500 Subject: [PATCH 058/119] Odom: added --udebug and --uinfo arguments. Added "gen_depth" argument to bumblebee.launch. --- launch/tests/bumblebee.launch | 16 ++++++++-------- src/OdometryROS.cpp | 8 ++++++++ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/launch/tests/bumblebee.launch b/launch/tests/bumblebee.launch index fe3eb9f9..467f2e45 100644 --- a/launch/tests/bumblebee.launch +++ b/launch/tests/bumblebee.launch @@ -10,15 +10,16 @@ + - + - - + + @@ -28,13 +29,12 @@ - + + + - - - - \ No newline at end of file + diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 6130eb57..44264834 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -265,6 +265,14 @@ void OdometryROS::processArguments(int argc, char * argv[], bool stereo) "argument \"--params\" is detected!"); exit(0); } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } } } From 6b5ccf99fbb70a541ce8b5366a950eab07495a60 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 29 Feb 2016 17:27:00 -0500 Subject: [PATCH 059/119] Nodelets: Changed ROS_INFO/ROS_ERROR to NODELET_INFO/NODELET_ERROR --- src/nodelets/data_throttle.cpp | 8 ++++---- src/nodelets/disparity_to_depth.cpp | 2 +- src/nodelets/obstacles_detection.cpp | 6 +++--- src/nodelets/point_cloud_xyz.cpp | 6 +++--- src/nodelets/point_cloud_xyzrgb.cpp | 6 +++--- src/nodelets/stereo_throttle.cpp | 6 +++--- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/nodelets/data_throttle.cpp b/src/nodelets/data_throttle.cpp index 923aac31..df13547a 100644 --- a/src/nodelets/data_throttle.cpp +++ b/src/nodelets/data_throttle.cpp @@ -91,16 +91,16 @@ private: bool approxSync = true; if(private_nh.getParam("max_rate", rate_)) { - ROS_WARN("\"max_rate\" is now known as \"rate\"."); + NODELET_WARN("\"max_rate\" is now known as \"rate\"."); } private_nh.param("rate", rate_, rate_); private_nh.param("queue_size", queueSize, queueSize); private_nh.param("approx_sync", approxSync, approxSync); private_nh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - ROS_INFO("Rate=%f Hz", rate_); - ROS_INFO("Decimation=%d", decimation_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { diff --git a/src/nodelets/disparity_to_depth.cpp b/src/nodelets/disparity_to_depth.cpp index b90fbc37..1e97e9fd 100644 --- a/src/nodelets/disparity_to_depth.cpp +++ b/src/nodelets/disparity_to_depth.cpp @@ -63,7 +63,7 @@ private: { if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) { - ROS_ERROR("Input type must be disparity=32FC1"); + NODELET_ERROR("Input type must be disparity=32FC1"); return; } diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 62dac3ab..c2c12f72 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -119,7 +119,7 @@ private: { if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1))) { - ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); + NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str()); return; } } @@ -129,7 +129,7 @@ private: } catch(tf::TransformException & ex) { - ROS_ERROR("%s",ex.what()); + NODELET_ERROR("%s",ex.what()); return; } @@ -255,7 +255,7 @@ private: obstaclesPub_.publish(rosCloud); } - //ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); + //NODELET_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); } private: diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 082c3789..dc8a6eaf 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -108,7 +108,7 @@ private: pnh.param("cut_right", cut_right_, cut_right_); pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { @@ -149,7 +149,7 @@ private: depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) { - ROS_ERROR("Input type depth=32FC1,16UC1,MONO16"); + NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); return; } @@ -224,7 +224,7 @@ private: if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0) { - ROS_ERROR("Input type must be disparity=32FC1 or 16SC1"); + NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1"); return; } diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 146068ef..fd84f48b 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -102,7 +102,7 @@ private: pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); cloudPub_ = nh.advertise("cloud", 1); @@ -167,7 +167,7 @@ private: imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0)) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16"); return; } @@ -212,7 +212,7 @@ private: imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)) { - ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); + NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str()); return; } diff --git a/src/nodelets/stereo_throttle.cpp b/src/nodelets/stereo_throttle.cpp index 629e8841..482f70ec 100644 --- a/src/nodelets/stereo_throttle.cpp +++ b/src/nodelets/stereo_throttle.cpp @@ -93,9 +93,9 @@ private: pnh.param("queue_size", queueSize, queueSize); pnh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - ROS_INFO("Rate=%f Hz", rate_); - ROS_INFO("Decimation=%d", decimation_); - ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); + NODELET_INFO("Rate=%f Hz", rate_); + NODELET_INFO("Decimation=%d", decimation_); + NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false"); if(approxSync) { From 26c656776c10c445b2f09b63b351f932c20c0cc8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Mar 2016 11:24:10 -0500 Subject: [PATCH 060/119] fixed laser_geometry laser to point cloud stamp (including scanning time) --- src/CoreWrapper.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index a9562e76..cb7c3817 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -887,7 +887,9 @@ void CoreWrapper::commonDepthCallback( if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec()); return; @@ -998,8 +1000,11 @@ void CoreWrapper::commonStereoCallback( if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec()); return; } From a4dfe5a21e69db48c55f0db06cc2856a43eae1f4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Mar 2016 17:31:57 -0500 Subject: [PATCH 061/119] Updated against latest changes from 0.11. Added localMap features to OdomInfo msg. Updated rgbdslam_datasets.launch. --- CMakeLists.txt | 1 + include/rtabmap_ros/MsgConversion.h | 7 ++++ launch/tests/rgbdslam_datasets.launch | 20 +++++------ msg/OdomInfo.msg | 2 ++ msg/Point3f.msg | 10 ++++++ src/CoreWrapper.cpp | 2 +- src/MsgConversion.cpp | 48 +++++++++++++++++++++++++-- src/OdometryROS.cpp | 15 ++++----- 8 files changed, 82 insertions(+), 23 deletions(-) create mode 100644 msg/Point3f.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bc31f84..0ea720fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,7 @@ add_message_files( Link.msg OdomInfo.msg Point2f.msg + Point3f.msg Goal.msg ) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index c6c80b5c..328b98a9 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -85,6 +86,12 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg); std::vector points2fFromROS(const std::vector & msg); void points2fToROS(const std::vector & kpts, std::vector & msg); +cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg); +void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg); + +std::vector points3fFromROS(const std::vector & msg); +void points3fToROS(const std::vector & kpts, std::vector & msg); + rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::CameraInfo & camInfo, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity()); diff --git a/launch/tests/rgbdslam_datasets.launch b/launch/tests/rgbdslam_datasets.launch index 4eb0765e..66a01b20 100644 --- a/launch/tests/rgbdslam_datasets.launch +++ b/launch/tests/rgbdslam_datasets.launch @@ -29,21 +29,21 @@ - - - + + + - - - + + + - + @@ -57,13 +57,11 @@ - + - - - + diff --git a/msg/OdomInfo.msg b/msg/OdomInfo.msg index e5e3e09f..e3107457 100644 --- a/msg/OdomInfo.msg +++ b/msg/OdomInfo.msg @@ -42,6 +42,8 @@ int32[] wordsKeys KeyPoint[] wordsValues int32[] wordMatches int32[] wordInliers +int32[] localMapKeys +Point3f[] localMapValues Point2f[] refCorners Point2f[] newCorners diff --git a/msg/Point3f.msg b/msg/Point3f.msg new file mode 100644 index 00000000..eca5d135 --- /dev/null +++ b/msg/Point3f.msg @@ -0,0 +1,10 @@ +#class cv::Point3f +#{ +# float x; +# float y; +# float z; +#} + +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index baaaf811..1ae7b684 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -925,7 +925,7 @@ void CoreWrapper::commonDepthCallback( if(sensorT.isNull()) { ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " - "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec()); + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), lastPoseStamp_.toSec()); } else { diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 306bea79..7f8b7b1c 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -295,6 +295,37 @@ void points2fToROS(const std::vector & kpts, std::vector points3fFromROS(const std::vector & msg) +{ + std::vector v(msg.size()); + for(unsigned int i=0; i & kpts, std::vector & msg) +{ + msg.resize(kpts.size()); + for(unsigned int i=0; ipreviousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - odom.twist.twist.linear.x = x*dt; - odom.twist.twist.linear.y = y*dt; - odom.twist.twist.linear.z = z*dt; - odom.twist.twist.angular.x = roll*dt; - odom.twist.twist.angular.y = pitch*dt; - odom.twist.twist.angular.z = yaw*dt; + odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + odom.twist.twist.linear.x = x; + odom.twist.twist.linear.y = y; + odom.twist.twist.linear.z = z; + odom.twist.twist.angular.x = roll; + odom.twist.twist.angular.y = pitch; + odom.twist.twist.angular.z = yaw; } previousStamp_ = stamp; From b67f3386992b05015e26ee50436755622aabec08 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Mar 2016 19:16:15 -0500 Subject: [PATCH 062/119] Updated a comment --- launch/demo/demo_robot_mapping.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch index 59a7ae1c..e09b96dd 100644 --- a/launch/demo/demo_robot_mapping.launch +++ b/launch/demo/demo_robot_mapping.launch @@ -37,7 +37,7 @@ - + From 542aa43cccd20b33de53ceb7723407db89e7d387 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 10 Mar 2016 16:29:48 -0500 Subject: [PATCH 063/119] Fixed issue #47 --- src/GuiWrapper.cpp | 308 +++++++++++++++++++++++++++++++++++---------- src/GuiWrapper.h | 68 ++++++++++ 2 files changed, 312 insertions(+), 64 deletions(-) diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index d8427bf2..cc4417a5 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -1069,6 +1069,24 @@ void GuiWrapper::depthScanCallback( rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::depthScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + odomInfoMsg); +} + void GuiWrapper::depthScan3dCallback( const sensor_msgs::PointCloud2ConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1086,6 +1104,24 @@ void GuiWrapper::depthScan3dCallback( rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::depthScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& depthMsg, + const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg) +{ + commonDepthCallback( + odomMsg, + imageMsg, + depthMsg, + cameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + odomInfoMsg); +} + void GuiWrapper::stereoScanCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1105,6 +1141,26 @@ void GuiWrapper::stereoScanCallback( rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::stereoScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + scanMsg, + sensor_msgs::PointCloud2ConstPtr(), + odomInfoMsg); +} + void GuiWrapper::stereoScan3dCallback( const sensor_msgs::PointCloud2ConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1124,6 +1180,26 @@ void GuiWrapper::stereoScan3dCallback( rtabmap_ros::OdomInfoConstPtr()); } +void GuiWrapper::stereoScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg) +{ + commonStereoCallback( + odomMsg, + leftImageMsg, + rightImageMsg, + leftCameraInfoMsg, + rightCameraInfoMsg, + sensor_msgs::LaserScanConstPtr(), + scanMsg, + odomInfoMsg); +} + void GuiWrapper::stereoOdomInfoCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -1368,42 +1444,92 @@ void GuiWrapper::setupCallbacks( if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); - depthScanSync_ = new message_filters::Synchronizer( - MyDepthScanSyncPolicy(queueSize), - scanSub_, - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + depthScanOdomInfoSync_ = new message_filters::Synchronizer( + MyDepthScanOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scanSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - odomSub_.getTopic().c_str(), - scanSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + depthScanSync_ = new message_filters::Synchronizer( + MyDepthScanSyncPolicy(queueSize), + scanSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str()); + } } else if(subscribeLaserScan3d) { scan3dSub_.subscribe(nh, "scan_cloud", 1); - depthScan3dSync_ = new message_filters::Synchronizer( - MyDepthScan3dSyncPolicy(queueSize), - scan3dSub_, - odomSub_, - *imageSubs_[0], - *imageDepthSubs_[0], - *cameraInfoSubs_[0]); - depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + depthScan3dOdomInfoSync_ = new message_filters::Synchronizer( + MyDepthScan3dOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scan3dSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageSubs_[0]->getTopic().c_str(), - imageDepthSubs_[0]->getTopic().c_str(), - cameraInfoSubs_[0]->getTopic().c_str(), - odomSub_.getTopic().c_str(), - scan3dSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + depthScan3dSync_ = new message_filters::Synchronizer( + MyDepthScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + *imageSubs_[0], + *imageDepthSubs_[0], + *cameraInfoSubs_[0]); + depthScan3dSync_->registerCallback(boost::bind(&GuiWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageSubs_[0]->getTopic().c_str(), + imageDepthSubs_[0]->getTopic().c_str(), + cameraInfoSubs_[0]->getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } } else if(subscribeOdomInfo) { @@ -1593,46 +1719,100 @@ void GuiWrapper::setupCallbacks( if(subscribeLaserScan2d) { scanSub_.subscribe(nh, "scan", 1); - stereoScanSync_ = new message_filters::Synchronizer( - MyStereoScanSyncPolicy(queueSize), - scanSub_, - odomSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + stereoScanOdomInfoSync_ = new message_filters::Synchronizer( + MyStereoScanOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scanSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScanOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str(), - odomSub_.getTopic().c_str(), - scanSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + stereoScanSync_ = new message_filters::Synchronizer( + MyStereoScanSyncPolicy(queueSize), + scanSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scanSub_.getTopic().c_str()); + } } else if(subscribeLaserScan3d) { scan3dSub_.subscribe(nh, "scan_cloud", 1); - stereoScan3dSync_ = new message_filters::Synchronizer( - MyStereoScan3dSyncPolicy(queueSize), - scan3dSub_, - odomSub_, - imageRectLeft_, - imageRectRight_, - cameraInfoLeft_, - cameraInfoRight_); - stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + if(subscribeOdomInfo) + { + odomInfoSub_.subscribe(nh, "odom_info", 1); + stereoScan3dOdomInfoSync_ = new message_filters::Synchronizer( + MyStereoScan3dOdomInfoSyncPolicy(queueSize), + odomInfoSub_, + scan3dSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dOdomInfoCallback, this, _1, _2, _3, _4, _5, _6, _7)); - ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", - ros::this_node::getName().c_str(), - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), - cameraInfoLeft_.getTopic().c_str(), - cameraInfoRight_.getTopic().c_str(), - odomSub_.getTopic().c_str(), - scan3dSub_.getTopic().c_str()); + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str(), + odomInfoSub_.getTopic().c_str()); + } + else + { + stereoScan3dSync_ = new message_filters::Synchronizer( + MyStereoScan3dSyncPolicy(queueSize), + scan3dSub_, + odomSub_, + imageRectLeft_, + imageRectRight_, + cameraInfoLeft_, + cameraInfoRight_); + stereoScan3dSync_->registerCallback(boost::bind(&GuiWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6)); + + ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", + ros::this_node::getName().c_str(), + imageRectLeft_.getTopic().c_str(), + imageRectRight_.getTopic().c_str(), + cameraInfoLeft_.getTopic().c_str(), + cameraInfoRight_.getTopic().c_str(), + odomSub_.getTopic().c_str(), + scan3dSub_.getTopic().c_str()); + } } else if(subscribeOdomInfo) { diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index f1d1b761..5eabe6d5 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -150,12 +150,26 @@ private: const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); void depthScan3dCallback( const sensor_msgs::PointCloud2ConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::CameraInfoConstPtr& camInfoMsg); + void depthScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& imageMsg, + const sensor_msgs::ImageConstPtr& imageDepthMsg, + const sensor_msgs::CameraInfoConstPtr& camInfoMsg); void stereoScanCallback( const sensor_msgs::LaserScanConstPtr& scanMsg, @@ -164,6 +178,14 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScanOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::LaserScanConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoScan3dCallback( const sensor_msgs::PointCloud2ConstPtr& scanMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -171,6 +193,14 @@ private: const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); + void stereoScan3dOdomInfoCallback( + const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, + const sensor_msgs::PointCloud2ConstPtr& scanMsg, + const nav_msgs::OdometryConstPtr & odomMsg, + const sensor_msgs::ImageConstPtr& leftImageMsg, + const sensor_msgs::ImageConstPtr& rightImageMsg, + const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, + const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); void stereoOdomInfoCallback( const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const nav_msgs::OdometryConstPtr & odomMsg, @@ -285,6 +315,15 @@ private: sensor_msgs::CameraInfo> MyDepthScanSyncPolicy; message_filters::Synchronizer * depthScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::LaserScan, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy; + message_filters::Synchronizer * depthScanOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, nav_msgs::Odometry, @@ -293,6 +332,15 @@ private: sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy; message_filters::Synchronizer * depthScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy; + message_filters::Synchronizer * depthScan3dOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< nav_msgs::Odometry, sensor_msgs::Image, @@ -325,6 +373,16 @@ private: sensor_msgs::CameraInfo> MyStereoScanSyncPolicy; message_filters::Synchronizer * stereoScanSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::LaserScan, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy; + message_filters::Synchronizer * stereoScanOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, nav_msgs::Odometry, @@ -334,6 +392,16 @@ private: sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy; message_filters::Synchronizer * stereoScan3dSync_; + typedef message_filters::sync_policies::ApproximateTime< + rtabmap_ros::OdomInfo, + sensor_msgs::PointCloud2, + nav_msgs::Odometry, + sensor_msgs::Image, + sensor_msgs::Image, + sensor_msgs::CameraInfo, + sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy; + message_filters::Synchronizer * stereoScan3dOdomInfoSync_; + typedef message_filters::sync_policies::ApproximateTime< rtabmap_ros::OdomInfo, nav_msgs::Odometry, From af87689718bff4bbc56298c7e4b8fd3f3eeefc70 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 16:56:32 -0500 Subject: [PATCH 064/119] RTAB-Map parameters can be also set using node arguments (or rtabmap_args for some launch files) --- src/CoreNode.cpp | 21 +++------------------ src/CoreWrapper.cpp | 9 ++++++++- src/CoreWrapper.h | 2 +- src/OdometryROS.cpp | 11 +++++++++++ 4 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index 31fff376..028cde83 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -48,18 +48,6 @@ int main(int argc, char** argv) { deleteDbOnStart = true; } - else if(strcmp(argv[i], "--udebug") == 0) - { - ULogger::setLevel(ULogger::kDebug); - } - else if(strcmp(argv[i], "--uinfo") == 0) - { - ULogger::setLevel(ULogger::kInfo); - } - else if(strcmp(argv[i], "--uwarn") == 0) - { - ULogger::setLevel(ULogger::kWarning); - } else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); @@ -98,14 +86,11 @@ int main(int argc, char** argv) "argument \"--params\" is detected!"); exit(0); } - else - { - ROS_ERROR("Not recognized argument \"%s\"", argv[i]); - exit(-1); - } } - CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart); + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); + + CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters); ROS_INFO("rtabmap %s started...", RTABMAP_VERSION); ros::spin(); diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 1ae7b684..1b55634c 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -71,7 +71,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace rtabmap; -CoreWrapper::CoreWrapper(bool deleteDbOnStart) : +CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) : paused_(false), lastPose_(Transform::getIdentity()), lastPoseIntermediate_(false), @@ -281,6 +281,13 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) : } } + //update with input arguments + for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + uInsert(parameters_, ParametersPair(iter->first, iter->second)); + ROS_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + } + // Backward compatibility for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); iter!=Parameters::getRemovedParameters().end(); diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 45d1a369..396a7402 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -80,7 +80,7 @@ typedef actionlib::SimpleActionClient MoveBaseCl class CoreWrapper { public: - CoreWrapper(bool deleteDbOnStart); + CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters); virtual ~CoreWrapper(); private: diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 9f492ec4..2c23836c 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -182,6 +182,17 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } } + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first); + if(jter!=parameters_.end()) + { + ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); + jter->second = iter->second; + } + } + // Backward compatibility for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); iter!=Parameters::getRemovedParameters().end(); From fdbac13a77af3ea04c0a4f2dc64b53f16f0c9bbc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 16:57:49 -0500 Subject: [PATCH 065/119] Updated main launch files for 0.11 --- launch/rgbd_mapping.launch | 46 ++++++++++++------------- launch/rgbd_mapping_kinect2.launch | 20 +++++------ launch/rtabmap.launch | 4 +-- launch/stereo_mapping.launch | 54 ++++++++++++++++-------------- 4 files changed, 63 insertions(+), 61 deletions(-) diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index e1e68830..a1cd9142 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -36,15 +36,15 @@ - + - + - + @@ -53,7 +53,7 @@ - + @@ -62,15 +62,15 @@ - - - - - - - + + + + + + + - + @@ -87,13 +87,13 @@ - - - - - - - + + + + + + + @@ -104,10 +104,10 @@ - - - - + + + + diff --git a/launch/rgbd_mapping_kinect2.launch b/launch/rgbd_mapping_kinect2.launch index db42349e..55c76c37 100644 --- a/launch/rgbd_mapping_kinect2.launch +++ b/launch/rgbd_mapping_kinect2.launch @@ -30,7 +30,7 @@ diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index e1593bb6..45089f93 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -76,7 +76,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index d3cc7f89..708658d6 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -18,6 +18,7 @@ + @@ -26,6 +27,7 @@ + @@ -37,15 +39,14 @@ - + - + - @@ -55,7 +56,7 @@ - + @@ -65,21 +66,21 @@ - - - - - - - - - - + + + + + + + + + + - + @@ -95,12 +96,13 @@ - - - - - - + + + + + + + @@ -110,12 +112,12 @@ - - - + + + - - + + From c8cfc7b3d4a9101f210563b76fbb8ec56a0d526d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 17:04:11 -0500 Subject: [PATCH 066/119] Updated data_recorder.launch for 0.11 --- launch/data_recorder.launch | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index 310e6f17..df31c343 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -31,15 +31,15 @@ - - - - + + + + - + From a0d7690d71a37ecb25c1423dcf510698c51a8e20 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 17:14:26 -0500 Subject: [PATCH 067/119] Removed test_odometry.launch test_stereo_data_recorder.launch test_stereo_odometry.launch --- launch/tests/test_odometry.launch | 61 ------------------- launch/tests/test_stereo_data_recorder.launch | 56 ----------------- launch/tests/test_stereo_odometry.launch | 48 --------------- 3 files changed, 165 deletions(-) delete mode 100644 launch/tests/test_odometry.launch delete mode 100644 launch/tests/test_stereo_data_recorder.launch delete mode 100644 launch/tests/test_stereo_odometry.launch diff --git a/launch/tests/test_odometry.launch b/launch/tests/test_odometry.launch deleted file mode 100644 index 7cce2a8b..00000000 --- a/launch/tests/test_odometry.launch +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tests/test_stereo_data_recorder.launch b/launch/tests/test_stereo_data_recorder.launch deleted file mode 100644 index 0f523a58..00000000 --- a/launch/tests/test_stereo_data_recorder.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/tests/test_stereo_odometry.launch b/launch/tests/test_stereo_odometry.launch deleted file mode 100644 index ff12a1c4..00000000 --- a/launch/tests/test_stereo_odometry.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file From c3e6384d9d1dc18088cd9655678f55c6291c981e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 17:17:26 -0500 Subject: [PATCH 068/119] updated appearance demo gui config --- launch/config/appearance_gui.ini | 150 ++++++++++++++++++++++++++++--- 1 file changed, 136 insertions(+), 14 deletions(-) diff --git a/launch/config/appearance_gui.ini b/launch/config/appearance_gui.ini index e0ecf09c..9e379330 100644 --- a/launch/config/appearance_gui.ini +++ b/launch/config/appearance_gui.ini @@ -6,17 +6,12 @@ General\loggerPauseLevel=4 General\loggerType=1 General\loggerPrintTime=true General\verticalLayoutUsed=false -General\imageFlipped=false General\imageRejectedShown=true General\imageHighestHypShown=true General\beep=false -General\keypointsOpacity=16 -General\voxelSize=0 -General\decimation=16 -MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x2\xaf\0\0\x1\xf4\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x1\xf4\0\0\x1\xcc\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x1\xf0\xfc\x2\0\0\0\x3\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0(\0\0\x1\xf4\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x1\xf0\0\0\0y\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0O\0\xff\xff\xff\0\0\0\0\0\0\x1\xf0\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" -MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xbd\0\0\0x\0\0\x5\xcc\0\0\x3k\0\0\0\xc5\0\0\0\x94\0\0\x5\xc4\0\0\x3\x63\0\0\0\0\0\0) +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\a\0\0\x2\x30\xfc\x2\0\0\0\x2\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x30\0\0\x2\x30\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1\xf4\0\0\0\x19\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x30\xfc\x2\0\0\0\x3\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0(\0\0\x1\xf4\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf7\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x2\x30\0\0\0+\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9c\xfc\x1\0\0\0\x6\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\x1\0\0\0\0\0\0\x5\0\0\0\0\x8d\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0O\0\xff\xff\xff\0\0\0\0\0\0\x2\x30\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc5\0\0\0K\0\0\x5\xd8\0\0\x3t\0\0\0\xcf\0\0\0q\0\0\x5\xce\0\0\x3j\0\0\0\0\0\0) General\showClouds0=true -General\voxelSize0=0 General\decimation0=4 General\maxDepth0=4 General\showScans0=true @@ -25,7 +20,6 @@ General\ptSize0=1 General\opacityScan0=1 General\ptSizeScan0=1 General\showClouds1=true -General\voxelSize1=0 General\decimation1=2 General\maxDepth1=0 General\showScans1=true @@ -33,12 +27,140 @@ General\opacity1=1 General\ptSize1=1 General\opacityScan1=1 General\ptSizeScan1=1 -General\showClouds2=true -General\voxelSize2=0.01 -General\decimation2=1 -General\maxDepth2=4 -General\showScans2=true -General\meshing0=false General\cloudFiltering=false General\cloudFilteringRadius=0.5 General\cloudFilteringAngle=30 +MainWindow\maximized=false +MainWindow\status_bar=false +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0\0\0\0\0\x3\xd7\0\0\x2\xb4\0\0\0\0\0\0) +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0\0\0\0\0\x3>\0\0\x2\xa8\0\0\0\0\0\0) +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_focal=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0) +widget_cloudViewer\grid=false +widget_cloudViewer\grid_cell_count=50 +widget_cloudViewer\grid_cell_size=1 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\image_shown=true +imageView_source\depth_shown=false +imageView_source\features_shown=true +imageView_source\lines_shown=true +imageView_source\alpha=50 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_loopClosure\image_shown=true +imageView_loopClosure\depth_shown=false +imageView_loopClosure\features_shown=true +imageView_loopClosure\lines_shown=true +imageView_loopClosure\alpha=50 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_odometry\image_shown=true +imageView_odometry\depth_shown=false +imageView_odometry\features_shown=true +imageView_odometry\lines_shown=true +imageView_odometry\alpha=200 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +ExportCloudsDialog\binary=true +ExportCloudsDialog\normals_k=6 +ExportCloudsDialog\regenerate=false +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_radius=0.02 +ExportCloudsDialog\filtering_min_neighbors=2 +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_voxel=0.01 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_point_radius=0.02 +ExportCloudsDialog\subtract_point_angle=45 +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_radius=0.04 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0 +ExportCloudsDialog\mls_point_density=0 +ExportCloudsDialog\mls_dilation_voxel_size=0.01 +ExportCloudsDialog\mls_dilation_iterations=0 +ExportCloudsDialog\mesh=false +ExportCloudsDialog\mesh_radius=0.04 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_triangle_size=2 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\cluster_radius=0.5 +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\iterations=1 +PostProcessingDialog\reextract_features=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\refine_lc=false +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_epsilon=0.0001 +PostProcessingDialog\sba_inlier_distance=0.05 +PostProcessingDialog\sba_min_inliers=10 +graphicsView_graphView\node_radius=0.00999999977648258 +graphicsView_graphView\link_width=0 +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\grid_visible=true +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +General\loggerPrintThreadId=false +General\notifyNewGlobalPath=false +General\odomQualityThr=50 +General\posteriorGraphView=true +General\showFeatures0=false +General\downsamplingScan0=1 +General\voxelSizeScan0=0 +General\ptSizeFeatures0=3 +General\showFeatures1=true +General\downsamplingScan1=1 +General\voxelSizeScan1=0 +General\ptSizeFeatures1=3 +General\showGraphs=true +General\showLabels=false +General\noFiltering=true +General\subtractFiltering=false +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\subtractFilteringAngle=45 +General\gridMapShown=false +General\gridMapResolution=0.05 +General\gridMapOccupancyFrom3DCloud=false +General\gridMapEroded=false +General\gridMapOpacity=0.75 +General\meshing=false +General\meshing_angle=15 +General\meshing_quad=false +General\meshing_triangle_size=2 +Figures\counts=1 +Figures\curves=Loop/Highest_hypothesis_value/ From f0d91c5eac92befcefa30df1395361d7768aee7b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 17:23:22 -0500 Subject: [PATCH 069/119] Updated data_recorder.launch --- launch/data_recorder.launch | 4 ++-- launch/demo/demo_data_recorder.launch | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/launch/data_recorder.launch b/launch/data_recorder.launch index df31c343..4963d293 100644 --- a/launch/data_recorder.launch +++ b/launch/data_recorder.launch @@ -4,7 +4,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/launch/demo/demo_data_recorder.launch b/launch/demo/demo_data_recorder.launch index a2a93fb3..e0c3cc4e 100644 --- a/launch/demo/demo_data_recorder.launch +++ b/launch/demo/demo_data_recorder.launch @@ -10,7 +10,7 @@ - + @@ -20,7 +20,6 @@ - From f26c8b55eb0b48b8b5de48a0632e9811e47ff6b2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 20:16:52 -0500 Subject: [PATCH 070/119] Updated Info msg: changed localLoopClosureId for proximityDetectionId. point_cloud_xyzrgb nodelet: handling bgr and rgb encoding. Updated demo_find_object.launch with 0.11. MapsManager: Fixed grids generated with only one point. --- launch/demo/demo_find_object.launch | 54 +++++++++++++++++++---------- msg/Info.msg | 2 +- src/MapsManager.cpp | 14 +++++++- src/MapsManager.h | 1 + src/MsgConversion.cpp | 4 +-- src/nodelets/point_cloud_xyzrgb.cpp | 16 ++++++++- src/rviz/InfoDisplay.cpp | 14 ++++---- 7 files changed, 75 insertions(+), 30 deletions(-) diff --git a/launch/demo/demo_find_object.launch b/launch/demo/demo_find_object.launch index 04a69264..f48c7a5f 100644 --- a/launch/demo/demo_find_object.launch +++ b/launch/demo/demo_find_object.launch @@ -1,8 +1,12 @@ - + + + + + @@ -10,7 +14,7 @@ - + @@ -25,24 +29,39 @@ - - - - - - - - - - - - + + + + + + + + + + - + + + + + + + + + + + + + + + + + + @@ -51,10 +70,9 @@ --> - + - - + diff --git a/msg/Info.msg b/msg/Info.msg index 8df59454..ef233374 100644 --- a/msg/Info.msg +++ b/msg/Info.msg @@ -7,7 +7,7 @@ Header header int32 refId int32 loopClosureId -int32 localLoopClosureId +int32 proximityDetectionId geometry_msgs/Transform loopClosureTransform diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index c34c99d0..f0046bee 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -39,6 +39,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : cloudFrustumCulling_(false), cloudNoiseFilteringRadius_(0.0), cloudNoiseFilteringMinNeighbors_(5), + scanDecimation_(0), scanVoxelSize_(0.0), scanOutputVoxelized_(false), projMaxGroundAngle_(45.0), // degrees @@ -48,6 +49,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : gridSize_(0), // meters gridEroded_(false), gridUnknownSpaceFilled_(false), + gridMaxUnknownSpaceFilledRange_(6.0), mapFilterRadius_(0.5), mapFilterAngle_(30.0), // degrees mapCacheCleanup_(true) @@ -89,6 +91,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); + pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_); // common map stuff pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); @@ -358,6 +361,7 @@ std::map MapsManager::updateMapCaches( { scan = util3d::downsample(scan, scanDecimation_); } + if(scanRequired || scanVoxelSize_ > 0.0) { pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); @@ -369,16 +373,24 @@ std::map MapsManager::updateMapCaches( scan = util3d::laserScan2dFromPointCloud(*scanCloud); } } + if(scanRequired) { uInsert(scans_, std::make_pair(iter->first, scanCloud)); } } } + if(gridRequired && scan.type() == CV_32FC2) { cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); + util3d::occupancy2DFromLaserScan( + scan, + ground, + obstacles, + gridCellSize_, + data.id() < 0 || gridUnknownSpaceFilled_, + data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange()); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); } } diff --git a/src/MapsManager.h b/src/MapsManager.h index 24a8df8d..48564a5f 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -85,6 +85,7 @@ private: double gridSize_; bool gridEroded_; bool gridUnknownSpaceFilled_; + double gridMaxUnknownSpaceFilledRange_; double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 7f8b7b1c..0220c1a4 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -146,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat) // rtabmap_ros::Info stat.setRefImageId(info.refId); stat.setLoopClosureId(info.loopClosureId); - stat.setLocalLoopClosureId(info.localLoopClosureId); + stat.setProximityDetectionId(info.proximityDetectionId); stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform)); @@ -190,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info) { info.refId = stats.refImageId(); info.loopClosureId = stats.loopClosureId(); - info.localLoopClosureId = stats.localLoopClosureId(); + info.proximityDetectionId = stats.proximityDetectionId(); rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform); diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index fd84f48b..578714b7 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -173,7 +173,21 @@ private: if(cloudPub_.getNumSubscribers()) { - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); + cv_bridge::CvImageConstPtr imagePtr; + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); image_geometry::PinholeCameraModel model; diff --git a/src/rviz/InfoDisplay.cpp b/src/rviz/InfoDisplay.cpp index bbe6e052..d4da9683 100644 --- a/src/rviz/InfoDisplay.cpp +++ b/src/rviz/InfoDisplay.cpp @@ -51,8 +51,8 @@ void InfoDisplay::onInitialize() this->setStatusStd(rviz::StatusProperty::Ok, "Info", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); - this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0"); - this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0"); + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0"); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0"); spinner_.start(); } @@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg ) boost::mutex::scoped_lock lock(info_mutex_); if(msg->loopClosureId) { - info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId); + info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId); globalCount_ += 1; } - else if(msg->localLoopClosureId) + else if(msg->proximityDetectionId) { - info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId); + info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId); localCount_ += 1; } else @@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt ) this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString()); } - this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString()); - this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString()); + this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString()); for(std::map::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter) { From 10a40b6c86024ac8e959c62c81cfc8cae4c8c436 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 20:21:13 -0500 Subject: [PATCH 071/119] Updated demo_hector_mapping.launch for 0.11 --- launch/demo/demo_hector_mapping.launch | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch index e9a72bcd..56c8fc4d 100644 --- a/launch/demo/demo_hector_mapping.launch +++ b/launch/demo/demo_hector_mapping.launch @@ -49,7 +49,7 @@ - + @@ -62,9 +62,9 @@ - - - + + + From 0ee20d596f1e6084dff9fa45e3009a6cd19c4f55 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 20:30:16 -0500 Subject: [PATCH 072/119] updated demo_multi-session_mapping.launch for 0.11 --- launch/demo/demo_multi-session_mapping.launch | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/launch/demo/demo_multi-session_mapping.launch b/launch/demo/demo_multi-session_mapping.launch index d23aa59e..42b78d54 100644 --- a/launch/demo/demo_multi-session_mapping.launch +++ b/launch/demo/demo_multi-session_mapping.launch @@ -17,7 +17,7 @@ - + @@ -32,17 +32,17 @@ - - - + + + - - - - - - + + + + + + @@ -51,13 +51,13 @@ - + - + From 43afe918e8a6e4c0fd55b82d3ea7c72bb7ef4051 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 21:32:50 -0500 Subject: [PATCH 073/119] updated demo_stereo_outdoor.launch --- launch/demo/demo_stereo_outdoor.launch | 24 ++++++------------------ 1 file changed, 6 insertions(+), 18 deletions(-) diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index fc3de495..1c3be006 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -48,17 +48,13 @@ - - - - + + - - - - + + @@ -79,20 +75,12 @@ - - - + - - - - - - + - From ea459567c6b7aa2c29fe1776b8cb35cc2866ad39 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 22:08:18 -0500 Subject: [PATCH 074/119] Updated demo_turtlebot_mapping.launch for 0.11 --- launch/demo/demo_turtlebot_mapping.launch | 34 +++++++++++------------ 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch index 81d8b6c2..03a79bda 100644 --- a/launch/demo/demo_turtlebot_mapping.launch +++ b/launch/demo/demo_turtlebot_mapping.launch @@ -25,9 +25,13 @@ - - + + + @@ -42,7 +46,7 @@ - + @@ -51,17 +55,16 @@ - - + - + - - - - + + + + @@ -77,22 +80,17 @@ - - + + - - - - - - + From 7debcc43715cfc51af6d13da578d9c6333a6aa4f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 11 Mar 2016 23:28:23 -0500 Subject: [PATCH 075/119] Updated demo_two_kinects.launch for 0.11 --- launch/demo/demo_two_kinects.launch | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/launch/demo/demo_two_kinects.launch b/launch/demo/demo_two_kinects.launch index 63d77cc7..e08f430e 100644 --- a/launch/demo/demo_two_kinects.launch +++ b/launch/demo/demo_two_kinects.launch @@ -26,7 +26,7 @@ From cfcc4b57a3555701e17fffafb13e9912ea5f1b49 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Mar 2016 17:01:48 -0400 Subject: [PATCH 076/119] Fixed large number of stereo correspondences rejected (thus disabling loop closure) because the left/right images were not cloned by the ROS wrapper --- launch/demo/demo_stereo_outdoor.launch | 46 +++++++++++++------------- src/CoreWrapper.cpp | 8 ++--- src/StereoOdometryNode.cpp | 4 +-- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index 1c3be006..014d6b1c 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -36,28 +36,28 @@ - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + @@ -81,7 +81,7 @@ - + @@ -94,7 +94,7 @@ - + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 1b55634c..ebdf6daa 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -1080,17 +1080,17 @@ void CoreWrapper::commonStereoCallback( scan = util3d::laserScanFromPointCloud(*pclScan); } - cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; + cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) { - ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8"); + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); } else { - ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8"); + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); } - ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8"); + ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index e138a634..55d4a952 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -175,8 +175,8 @@ public: } } - cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8"); - cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8"); + cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8"); + cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8"); UTimer stepTimer; // From 238e78bae0844ef5f20f066ed8b12ae87ad513b6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Mar 2016 18:05:27 -0400 Subject: [PATCH 077/119] Cleanup launch files --- .../demo/demo_appearance_localization.launch | 47 ----------- launch/demo/demo_appearance_mapping.launch | 45 ++++++---- launch/demo/demo_find_object.launch | 64 +++++++------- launch/demo/demo_hector_mapping.launch | 28 ++++--- launch/demo/demo_multi-session_mapping.launch | 63 +++++++------- launch/demo/demo_robot_localization.launch | 83 ------------------- launch/demo/demo_robot_mapping.launch | 56 ++++++++----- launch/demo/demo_stereo_outdoor.launch | 57 ++++++------- launch/demo/demo_turtlebot_mapping.launch | 19 +++-- launch/rgbd_mapping.launch | 70 +++++++--------- launch/rtabmap.launch | 27 +++--- launch/stereo_mapping.launch | 64 +++++++------- 12 files changed, 257 insertions(+), 366 deletions(-) delete mode 100644 launch/demo/demo_appearance_localization.launch delete mode 100644 launch/demo/demo_robot_localization.launch diff --git a/launch/demo/demo_appearance_localization.launch b/launch/demo/demo_appearance_localization.launch deleted file mode 100644 index 97b6949f..00000000 --- a/launch/demo/demo_appearance_localization.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/demo/demo_appearance_mapping.launch b/launch/demo/demo_appearance_mapping.launch index 78f5f102..5a6d555c 100644 --- a/launch/demo/demo_appearance_mapping.launch +++ b/launch/demo/demo_appearance_mapping.launch @@ -4,27 +4,35 @@ + + + + + - + - + - + - - - - - - - - - - + + + + + + + + + + + + + @@ -40,11 +48,12 @@ - + + - - - - + + + + diff --git a/launch/demo/demo_find_object.launch b/launch/demo/demo_find_object.launch index f48c7a5f..abe1d058 100644 --- a/launch/demo/demo_find_object.launch +++ b/launch/demo/demo_find_object.launch @@ -19,46 +19,48 @@ - - + + - + - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - + + - - + + - - + + - + @@ -78,7 +80,7 @@ - + @@ -87,16 +89,16 @@ - - + + - + - - + + - + diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch index 56c8fc4d..40c8ba56 100644 --- a/launch/demo/demo_hector_mapping.launch +++ b/launch/demo/demo_hector_mapping.launch @@ -49,37 +49,39 @@ - + - - + + - - + + + + - + - + - - + + - - + + - + @@ -93,7 +95,7 @@ - + diff --git a/launch/demo/demo_multi-session_mapping.launch b/launch/demo/demo_multi-session_mapping.launch index 42b78d54..cbf3fa24 100644 --- a/launch/demo/demo_multi-session_mapping.launch +++ b/launch/demo/demo_multi-session_mapping.launch @@ -17,57 +17,56 @@ - + - - + + - + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - - - + + + + - - - + + + - - + + - - + + - + @@ -81,7 +80,7 @@ - + diff --git a/launch/demo/demo_robot_localization.launch b/launch/demo/demo_robot_localization.launch deleted file mode 100644 index 23b8dbe4..00000000 --- a/launch/demo/demo_robot_localization.launch +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch index e09b96dd..0cbd6e56 100644 --- a/launch/demo/demo_robot_mapping.launch +++ b/launch/demo/demo_robot_mapping.launch @@ -10,53 +10,63 @@ + + + + + - - + + - + - - + + - + - - - - - - - - + + + + + + + + + + + + + - - - + + + - - + + - - + + - + @@ -69,7 +79,7 @@ - + diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index 014d6b1c..00743af6 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -21,7 +21,7 @@ - + @@ -47,26 +47,26 @@ - + - + - - - - + + + + - + - + - - - + + + @@ -74,29 +74,30 @@ - - - - - - + + + + + + - + - + - - - - - + + + + + + - - - + + + diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch index 03a79bda..5b29e180 100644 --- a/launch/demo/demo_turtlebot_mapping.launch +++ b/launch/demo/demo_turtlebot_mapping.launch @@ -68,7 +68,9 @@ - + + + @@ -78,10 +80,11 @@ - - - - + + + + + @@ -89,9 +92,9 @@ - - - + + + diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index a1cd9142..98022b62 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -9,6 +9,9 @@ + + + @@ -29,27 +32,19 @@ + + + - - - - - - - - - - - - + @@ -58,54 +53,52 @@ - - + + - - - - - - - - - - + - - - - - + + + + + + + - - - - + + + + + - - - - + + + + + + + - + + @@ -114,6 +107,7 @@ + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 45089f93..2fc365d3 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -54,6 +54,9 @@ + + + @@ -110,15 +113,16 @@ - - - - + + + + + - - + + - + @@ -130,6 +134,7 @@ + @@ -137,11 +142,12 @@ - + + - - + + @@ -153,6 +159,7 @@ + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index 708658d6..d55b810e 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -9,6 +9,9 @@ + + + @@ -32,23 +35,15 @@ + + + - - - - - - - - - - - @@ -66,56 +61,54 @@ - - - - - - - - - - - - - + + + + + - - + + + - - - - + + + + + - - - - + + + + + + + - + + @@ -125,6 +118,7 @@ + From 68e630089c8b1e561a97476624a492361f34f59d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 13 Mar 2016 18:12:13 -0400 Subject: [PATCH 078/119] Updated demo_stereo_outdoor.rviz --- launch/config/demo_stereo_outdoor.rviz | 66 ++++++++++++++++++++++++-- 1 file changed, 62 insertions(+), 4 deletions(-) diff --git a/launch/config/demo_stereo_outdoor.rviz b/launch/config/demo_stereo_outdoor.rviz index 8c27f14d..ce7efce9 100644 --- a/launch/config/demo_stereo_outdoor.rviz +++ b/launch/config/demo_stereo_outdoor.rviz @@ -6,7 +6,6 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /Info1 Splitter Ratio: 0.5 Tree Height: 438 - Class: rviz/Selection @@ -134,6 +133,7 @@ Visualization Manager: Download graph: false Download map: false Enabled: true + Filter ceiling (m): 0 Filter floor (m): 0 Invert Rainbow: false Max Color: 255; 255; 255 @@ -159,7 +159,7 @@ Visualization Manager: Merged neighbor: 255; 170; 0 Name: MapGraph Neighbor: 0; 0; 255 - Topic: /rtabmap/mapDataGraph_optimized + Topic: /rtabmap/mapGraph User: 255; 0; 0 Value: true Virtual: 255; 0; 255 @@ -187,6 +187,64 @@ Visualization Manager: Name: Info Topic: /rtabmap/info Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.17745 + Min Value: -1.72299 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: OdomFrame + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /rtabmap/odom_last_frame + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -246,5 +304,5 @@ Window Geometry: Views: collapsed: false Width: 1341 - X: 117 - Y: 18 + X: 97 + Y: 14 From f20b55a756ffac92807d7a2374bfb352cceff8db Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 14 Mar 2016 19:37:37 -0400 Subject: [PATCH 079/119] updated demo_stereo_outdoor.launch --- launch/demo/demo_stereo_outdoor.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch index 00743af6..95877fc2 100644 --- a/launch/demo/demo_stereo_outdoor.launch +++ b/launch/demo/demo_stereo_outdoor.launch @@ -53,6 +53,7 @@ + From 99ab4051406609505070bad9c33a6fc399f40325 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 15 Mar 2016 17:04:45 -0400 Subject: [PATCH 080/119] MapCloud rviz plugin: fixed memory increasing issue on localization mode --- src/rviz/MapCloudDisplay.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index f2424976..d6206aef 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -256,11 +256,18 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg ) void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) { + std::map poses; + for(unsigned int i=0; i poses; - for(unsigned int i=0; igetFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) { poses = rtabmap::graph::radiusPosesFiltering(poses, From f80b48bee9a82c6e938ba7a802352c8c5094c3c6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 15 Mar 2016 17:06:05 -0400 Subject: [PATCH 081/119] Updated az3_mapping_robot_nav.launch for 0.11. Added az3_mapping_robot_nav_kinect-only.launch. --- launch/azimut3/az3_mapping_robot_nav.launch | 89 +++++----- .../az3_mapping_robot_nav_kinect-only.launch | 161 ++++++++++++++++++ 2 files changed, 206 insertions(+), 44 deletions(-) create mode 100644 launch/azimut3/az3_mapping_robot_nav_kinect-only.launch diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch index 01c8dc9d..4628686a 100644 --- a/launch/azimut3/az3_mapping_robot_nav.launch +++ b/launch/azimut3/az3_mapping_robot_nav.launch @@ -1,8 +1,10 @@ - - + + + + @@ -13,61 +15,61 @@ - - + + - - - + + + - - + + - - + + - + - + - - + + - + - - - + + + - - - - + + - - - + + + - - + - - - - - + + + + - - - - + + + + + + + + @@ -86,10 +88,9 @@ - - + @@ -130,7 +131,7 @@ - + @@ -139,10 +140,10 @@ - - - - + + + + diff --git a/launch/azimut3/az3_mapping_robot_nav_kinect-only.launch b/launch/azimut3/az3_mapping_robot_nav_kinect-only.launch new file mode 100644 index 00000000..eb021858 --- /dev/null +++ b/launch/azimut3/az3_mapping_robot_nav_kinect-only.launch @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From dfa567916de81b3908ba5c07d147b1caff64f347 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 16 Mar 2016 15:11:03 -0400 Subject: [PATCH 082/119] Added az3 nav kinect-only launch --- ...apping_robot_nav.launch => az3_nav.launch} | 0 ...lient_nav.launch => az3_nav_client.launch} | 0 launch/azimut3/az3_nav_kinect-only.launch | 38 +++++++++++++++++++ ...only.launch => az3_nav_kinect_odom.launch} | 2 +- 4 files changed, 39 insertions(+), 1 deletion(-) rename launch/azimut3/{az3_mapping_robot_nav.launch => az3_nav.launch} (100%) rename launch/azimut3/{az3_mapping_client_nav.launch => az3_nav_client.launch} (100%) create mode 100644 launch/azimut3/az3_nav_kinect-only.launch rename launch/azimut3/{az3_mapping_robot_nav_kinect-only.launch => az3_nav_kinect_odom.launch} (99%) diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_nav.launch similarity index 100% rename from launch/azimut3/az3_mapping_robot_nav.launch rename to launch/azimut3/az3_nav.launch diff --git a/launch/azimut3/az3_mapping_client_nav.launch b/launch/azimut3/az3_nav_client.launch similarity index 100% rename from launch/azimut3/az3_mapping_client_nav.launch rename to launch/azimut3/az3_nav_client.launch diff --git a/launch/azimut3/az3_nav_kinect-only.launch b/launch/azimut3/az3_nav_kinect-only.launch new file mode 100644 index 00000000..ead33f63 --- /dev/null +++ b/launch/azimut3/az3_nav_kinect-only.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/azimut3/az3_mapping_robot_nav_kinect-only.launch b/launch/azimut3/az3_nav_kinect_odom.launch similarity index 99% rename from launch/azimut3/az3_mapping_robot_nav_kinect-only.launch rename to launch/azimut3/az3_nav_kinect_odom.launch index eb021858..b075e309 100644 --- a/launch/azimut3/az3_mapping_robot_nav_kinect-only.launch +++ b/launch/azimut3/az3_nav_kinect_odom.launch @@ -158,4 +158,4 @@ - + \ No newline at end of file From a91a0e0dc8f5434808412aafb100eb542e894235 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 16 Mar 2016 16:24:29 -0400 Subject: [PATCH 083/119] updated az3_nav_kinect-only.launch --- launch/azimut3/az3_nav_kinect-only.launch | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/launch/azimut3/az3_nav_kinect-only.launch b/launch/azimut3/az3_nav_kinect-only.launch index ead33f63..402d5087 100644 --- a/launch/azimut3/az3_nav_kinect-only.launch +++ b/launch/azimut3/az3_nav_kinect-only.launch @@ -27,8 +27,10 @@ - - + + + + From 91c83cc396d7b07e0772b0d01cb3897658ce4d51 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Mar 2016 14:41:35 -0400 Subject: [PATCH 084/119] updated rtabmap.launch --- launch/azimut3/az3_nav_kinect_odom.launch | 4 ++-- launch/rtabmap.launch | 5 +++-- src/CoreWrapper.cpp | 3 ++- src/GuiNode.cpp | 1 + src/GuiWrapper.cpp | 3 ++- 5 files changed, 10 insertions(+), 6 deletions(-) diff --git a/launch/azimut3/az3_nav_kinect_odom.launch b/launch/azimut3/az3_nav_kinect_odom.launch index b075e309..ad6b7f7f 100644 --- a/launch/azimut3/az3_nav_kinect_odom.launch +++ b/launch/azimut3/az3_nav_kinect_odom.launch @@ -60,7 +60,7 @@ - + @@ -158,4 +158,4 @@ - \ No newline at end of file + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 2fc365d3..f96326b5 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -23,7 +23,7 @@ - + @@ -60,6 +60,7 @@ + @@ -79,7 +80,7 @@ - + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index ebdf6daa..f98a6e3c 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -726,7 +726,8 @@ Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std:: //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) { - ROS_WARN("rtabmap: Could not get transform from %s to %s after %f second!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_); + ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)!", + fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); return transform; } } diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index 8ac60d89..c0d82755 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include void my_handler(int s){ + ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app..."); QApplication::exit(); } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index cc4417a5..5da97c1c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -493,7 +493,8 @@ Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::s //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_))) { - ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_); + ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)!", + fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec()); return transform; } } From 9096c3d1d04f989b9f1257effd46a1e1e0ec1582 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Mar 2016 16:15:49 -0400 Subject: [PATCH 085/119] updated rtabmap.launch --- launch/rtabmap.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index f96326b5..e972a1db 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -24,6 +24,7 @@ + @@ -140,7 +141,7 @@ - + From 9c2e87ea380ef50b90740470a8fa7d2dd3ed0ca0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Mar 2016 16:36:44 -0400 Subject: [PATCH 086/119] Fixed rtabmapviz exit without hard kill --- src/GuiNode.cpp | 13 +++++++++++-- src/GuiWrapper.cpp | 9 --------- src/GuiWrapper.h | 3 --- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index c0d82755..8e94f874 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -33,9 +33,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +QApplication * app = 0; + void my_handler(int s){ ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app..."); - QApplication::exit(); + if(app) + { + QMetaObject::invokeMethod(app, "quit"); + } } int main(int argc, char** argv) @@ -47,6 +52,9 @@ int main(int argc, char** argv) ros::init(argc, argv, "rtabmapviz"); + app = new QApplication(argc, argv); + app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); + GuiWrapper gui(argc, argv); // Catch ctrl-c to close the gui @@ -63,10 +71,11 @@ int main(int argc, char** argv) ROS_INFO("rtabmapviz started."); // Now wait for application to finish - int r = gui.exec();// MUST be called by the Main Thread + int r = app->exec();// MUST be called by the Main Thread spinner.stop(); + delete app; ROS_INFO("rtabmapviz: All done! Closing..."); return r; } diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 5da97c1c..87c408b5 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -68,7 +68,6 @@ float max3( const float& a, const float& b, const float& c) } GuiWrapper::GuiWrapper(int & argc, char** argv) : - app_(0), mainWindow_(0), frameId_("base_link"), waitForTransform_(true), @@ -85,7 +84,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : depthOdomInfo2Sync_(0) { ros::NodeHandle nh; - app_ = new QApplication(argc, argv); QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini"; for(int i=1; isetMonitoringState(paused); - app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) ); ros::NodeHandle pnh("~"); @@ -249,12 +246,6 @@ GuiWrapper::~GuiWrapper() delete infoMapSync_; delete mainWindow_; - delete app_; -} - -int GuiWrapper::exec() -{ - return app_->exec(); } void GuiWrapper::infoMapCallback( diff --git a/src/GuiWrapper.h b/src/GuiWrapper.h index 5eabe6d5..6f39d2d0 100644 --- a/src/GuiWrapper.h +++ b/src/GuiWrapper.h @@ -68,8 +68,6 @@ public: GuiWrapper(int & argc, char** argv); virtual ~GuiWrapper(); - int exec(); - protected: virtual void handleEvent(UEvent * anEvent); @@ -263,7 +261,6 @@ private: rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; private: - QApplication * app_; rtabmap::MainWindow * mainWindow_; std::string cameraNodeName_; double lastOdomInfoUpdateTime_; From 8391a21e75af94a3920d709d1cd9cc95ba7db6a9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Mar 2016 17:14:43 -0400 Subject: [PATCH 087/119] Updated default wait_for_transform to 0.2 s --- launch/rgbd_mapping.launch | 2 +- launch/rtabmap.launch | 2 +- launch/stereo_mapping.launch | 2 +- src/CoreWrapper.cpp | 2 +- src/GuiNode.cpp | 17 +++++++++-------- src/GuiWrapper.cpp | 3 ++- 6 files changed, 15 insertions(+), 13 deletions(-) diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch index 98022b62..2b0a43fc 100644 --- a/launch/rgbd_mapping.launch +++ b/launch/rgbd_mapping.launch @@ -39,7 +39,7 @@ - + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index e972a1db..7fab41d8 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -31,7 +31,7 @@ - + diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch index d55b810e..f5d8ad5a 100644 --- a/launch/stereo_mapping.launch +++ b/launch/stereo_mapping.launch @@ -42,7 +42,7 @@ - + diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index f98a6e3c..03ac463a 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -85,7 +85,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) configPath_(""), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), waitForTransform_(true), - waitForTransformDuration_(0.1), // 100 ms + waitForTransformDuration_(0.2), // 200 ms useActionForGoal_(false), genScan_(false), genScanMaxDepth_(4.0), diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp index 8e94f874..c506e314 100644 --- a/src/GuiNode.cpp +++ b/src/GuiNode.cpp @@ -34,13 +34,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include QApplication * app = 0; +ros::AsyncSpinner * spinner = 0; void my_handler(int s){ ROS_INFO("rtabmapviz: ctrl-c catched! Exiting Qt app..."); - if(app) - { - QMetaObject::invokeMethod(app, "quit"); - } + spinner->stop(); + exit(-1); } int main(int argc, char** argv) @@ -55,7 +54,7 @@ int main(int argc, char** argv) app = new QApplication(argc, argv); app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); - GuiWrapper gui(argc, argv); + GuiWrapper * gui = new GuiWrapper(argc, argv); // Catch ctrl-c to close the gui // (Place this after QApplication's constructor) @@ -66,15 +65,17 @@ int main(int argc, char** argv) sigaction(SIGINT, &sigIntHandler, NULL); // Here start the ROS events loop - ros::AsyncSpinner spinner(4); // Use 4 threads - spinner.start(); + spinner = new ros::AsyncSpinner(1); // Use 1 thread + spinner->start(); ROS_INFO("rtabmapviz started."); // Now wait for application to finish int r = app->exec();// MUST be called by the Main Thread - spinner.stop(); + spinner->stop(); + delete spinner; + delete gui; delete app; ROS_INFO("rtabmapviz: All done! Closing..."); return r; diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 87c408b5..eeee9034 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -71,7 +71,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : mainWindow_(0), frameId_("base_link"), waitForTransform_(true), - waitForTransformDuration_(0.1), // 100 ms + waitForTransformDuration_(0.2), // 200 ms cameraNodeName_(""), lastOdomInfoUpdateTime_(0), depthScanSync_(0), @@ -211,6 +211,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : GuiWrapper::~GuiWrapper() { + UDEBUG(""); if(depthSync_) delete depthSync_; if(depth2Sync_) From 81096a3f15918e2c4dd8239c32d66b3dd6ba4f98 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 17 Mar 2016 18:20:32 -0400 Subject: [PATCH 088/119] Updated data_player to use database rate by default --- src/DbPlayerNode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp index de4fab77..6bbf2e56 100644 --- a/src/DbPlayerNode.cpp +++ b/src/DbPlayerNode.cpp @@ -90,7 +90,7 @@ int main(int argc, char** argv) std::string odomFrameId = "odom"; std::string cameraFrameId = "camera_optical_link"; std::string scanFrameId = "base_laser_link"; - double rate = 1.0f; + double rate = -1.0f; std::string databasePath = ""; bool publishTf = true; int startId = 0; From 19225a66be982ddfc9622d2e9089026620e73118 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Mar 2016 10:09:30 -0400 Subject: [PATCH 089/119] Updated camera_info conversion for odometry nodes (https://github.com/introlab/rtabmap/issues/64) --- src/RGBDOdometryNode.cpp | 18 ++---------------- src/StereoOdometryNode.cpp | 19 +++++-------------- 2 files changed, 7 insertions(+), 30 deletions(-) diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index abd08b0c..c1c07958 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -189,14 +189,7 @@ public: if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - rtabmap::CameraModel rtabmapModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform); + rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform); cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); @@ -321,14 +314,7 @@ public: return; } - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*infoMsgs[i]); - cameraModels.push_back(rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform)); + cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform)); } rtabmap::SensorData data( diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index 55d4a952..28f0dcac 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -145,24 +145,15 @@ public: int quality = -1; if(imageRectLeft->data.size() && imageRectRight->data.size()) { - image_geometry::StereoCameraModel model; - model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); - if(model.baseline() <= 0) + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform); + if(stereoModel.baseline() <= 0) { ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " - "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline()); + "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); return; } - rtabmap::StereoCameraModel stereoModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform); - - if(model.baseline() > 10.0) + if(stereoModel.baseline() > 10.0) { static bool shown = false; if(!shown) @@ -170,7 +161,7 @@ public: ROS_WARN("Detected baseline (%f m) is quite large! Is your " "right camera_info P(0,3) correctly set? Note that " "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - model.baseline()); + stereoModel.baseline()); shown = true; } } From 4100c38c46157954e5f829463b4391f530c2f5d2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 31 Mar 2016 14:11:02 -0400 Subject: [PATCH 090/119] MapsManager: Added parameter "map_negative_poses_ignored" (default false) --- src/MapsManager.cpp | 19 ++++++++++++++++++- src/MapsManager.h | 1 + 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index f0046bee..d714bb45 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -52,7 +52,8 @@ MapsManager::MapsManager(bool usePublicNamespace) : gridMaxUnknownSpaceFilledRange_(6.0), mapFilterRadius_(0.5), mapFilterAngle_(30.0), // degrees - mapCacheCleanup_(true) + mapCacheCleanup_(true), + negativePosesIgnored(false) { ros::NodeHandle nh; @@ -97,6 +98,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_); pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); + pnh.param("map_negative_poses_ignored", negativePosesIgnored, negativePosesIgnored); // If true, the last message published on // the map topics will be saved and sent to new subscribers when they @@ -206,6 +208,21 @@ std::map MapsManager::updateMapCaches( filteredPoses = poses; } + if(negativePosesIgnored) + { + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();) + { + if(iter->first <= 0) + { + filteredPoses.erase(iter++); + } + else + { + ++iter; + } + } + } + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) diff --git a/src/MapsManager.h b/src/MapsManager.h index 48564a5f..5ee200fd 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -89,6 +89,7 @@ private: double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; + bool negativePosesIgnored; ros::Publisher cloudMapPub_; ros::Publisher projMapPub_; From f1636d3c7f838c9281e83620be11a18dcfbc0a12 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 8 Apr 2016 16:55:18 -0400 Subject: [PATCH 091/119] nodeDataFromROS() Fixed words3 not filled --- src/MsgConversion.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 0220c1a4..97d6ce5f 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -452,10 +452,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) std::multimap words3D; pcl::PointCloud cloud; if(msg.wordPts.data.size() && - msg.wordPts.data.size() == msg.wordIds.size()) + msg.wordPts.height*msg.wordPts.width == msg.wordIds.size()) { pcl::fromROSMsg(msg.wordPts, cloud); } + for(unsigned int i=0; i Date: Tue, 12 Apr 2016 11:51:06 -0400 Subject: [PATCH 092/119] Nodelets point_cloud_xxxxxx: added min_depth parameter (fixed #66) --- src/nodelets/obstacles_detection.cpp | 4 ++-- src/nodelets/point_cloud_xyz.cpp | 7 +++++-- src/nodelets/point_cloud_xyzrgb.cpp | 7 +++++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index c2c12f72..d13867d4 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -104,7 +104,7 @@ private: void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { - ros::Time time = ros::Time::now(); + ros::WallTime time = ros::WallTime::now(); if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0) { @@ -255,7 +255,7 @@ private: obstaclesPub_.publish(rosCloud); } - //NODELET_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec()); + //NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec()); } private: diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index dc8a6eaf..eabe22ff 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -64,6 +64,7 @@ class PointCloudXYZ : public nodelet::Nodelet public: PointCloudXYZ() : maxDepth_(0.0), + minDepth_(0.0), voxelSize_(0.0), decimation_(1), noiseFilterRadius_(0.0), @@ -100,6 +101,7 @@ private: pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); @@ -254,9 +256,9 @@ private: void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) { - if(pclCloud->size() && maxDepth_ > 0) + if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_); + pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); } if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) @@ -284,6 +286,7 @@ private: private: double maxDepth_; + double minDepth_; double voxelSize_; int decimation_; double noiseFilterRadius_; diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 578714b7..d762106f 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -64,6 +64,7 @@ class PointCloudXYZRGB : public nodelet::Nodelet public: PointCloudXYZRGB() : maxDepth_(0.0), + minDepth_(0.0), voxelSize_(0.0), decimation_(1), noiseFilterRadius_(0.0), @@ -97,6 +98,7 @@ private: pnh.param("approx_sync", approxSync, approxSync); pnh.param("queue_size", queueSize, queueSize); pnh.param("max_depth", maxDepth_, maxDepth_); + pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("decimation", decimation_, decimation_); pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_); @@ -257,9 +259,9 @@ private: void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) { - if(pclCloud->size() && maxDepth_ > 0) + if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_); + pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); } if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) @@ -287,6 +289,7 @@ private: private: double maxDepth_; + double minDepth_; double voxelSize_; int decimation_; double noiseFilterRadius_; From 63de9018dfa4e582fca2407683676f10d3ed1032 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Apr 2016 15:18:29 -0400 Subject: [PATCH 093/119] 0.11.4: updated with API changes --- CMakeLists.txt | 2 +- src/CoreWrapper.cpp | 3 +++ src/CoreWrapper.h | 1 + src/MapsManager.cpp | 18 ++++++++++++++++-- src/MapsManager.h | 1 + src/rviz/MapCloudDisplay.cpp | 15 ++++++++++++++- src/rviz/MapCloudDisplay.h | 1 + 7 files changed, 37 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ea720fb..51d41c33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(octomap_ros) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.0 REQUIRED) +find_package(RTABMap 0.11.4 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 03ac463a..b9d6303e 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) useActionForGoal_(false), genScan_(false), genScanMaxDepth_(4.0), + genScanMinDepth_(0.0), mapToOdom_(rtabmap::Transform::getIdentity()), mapsManager_(true), depthSync_(0), @@ -170,6 +171,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); pnh.param("gen_scan", genScan_, genScan_); pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_); + pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_); if(!tfPrefix.empty()) { @@ -900,6 +902,7 @@ void CoreWrapper::commonDepthCallback( cameraModels.back().cx(), cameraModels.back().cy(), genScanMaxDepth_, + genScanMinDepth_, localTransform); genMaxScanPts += subDepth.cols; } diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h index 396a7402..5c0e03b4 100644 --- a/src/CoreWrapper.h +++ b/src/CoreWrapper.h @@ -275,6 +275,7 @@ private: bool useActionForGoal_; bool genScan_; double genScanMaxDepth_; + double genScanMinDepth_; rtabmap::Transform mapToOdom_; boost::mutex mapToOdomMutex_; diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index d714bb45..6013817e 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -32,6 +32,7 @@ using namespace rtabmap; MapsManager::MapsManager(bool usePublicNamespace) : cloudDecimation_(4), cloudMaxDepth_(4.0), // meters + cloudMinDepth_(0.0), // meters cloudVoxelSize_(0.05), // meters cloudFloorCullingHeight_(0.0), cloudCeilingCullingHeight_(0.0), @@ -62,6 +63,7 @@ MapsManager::MapsManager(bool usePublicNamespace) : // cloud map stuff pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); + pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_); pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_); @@ -268,11 +270,17 @@ std::map MapsManager::updateMapCaches( { if(!image.empty() && !depth.empty()) { + pcl::IndicesPtr validIndices(new std::vector); cloudRGB = util3d::cloudRGBFromSensorData( data, cloudDecimation_, cloudMaxDepth_, - cloudVoxelSize_); + cloudMinDepth_, + validIndices.get()); + if(cloudVoxelSize_) + { + cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_); + } if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) { pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); @@ -290,11 +298,17 @@ std::map MapsManager::updateMapCaches( { if( !depth.empty()) { + pcl::IndicesPtr validIndices(new std::vector); cloudXYZ = util3d::cloudFromSensorData( data, cloudDecimation_, cloudMaxDepth_, - gridCellSize_); // use gridCellSize since this cloud is only for the projection map + cloudMinDepth_, + validIndices.get()); // use gridCellSize since this cloud is only for the projection map + if(gridCellSize_) + { + cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); + } if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) { pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); diff --git a/src/MapsManager.h b/src/MapsManager.h index 5ee200fd..3784553b 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -68,6 +68,7 @@ private: // mapping stuff int cloudDecimation_; double cloudMaxDepth_; + double cloudMinDepth_; double cloudVoxelSize_; double cloudFloorCullingHeight_; double cloudCeilingCullingHeight_; diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index d6206aef..5b81b35e 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -143,6 +143,12 @@ MapCloudDisplay::MapCloudDisplay() cloud_max_depth_->setMin( 0.0f ); cloud_max_depth_->setMax( 999.0f ); + cloud_min_depth_ = new rviz::FloatProperty( "Cloud min depth (m)", 0.0f, + "Minimum depth of the generated clouds.", + this, SLOT( updateCloudParameters() ), this ); + cloud_min_depth_->setMin( 0.0f ); + cloud_min_depth_->setMax( 999.0f ); + cloud_voxel_size_ = new rviz::FloatProperty( "Cloud voxel size (m)", 0.01f, "Voxel size of the generated clouds.", this, SLOT( updateCloudParameters() ), this ); @@ -282,11 +288,18 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map) if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty()) { pcl::PointCloud::Ptr cloud; + pcl::IndicesPtr validIndices(new std::vector); cloud = rtabmap::util3d::cloudRGBFromSensorData( s.sensorData(), cloud_decimation_->getInt(), cloud_max_depth_->getFloat(), - cloud_voxel_size_->getFloat()); + cloud_min_depth_->getFloat(), + validIndices.get()); + + if(cloud_voxel_size_->getFloat()) + { + cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat()); + } if(cloud->size()) { diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index 688ded8f..f43ff9d6 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -106,6 +106,7 @@ public: rviz::EnumProperty* style_property_; rviz::IntProperty* cloud_decimation_; rviz::FloatProperty* cloud_max_depth_; + rviz::FloatProperty* cloud_min_depth_; rviz::FloatProperty* cloud_voxel_size_; rviz::FloatProperty* cloud_filter_floor_height_; rviz::FloatProperty* cloud_filter_ceiling_height_; From 30ad691027785051e9862a0563f4f4b499bceb9f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Apr 2016 18:04:20 -0400 Subject: [PATCH 094/119] MapsManager 3D projection: adding pose rotation (roll, pitch) before projection --- src/MapsManager.cpp | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 6013817e..d7ab5fa1 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -91,6 +91,10 @@ MapsManager::MapsManager(bool usePublicNamespace) : // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m + if(gridCellSize_ <= 0) + { + ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_); + } pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); @@ -305,10 +309,8 @@ std::map MapsManager::updateMapCaches( cloudMaxDepth_, cloudMinDepth_, validIndices.get()); // use gridCellSize since this cloud is only for the projection map - if(gridCellSize_) - { - cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); - } + UASSERT(gridCellSize_ > 0); + cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) { pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); @@ -366,6 +368,14 @@ std::map MapsManager::updateMapCaches( if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) { cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); + } + if(cloudClipped->size()) + { + // add pose rotation without yaw + float roll, pitch, yaw; + iter->second.getEulerAngles(roll, pitch, yaw); + cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); } } @@ -378,6 +388,11 @@ std::map MapsManager::updateMapCaches( } if(cloudClipped->size()) { + // add pose rotation without yaw + float roll, pitch, yaw; + iter->second.getEulerAngles(roll, pitch, yaw); + cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); } } @@ -546,7 +561,7 @@ void MapsManager::publishMaps( int size = assembledCloud->size(); assembledCloud = util3d::frustumFiltering( assembledCloud, - iter->second, + iter->second, // FIXME: should include camera local transform kter->second[i].horizontalFOV(), kter->second[i].verticalFOV(), 0.0f, From fded3685af9ddef8c9343e02de91caf67d747dd4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 Apr 2016 18:58:11 -0400 Subject: [PATCH 095/119] API change: updated obstacles_detection nodelet with new segmentObstaclesFromGround() --- src/nodelets/obstacles_detection.cpp | 29 ++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index d13867d4..a0742603 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -67,8 +67,9 @@ class ObstaclesDetection : public nodelet::Nodelet public: ObstaclesDetection() : frameId_("base_link"), - normalEstimationRadius_(0.05), + normalKSearch_(20), groundNormalAngle_(M_PI_4), + clusterRadius_(0.05), minClusterSize_(20), maxObstaclesHeight_(0.0), // if<=0.0 -> disabled waitForTransform_(false), @@ -87,8 +88,20 @@ private: int queueSize = 10; pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); - pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_); + pnh.param("normal_k", normalKSearch_, normalKSearch_); pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_); + if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius")) + { + NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed " + "to \"cluster_radius\"! Your value is still copied to " + "corresponding parameter. Instead of normal radius, nearest neighbors count " + "\"normal_k\" is used instead (default 20)."); + pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_); + } + else + { + pnh.param("cluster_radius", clusterRadius_, clusterRadius_); + } pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); @@ -158,8 +171,9 @@ private: originalCloud, ground, obstacles, - normalEstimationRadius_, + normalKSearch_, groundNormalAngle_, + clusterRadius_, minClusterSize_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) @@ -190,8 +204,9 @@ private: originalCloud_near, ground, obstacles, - normalEstimationRadius_, + normalKSearch_, groundNormalAngle_, + clusterRadius_, minClusterSize_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) @@ -211,8 +226,9 @@ private: originalCloud_far, ground, obstacles, - 3.*normalEstimationRadius_, + normalKSearch_, 2.*groundNormalAngle_, + 3.*clusterRadius_, minClusterSize_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) @@ -260,8 +276,9 @@ private: private: std::string frameId_; - double normalEstimationRadius_; + int normalKSearch_; double groundNormalAngle_; + double clusterRadius_; int minClusterSize_; double maxObstaclesHeight_; bool waitForTransform_; From 157ae27e2fca0b934b755a82f43f4b06ead5f2b3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 13 Apr 2016 11:59:46 -0400 Subject: [PATCH 096/119] PreferencesDialogROS: load/save local working directory for the GUI --- src/PreferencesDialogROS.cpp | 99 ++++++++++++++++++++++++++---------- src/PreferencesDialogROS.h | 2 +- 2 files changed, 73 insertions(+), 28 deletions(-) diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index 9025697a..1707fce6 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include using namespace rtabmap; @@ -76,19 +77,42 @@ QString PreferencesDialogROS::getParamMessage() bool PreferencesDialogROS::readCoreSettings(const QString & filePath) { - if(filePath.isEmpty() || filePath.compare(getTmpIniFilePath()) == 0) + QString path = getIniFilePath(); + if(!filePath.isEmpty()) { - ros::NodeHandle nh; - ROS_INFO("%s", this->getParamMessage().toStdString().c_str()); - bool validParameters = true; - int readCount = 0; - rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) + path = filePath; + } + + ros::NodeHandle nh; + ROS_INFO("%s", this->getParamMessage().toStdString().c_str()); + bool validParameters = true; + int readCount = 0; + rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); + for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) + { + if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0) + { + // use working directory of the GUI, not the one on rosparam server + QSettings settings(path, QSettings::IniFormat); + settings.beginGroup("Core"); + QString value = settings.value(rtabmap::Parameters::kRtabmapWorkingDirectory().c_str(), "").toString(); + if(!value.isEmpty() && QDir(value).exists()) + { + this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), value.toStdString()); + } + else + { + // use default one + this->setParameter(rtabmap::Parameters::kRtabmapWorkingDirectory(), (QDir::homePath()+"/.ros").toStdString()); + } + settings.endGroup(); + } + else { std::string value; - if(nh.getParam((*i).first,value)) + if(nh.getParam(i->first,value)) { - PreferencesDialog::setParameter((*i).first, value); + PreferencesDialog::setParameter(i->first, value); ++readCount; } else @@ -96,29 +120,50 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) validParameters = false; } } + } - ROS_INFO("Parameters read = %d", readCount); + ROS_INFO("Parameters read = %d", readCount); - if(validParameters) - { - ROS_INFO("Parameters successfully read."); - } - else - { - if(this->isVisible()) - { - QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); - ROS_WARN("%s", warning.toStdString().c_str()); - QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); - } - return false; - } - return true; + if(validParameters) + { + ROS_INFO("Parameters successfully read."); } else { - return PreferencesDialog::readCoreSettings(filePath); + if(this->isVisible()) + { + QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work..."); + ROS_WARN("%s", warning.toStdString().c_str()); + QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning); + } + return false; + } + return true; +} + +void PreferencesDialogROS::writeCoreSettings(const QString & filePath) const +{ + QString path = getIniFilePath(); + if(!filePath.isEmpty()) + { + path = filePath; + } + + if(QFile::exists(path)) + { + rtabmap::ParametersMap parameters = this->getAllParameters(); + + std::string workingDir = uValue(parameters, Parameters::kRtabmapWorkingDirectory(), std::string("")); + + if(!workingDir.empty()) + { + //Just update GUI working directory + QSettings settings(path, QSettings::IniFormat); + settings.beginGroup("Core"); + settings.remove(""); + settings.setValue(Parameters::kRtabmapWorkingDirectory().c_str(), workingDir.c_str()); + settings.endGroup(); + } } - } diff --git a/src/PreferencesDialogROS.h b/src/PreferencesDialogROS.h index 18dced84..4e73f8a8 100644 --- a/src/PreferencesDialogROS.h +++ b/src/PreferencesDialogROS.h @@ -48,7 +48,7 @@ protected: virtual void readCameraSettings(const QString & filePath); virtual bool readCoreSettings(const QString & filePath); virtual void writeCameraSettings(const QString & filePath) const {} - virtual void writeCoreSettings(const QString & filePath) const {} + virtual void writeCoreSettings(const QString & filePath) const; private: QString configFile_; From d730c60342c0075d9955dae19cef6e3e0925285b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 13 Apr 2016 17:45:08 -0400 Subject: [PATCH 097/119] obstacles_detection: Added "detect_flat_obstacles" parameter (default false) --- src/nodelets/obstacles_detection.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index a0742603..08cffcb3 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -72,6 +72,7 @@ public: clusterRadius_(0.05), minClusterSize_(20), maxObstaclesHeight_(0.0), // if<=0.0 -> disabled + segmentFlatObstacles_(false), waitForTransform_(false), optimizeForCloseObjects_(false) {} @@ -104,6 +105,7 @@ private: } pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); @@ -174,7 +176,8 @@ private: normalKSearch_, groundNormalAngle_, clusterRadius_, - minClusterSize_); + minClusterSize_, + segmentFlatObstacles_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -207,7 +210,8 @@ private: normalKSearch_, groundNormalAngle_, clusterRadius_, - minClusterSize_); + minClusterSize_, + segmentFlatObstacles_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -229,7 +233,8 @@ private: normalKSearch_, 2.*groundNormalAngle_, 3.*clusterRadius_, - minClusterSize_); + minClusterSize_, + segmentFlatObstacles_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -281,6 +286,7 @@ private: double clusterRadius_; int minClusterSize_; double maxObstaclesHeight_; + bool segmentFlatObstacles_; bool waitForTransform_; bool optimizeForCloseObjects_; From 7e5c7d74621a0f5b785213d44791913fa73a1eaa Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Fri, 15 Apr 2016 17:44:36 -0400 Subject: [PATCH 098/119] Added max ground height parameter for MapsManagerand obstacles_detection --- src/MapsManager.cpp | 37 ++++++++++++++++++++++------ src/MapsManager.h | 4 ++- src/nodelets/obstacles_detection.cpp | 12 ++++++--- 3 files changed, 41 insertions(+), 12 deletions(-) diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index d7ab5fa1..d00b339d 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -45,7 +45,9 @@ MapsManager::MapsManager(bool usePublicNamespace) : scanOutputVoxelized_(false), projMaxGroundAngle_(45.0), // degrees projMinClusterSize_(20), - projMaxHeight_(2.0), // meters + projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) + projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) + projDetectFlatObstacles_(false), gridCellSize_(0.05), // meters gridSize_(0), // meters gridEroded_(false), @@ -87,7 +89,19 @@ MapsManager::MapsManager(bool usePublicNamespace) : //projection map stuff pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_); - pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_); + if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height")) + { + ROS_WARN("Parameter \"proj_max_height\" has been renamed " + "to \"proj_max_obstacles_height\"! Your value is still copied to " + "corresponding parameter."); + pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); + } + else + { + pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); + } + pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); + pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m @@ -194,6 +208,7 @@ std::map MapsManager::updateMapCaches( // filter nodes if(mapFilterRadius_ > 0.0) { + UDEBUG("Filter nodes..."); double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0; filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle); for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) @@ -244,6 +259,7 @@ std::map MapsManager::updateMapCaches( scanRequired || gridRequired) { + UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); if(findIter != signatures.end()) { @@ -272,6 +288,7 @@ std::map MapsManager::updateMapCaches( pcl::PointCloud::Ptr cloudXYZ; if(rgbDepthRequired) { + UDEBUG("rgbDepthRequired"); if(!image.empty() && !depth.empty()) { pcl::IndicesPtr validIndices(new std::vector); @@ -300,6 +317,7 @@ std::map MapsManager::updateMapCaches( } else if(depthRequired) { + UDEBUG("depthRequired"); if( !depth.empty()) { pcl::IndicesPtr validIndices(new std::vector); @@ -357,13 +375,14 @@ std::map MapsManager::updateMapCaches( if(depthRequired) { + UDEBUG("Creating proj map for %d...", iter->first); cv::Mat ground, obstacles; if(cloudRGB.get()) { pcl::PointCloud::Ptr cloudClipped = cloudRGB; - if(cloudClipped->size() && projMaxHeight_ > 0) + if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxHeight_); + cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); } if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) { @@ -376,15 +395,15 @@ std::map MapsManager::updateMapCaches( iter->second.getEulerAngles(roll, pitch, yaw); cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); } } else if(cloudXYZ.get()) { pcl::PointCloud::Ptr cloudClipped = cloudXYZ; - if(cloudClipped->size() && projMaxHeight_ > 0) + if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxHeight_); + cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); } if(cloudClipped->size()) { @@ -393,7 +412,8 @@ std::map MapsManager::updateMapCaches( iter->second.getEulerAngles(roll, pitch, yaw); cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); - util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_); + UDEBUG("util3d::occupancy2DFromCloud3D()"); + util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_); } } uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); @@ -458,6 +478,7 @@ std::map MapsManager::updateMapCaches( } // cleanup not used nodes + UDEBUG("Cleanup not used nodes"); for(std::map::Ptr >::iterator iter=clouds_.begin(); iter!=clouds_.end();) { diff --git a/src/MapsManager.h b/src/MapsManager.h index 3784553b..5b4adfb0 100644 --- a/src/MapsManager.h +++ b/src/MapsManager.h @@ -81,7 +81,9 @@ private: bool scanOutputVoxelized_; double projMaxGroundAngle_; int projMinClusterSize_; - double projMaxHeight_; + double projMaxObstaclesHeight_; + double projMaxGroundHeight_; + bool projDetectFlatObstacles_; double gridCellSize_; double gridSize_; bool gridEroded_; diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp index 08cffcb3..ad21b45a 100644 --- a/src/nodelets/obstacles_detection.cpp +++ b/src/nodelets/obstacles_detection.cpp @@ -72,6 +72,7 @@ public: clusterRadius_(0.05), minClusterSize_(20), maxObstaclesHeight_(0.0), // if<=0.0 -> disabled + maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true segmentFlatObstacles_(false), waitForTransform_(false), optimizeForCloseObjects_(false) @@ -105,6 +106,7 @@ private: } pnh.param("min_cluster_size", minClusterSize_, minClusterSize_); pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_); + pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_); pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_); @@ -177,7 +179,8 @@ private: groundNormalAngle_, clusterRadius_, minClusterSize_, - segmentFlatObstacles_); + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -211,7 +214,8 @@ private: groundNormalAngle_, clusterRadius_, minClusterSize_, - segmentFlatObstacles_); + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -234,7 +238,8 @@ private: 2.*groundNormalAngle_, 3.*clusterRadius_, minClusterSize_, - segmentFlatObstacles_); + segmentFlatObstacles_, + maxGroundHeight_); if(groundPub_.getNumSubscribers() && ground.get() && ground->size()) { @@ -286,6 +291,7 @@ private: double clusterRadius_; int minClusterSize_; double maxObstaclesHeight_; + double maxGroundHeight_; bool segmentFlatObstacles_; bool waitForTransform_; bool optimizeForCloseObjects_; From 0619936185e1fa53ab8e76a3977110e7230c304a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Apr 2016 10:25:37 -0400 Subject: [PATCH 099/119] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ccd5ad73..9504de96 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ source ~/catkin_ws/devel/setup.bash 0. Optional dependencies * If you want SURF/SIFT on Indigo/Jade (Hydro has already SIFT/SURF), you have to build [OpenCV]([OpenCV](http://opencv.org/)) from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS. I recommend to use latest 2.4 version ([2.4.11](https://github.com/Itseez/opencv/archive/2.4.11.zip)) and build it from source following these [instructions](http://docs.opencv.org/doc/tutorials/introduction/linux_install/linux_install.html#building-opencv-from-source-using-cmake-using-the-command-line). RTAB-Map can build with OpenCV3+[xfeatures2d](https://github.com/Itseez/opencv_contrib/tree/master/modules/xfeatures2d) module, but rtabmap_ros package will have libraries conflict as cv-bridge is depending on OpenCV2. If you want OpenCV3, you should build ros [vision-opencv](https://github.com/ros-perception/vision_opencv) package yourself (and all ros packages depending on it) so it can link on OpenCV3. - * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`). + * ROS (Qt, PCL, dc1394, OpenNI, OpenNI2, Freenect, g2o, Costmap2d, Rviz, Octomap, CvBridge). Note that I've found that [latest g2o version](https://github.com/RainerKuemmerle/g2o) built from source is faster (install `libsuitesparse-dev` before building `g2o`) and would be [required to avoid some crashes](http://official-rtab-map-forum.67519.x6.nabble.com/ROS-2D-occupancy-grid-tp1204p1215.html). ```bash $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-indigo-openni-launch ros-indigo-openni2-launch ros-indigo-freenect-launch ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-g2o ros-indigo-rviz ros-indigo-cv-bridge ``` From 6264a01694cdb0e232b39d902a1b165dfb0f3c35 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Apr 2016 13:51:56 -0400 Subject: [PATCH 100/119] Update README.md --- README.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9504de96..b7bade6b 100644 --- a/README.md +++ b/README.md @@ -31,8 +31,13 @@ This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade** * The next instructions assume that you have set up your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). I will use indigo prefix for convenience, but it should work with hydro and jade. The workspace path is `~/catkin_ws` and your `~/.bashrc` contains: ```bash -source /opt/ros/indigo/setup.bash -source ~/catkin_ws/devel/setup.bash +$ source /opt/ros/indigo/setup.bash +$ source ~/catkin_ws/devel/setup.bash +``` + + * Make sure you don't have the binaries installed too (if you tried them before): + ```bash +$ sudo apt-get remove ros-indigo-rtabmap ``` 0. Optional dependencies From 4f7e8d4b73c8084d2fb15e06b818afea6e979663 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Apr 2016 18:10:40 -0400 Subject: [PATCH 101/119] Odom: Added "publish_null_when_lost" (default true) parameter, Odom/ResetCountDown will reset to latest odom pose on TF if available --- src/OdometryROS.cpp | 39 +++++++++++++++++++++++++++++++++++---- src/OdometryROS.h | 3 +++ 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 2c23836c..0f7afa0e 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -61,7 +61,10 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms - paused_(false) + publishNullWhenLost_(true), + paused_(false), + resetCountdown_(0), + resetCurrentCount_(0) { ros::NodeHandle nh; @@ -85,6 +88,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw" pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("config_path", configPath, configPath); + pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_); + configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); if(configPath.size() && configPath.at(0) != '/') { @@ -224,8 +229,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } } - int odomStrategy = 0; // BOW - Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); + Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_); + parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { @@ -341,6 +346,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) rtabmap::Transform pose = odometry_->process(dataCpy, &info); if(!pose.isNull()) { + resetCurrentCount_ = resetCountdown_; + //********************* // Update odometry //********************* @@ -455,7 +462,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } } } - else + else if(publishNullWhenLost_) { //ROS_WARN("Odometry lost!"); @@ -469,6 +476,30 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomPub_.publish(odom); } + if(pose.isNull() && resetCurrentCount_ > 0) + { + ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); + + --resetCurrentCount_; + if(resetCurrentCount_ == 0) + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp); + if(tfPose.isNull()) + { + ROS_WARN("Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + ROS_WARN("Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + + } + } + if(odomInfoPub_.getNumSubscribers()) { rtabmap_ros::OdomInfo infoMsg; diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 795461df..bbeb2dcb 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -83,6 +83,7 @@ private: bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; + bool publishNullWhenLost_; rtabmap::ParametersMap parameters_; ros::Publisher odomPub_; @@ -101,6 +102,8 @@ private: tf::TransformListener tfListener_; bool paused_; + int resetCountdown_; + int resetCurrentCount_; ros::Time previousStamp_; }; From d4d5d9f06902e1ee8c9b35f930558506ccbc2dae Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Apr 2016 18:53:03 -0400 Subject: [PATCH 102/119] Odom: Added twist covariance (pose covariance/2) --- src/CoreWrapper.cpp | 3 ++- src/OdometryROS.cpp | 27 +++++++++++++++++++++++---- src/OdometryROS.h | 1 - 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index b9d6303e..bc095262 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#define BAD_COVARIANCE 9999 //msgs #include "rtabmap_ros/Info.h" @@ -609,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= 9999)) + if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); rtabmap_.triggerNewMap(); diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 0f7afa0e..02b36fce 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UFile.h" +#define BAD_COVARIANCE 9999 + using namespace rtabmap; namespace rtabmap_ros { @@ -385,7 +387,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.covariance.at(35) = info.variance; // yawyaw //set velocity - if(previousStamp_.isValid()) + bool setTwist = !odometry_->previousVelocityTransform().isNull(); + if(setTwist) { float x,y,z,roll,pitch,yaw; odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); @@ -396,7 +399,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.z = yaw; } - previousStamp_ = stamp; + // libviso2 uses approximately pose variance/2 + odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); @@ -471,6 +480,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.header.stamp = stamp; // use corresponding time stamp to image odom.header.frame_id = odomFrameId_; odom.child_frame_id = frameId_; + odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx + odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy + odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz + odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr + odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp + odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw + odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); @@ -521,7 +542,6 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { ROS_INFO("visual_odometry: reset odom!"); odometry_->reset(); - previousStamp_ = ros::Time(); return true; } @@ -530,7 +550,6 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros: Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw); ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); odometry_->reset(pose); - previousStamp_ = ros::Time(); return true; } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index bbeb2dcb..ec705d41 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -104,7 +104,6 @@ private: bool paused_; int resetCountdown_; int resetCurrentCount_; - ros::Time previousStamp_; }; } From e7e12c85ef50aae33587ac686c3e375f0747b96b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 20 Apr 2016 10:59:07 -0400 Subject: [PATCH 103/119] Fixed odom reset detection (new odom should not be null) --- src/CoreWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index bc095262..fc92982d 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -610,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); rtabmap_.triggerNewMap(); From 704532ce62a78f0ce5403ae519536a4fbe8c553c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 20 Apr 2016 18:15:25 -0400 Subject: [PATCH 104/119] CoreWrapper: using twist covariance instead of pose covariance (which could grow out of bounds depending of the odometry used) --- src/CoreWrapper.cpp | 8 ++++---- src/OdometryROS.cpp | 27 ++++++++++++++------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index fc92982d..ed9b1248 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -610,9 +610,9 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->twist.covariance[0] >= BAD_COVARIANCE)) { - UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->twist.covariance[0]); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; @@ -621,8 +621,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); - float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); + float transVariance = uMax3(odomMsg->twist.covariance[0], odomMsg->twist.covariance[7], odomMsg->twist.covariance[14]); + float rotVariance = uMax3(odomMsg->twist.covariance[21], odomMsg->twist.covariance[28], odomMsg->twist.covariance[35]); if(uIsFinite(rotVariance) && rotVariance > rotVariance_) { rotVariance_ = rotVariance; diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 02b36fce..a54f9102 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -379,12 +379,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.pose.orientation = poseMsg.transform.rotation; //set covariance - odom.pose.covariance.at(0) = info.variance; // xx - odom.pose.covariance.at(7) = info.variance; // yy - odom.pose.covariance.at(14) = info.variance; // zz - odom.pose.covariance.at(21) = info.variance; // rr - odom.pose.covariance.at(28) = info.variance; // pp - odom.pose.covariance.at(35) = info.variance; // yawyaw + // libviso2 uses approximately vel variance * 2 + odom.pose.covariance.at(0) = info.variance*2; // xx + odom.pose.covariance.at(7) = info.variance*2; // yy + odom.pose.covariance.at(14) = info.variance*2; // zz + odom.pose.covariance.at(21) = info.variance*2; // rr + odom.pose.covariance.at(28) = info.variance*2; // pp + odom.pose.covariance.at(35) = info.variance*2; // yawyaw //set velocity bool setTwist = !odometry_->previousVelocityTransform().isNull(); @@ -399,13 +400,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.z = yaw; } - // libviso2 uses approximately pose variance/2 - odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx - odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy - odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz - odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr - odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp - odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw + + odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); From ae8563934749251c9e0a1ae4df6df29b2b47cd94 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 25 Apr 2016 09:46:38 -0400 Subject: [PATCH 105/119] Added cameraModelToROS() --- include/rtabmap_ros/MsgConversion.h | 4 ++++ src/MsgConversion.cpp | 34 +++++++++++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 328b98a9..fd4bb016 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -95,6 +95,10 @@ void points3fToROS(const std::vector & kpts, std::vector(model.D_raw().cols); + memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double)); + + UASSERT(model.K_raw().total() == 9); + memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double)); + + UASSERT(model.R().total() == 9); + memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double)); + + UASSERT(model.P().total() == 12); + memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double)); + + if(camInfo.D.size() > 5) + { + camInfo.distortion_model = "rational_polynomial"; + } + else + { + camInfo.distortion_model = "plumb_bob"; + } + camInfo.binning_x = 1; + camInfo.binning_y = 1; + camInfo.roi.width = model.imageWidth(); + camInfo.roi.height = model.imageHeight(); + + camInfo.width = model.imageWidth(); + camInfo.height = model.imageHeight(); +} rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, From 446026a5f47743fd63f9d99e3a80cbe3a346deb3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 Apr 2016 10:17:03 -0400 Subject: [PATCH 106/119] Fixed rviz links error --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 51d41c33..4f05e21b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,6 +125,7 @@ SET(Libraries ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${RTABMap_LIBRARIES} + ${rviz_DEFAULT_PLUGIN_LIBRARIES} ) ## RVIZ plugin From e18365e28f40c15fe17a183be530575faca7932b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Apr 2016 18:10:40 -0400 Subject: [PATCH 107/119] Odom: Added "publish_null_when_lost" (default true) parameter, Odom/ResetCountDown will reset to latest odom pose on TF if available --- src/OdometryROS.cpp | 39 +++++++++++++++++++++++++++++++++++---- src/OdometryROS.h | 3 +++ 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 2c23836c..0f7afa0e 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -61,7 +61,10 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms - paused_(false) + publishNullWhenLost_(true), + paused_(false), + resetCountdown_(0), + resetCurrentCount_(0) { ros::NodeHandle nh; @@ -85,6 +88,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw" pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("config_path", configPath, configPath); + pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_); + configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); if(configPath.size() && configPath.at(0) != '/') { @@ -224,8 +229,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : } } - int odomStrategy = 0; // BOW - Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); + Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_); + parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { @@ -341,6 +346,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) rtabmap::Transform pose = odometry_->process(dataCpy, &info); if(!pose.isNull()) { + resetCurrentCount_ = resetCountdown_; + //********************* // Update odometry //********************* @@ -455,7 +462,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) } } } - else + else if(publishNullWhenLost_) { //ROS_WARN("Odometry lost!"); @@ -469,6 +476,30 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odomPub_.publish(odom); } + if(pose.isNull() && resetCurrentCount_ > 0) + { + ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); + + --resetCurrentCount_; + if(resetCurrentCount_ == 0) + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp); + if(tfPose.isNull()) + { + ROS_WARN("Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + ROS_WARN("Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + + } + } + if(odomInfoPub_.getNumSubscribers()) { rtabmap_ros::OdomInfo infoMsg; diff --git a/src/OdometryROS.h b/src/OdometryROS.h index 795461df..bbeb2dcb 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -83,6 +83,7 @@ private: bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; + bool publishNullWhenLost_; rtabmap::ParametersMap parameters_; ros::Publisher odomPub_; @@ -101,6 +102,8 @@ private: tf::TransformListener tfListener_; bool paused_; + int resetCountdown_; + int resetCurrentCount_; ros::Time previousStamp_; }; From 3f6811b156a1db80d7273a6f6ba06dac95b9303b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Apr 2016 18:53:03 -0400 Subject: [PATCH 108/119] Odom: Added twist covariance (pose covariance/2) --- src/CoreWrapper.cpp | 3 ++- src/OdometryROS.cpp | 27 +++++++++++++++++++++++---- src/OdometryROS.h | 1 - 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index b9d6303e..bc095262 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#define BAD_COVARIANCE 9999 //msgs #include "rtabmap_ros/Info.h" @@ -609,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= 9999)) + if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); rtabmap_.triggerNewMap(); diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 0f7afa0e..02b36fce 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UFile.h" +#define BAD_COVARIANCE 9999 + using namespace rtabmap; namespace rtabmap_ros { @@ -385,7 +387,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.covariance.at(35) = info.variance; // yawyaw //set velocity - if(previousStamp_.isValid()) + bool setTwist = !odometry_->previousVelocityTransform().isNull(); + if(setTwist) { float x,y,z,roll,pitch,yaw; odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); @@ -396,7 +399,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.z = yaw; } - previousStamp_ = stamp; + // libviso2 uses approximately pose variance/2 + odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); @@ -471,6 +480,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.header.stamp = stamp; // use corresponding time stamp to image odom.header.frame_id = odomFrameId_; odom.child_frame_id = frameId_; + odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx + odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy + odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz + odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr + odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp + odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw + odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); @@ -521,7 +542,6 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { ROS_INFO("visual_odometry: reset odom!"); odometry_->reset(); - previousStamp_ = ros::Time(); return true; } @@ -530,7 +550,6 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros: Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw); ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); odometry_->reset(pose); - previousStamp_ = ros::Time(); return true; } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index bbeb2dcb..ec705d41 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -104,7 +104,6 @@ private: bool paused_; int resetCountdown_; int resetCurrentCount_; - ros::Time previousStamp_; }; } From af89e0c00978426f07fddbb9475584c12343f460 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 20 Apr 2016 10:59:07 -0400 Subject: [PATCH 109/119] Fixed odom reset detection (new odom should not be null) --- src/CoreWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index bc095262..fc92982d 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -610,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); rtabmap_.triggerNewMap(); From 76cb4365dd3b7e189325e991c36241c8d6522736 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 20 Apr 2016 18:15:25 -0400 Subject: [PATCH 110/119] CoreWrapper: using twist covariance instead of pose covariance (which could grow out of bounds depending of the odometry used) --- src/CoreWrapper.cpp | 8 ++++---- src/OdometryROS.cpp | 27 ++++++++++++++------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index fc92982d..ed9b1248 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -610,9 +610,9 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->twist.covariance[0] >= BAD_COVARIANCE)) { - UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->twist.covariance[0]); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; @@ -621,8 +621,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); - float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); + float transVariance = uMax3(odomMsg->twist.covariance[0], odomMsg->twist.covariance[7], odomMsg->twist.covariance[14]); + float rotVariance = uMax3(odomMsg->twist.covariance[21], odomMsg->twist.covariance[28], odomMsg->twist.covariance[35]); if(uIsFinite(rotVariance) && rotVariance > rotVariance_) { rotVariance_ = rotVariance; diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 02b36fce..a54f9102 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -379,12 +379,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.pose.orientation = poseMsg.transform.rotation; //set covariance - odom.pose.covariance.at(0) = info.variance; // xx - odom.pose.covariance.at(7) = info.variance; // yy - odom.pose.covariance.at(14) = info.variance; // zz - odom.pose.covariance.at(21) = info.variance; // rr - odom.pose.covariance.at(28) = info.variance; // pp - odom.pose.covariance.at(35) = info.variance; // yawyaw + // libviso2 uses approximately vel variance * 2 + odom.pose.covariance.at(0) = info.variance*2; // xx + odom.pose.covariance.at(7) = info.variance*2; // yy + odom.pose.covariance.at(14) = info.variance*2; // zz + odom.pose.covariance.at(21) = info.variance*2; // rr + odom.pose.covariance.at(28) = info.variance*2; // pp + odom.pose.covariance.at(35) = info.variance*2; // yawyaw //set velocity bool setTwist = !odometry_->previousVelocityTransform().isNull(); @@ -399,13 +400,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.z = yaw; } - // libviso2 uses approximately pose variance/2 - odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx - odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy - odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz - odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr - odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp - odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw + + odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom); From e67770cc319f3eeddea00bf4f039c48d0f4a6997 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 25 Apr 2016 09:46:38 -0400 Subject: [PATCH 111/119] Added cameraModelToROS() --- include/rtabmap_ros/MsgConversion.h | 4 ++++ src/MsgConversion.cpp | 34 +++++++++++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 328b98a9..fd4bb016 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -95,6 +95,10 @@ void points3fToROS(const std::vector & kpts, std::vector(model.D_raw().cols); + memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double)); + + UASSERT(model.K_raw().total() == 9); + memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double)); + + UASSERT(model.R().total() == 9); + memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double)); + + UASSERT(model.P().total() == 12); + memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double)); + + if(camInfo.D.size() > 5) + { + camInfo.distortion_model = "rational_polynomial"; + } + else + { + camInfo.distortion_model = "plumb_bob"; + } + camInfo.binning_x = 1; + camInfo.binning_y = 1; + camInfo.roi.width = model.imageWidth(); + camInfo.roi.height = model.imageHeight(); + + camInfo.width = model.imageWidth(); + camInfo.height = model.imageHeight(); +} rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, From 424fcd8c4d111597f09f2d6f669a146b3c092234 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 27 Apr 2016 15:12:36 -0400 Subject: [PATCH 112/119] Making rviz dependency optional (rviz plugins won't be built if not detected) --- CMakeLists.txt | 68 +++++++++++++++++++++++++++++++++----------------- 1 file changed, 45 insertions(+), 23 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f05e21b..b448d295 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,13 +7,14 @@ project(rtabmap_ros) find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions - pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader + pcl_ros nodelet dynamic_reconfigure message_filters class_loader genmsg stereo_msgs move_base_msgs ) # Optional components find_package(costmap_2d) find_package(octomap_ros) +find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -95,13 +96,16 @@ ENDIF(costmap_2d_FOUND) IF(octomap_ros_FOUND) SET(optional_dependencies ${optional_dependencies} octomap_ros) ENDIF(octomap_ros_FOUND) +IF(rviz_FOUND) +SET(optional_dependencies ${optional_dependencies} rviz) +ENDIF(rviz_FOUND) catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_ros CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions - pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader + pcl_ros nodelet dynamic_reconfigure message_filters class_loader stereo_msgs move_base_msgs ${optional_dependencies} DEPENDS RTABMap OpenCV ) @@ -125,23 +129,8 @@ SET(Libraries ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${RTABMap_LIBRARIES} - ${rviz_DEFAULT_PLUGIN_LIBRARIES} ) - -## RVIZ plugin -qt4_wrap_cpp(MOC_FILES - src/rviz/MapCloudDisplay.h - src/rviz/MapGraphDisplay.h - src/rviz/InfoDisplay.h - src/rviz/OrbitOrientedViewController.h -) - -# tf:message_filters, mixing boost and Qt signals -set_property( - SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp - PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS - ) - + SET(rtabmap_ros_lib_src src/nodelets/data_throttle.cpp src/nodelets/stereo_throttle.cpp @@ -153,12 +142,7 @@ SET(rtabmap_ros_lib_src src/nodelets/point_cloud_aggregator.cpp src/MsgConversion.cpp src/OdometryROS.cpp - src/rviz/MapCloudDisplay.cpp - src/rviz/MapGraphDisplay.cpp - src/rviz/InfoDisplay.cpp - src/rviz/OrbitOrientedViewController.cpp src/MapsManager.cpp - ${MOC_FILES} ) # If costmap_2d is found, add the plugin @@ -175,7 +159,45 @@ SET(rtabmap_ros_lib_src ) ENDIF(costmap_2d_FOUND) +# If rviz is found, add plugins +IF(rviz_FOUND) +MESSAGE(STATUS "WITH rviz") +include_directories( + ${rviz_INCLUDE_DIRS} +) +SET(Libraries + ${Libraries} + ${rviz_LIBRARIES} + ${rviz_DEFAULT_PLUGIN_LIBRARIES} +) + +## RVIZ plugin +qt4_wrap_cpp(MOC_FILES + src/rviz/MapCloudDisplay.h + src/rviz/MapGraphDisplay.h + src/rviz/InfoDisplay.h + src/rviz/OrbitOrientedViewController.h +) + +# tf:message_filters, mixing boost and Qt signals +set_property( + SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp + PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS +) + +SET(rtabmap_ros_lib_src + ${rtabmap_ros_lib_src} + src/rviz/MapCloudDisplay.cpp + src/rviz/MapGraphDisplay.cpp + src/rviz/InfoDisplay.cpp + src/rviz/OrbitOrientedViewController.cpp + ${MOC_FILES} +) +ENDIF(rviz_FOUND) + +############################ ## Declare a cpp library +############################ add_library(rtabmap_ros ${rtabmap_ros_lib_src} ) From 1d0f9d2b90700702362c48e1644f948789ba44cd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 4 May 2016 17:07:40 -0400 Subject: [PATCH 113/119] Fixed build for Qt5 (default version used by Kinetic) --- CMakeLists.txt | 42 +++++++++++++++++++++++++++++++++--- package.xml | 3 ++- src/GuiWrapper.cpp | 4 ++-- src/PreferencesDialogROS.cpp | 14 ++++++------ src/rviz/MapCloudDisplay.cpp | 6 +++--- 5 files changed, 53 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b448d295..4d364a21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,8 +23,16 @@ find_package(RTABMap 0.11.4 REQUIRED) find_package(OpenCV REQUIRED) #Qt stuff -FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) -INCLUDE(${QT_USE_FILE}) +# If Qt is here, rtabmapviz will be built +# look for Qt5 before Qt4 +FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui) +IF(Qt5_FOUND) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui) +ENDIF(Qt5_FOUND) + +IF(QT4_FOUND) + INCLUDE(${QT_USE_FILE}) +ENDIF(QT4_FOUND) ## We also use Ogre include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) @@ -159,6 +167,13 @@ SET(rtabmap_ros_lib_src ) ENDIF(costmap_2d_FOUND) +IF(QT4_FOUND OR Qt5_FOUND) +SET(Libraries + ${Libraries} + ${QT_LIBRARIES} +) +ENDIF(QT4_FOUND OR Qt5_FOUND) + # If rviz is found, add plugins IF(rviz_FOUND) MESSAGE(STATUS "WITH rviz") @@ -172,12 +187,21 @@ SET(Libraries ) ## RVIZ plugin +IF(QT4_FOUND) qt4_wrap_cpp(MOC_FILES src/rviz/MapCloudDisplay.h src/rviz/MapGraphDisplay.h src/rviz/InfoDisplay.h src/rviz/OrbitOrientedViewController.h ) +ELSE() +qt5_wrap_cpp(MOC_FILES + src/rviz/MapCloudDisplay.h + src/rviz/MapGraphDisplay.h + src/rviz/InfoDisplay.h + src/rviz/OrbitOrientedViewController.h +) +ENDIF() # tf:message_filters, mixing boost and Qt signals set_property( @@ -201,11 +225,14 @@ ENDIF(rviz_FOUND) add_library(rtabmap_ros ${rtabmap_ros_lib_src} ) + target_link_libraries(rtabmap_ros ${Libraries} - ${QT_LIBRARIES} ${OGRE_LIBRARIES} ) +IF(Qt5_FOUND) + QT5_USE_MODULES(rtabmap_ros Widgets Core Gui) +ENDIF() add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS}) # If octomap is found, add definition @@ -243,6 +270,9 @@ target_link_libraries(camera ${Libraries}) IF(RTABMAP_GUI) add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp) target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries}) + IF(Qt5_FOUND) + QT5_USE_MODULES(rtabmapviz Widgets Core Gui) + ENDIF() ELSE() MESSAGE(WARNING "Found RTAB-Map built without its GUI library. Node rtabmapviz will not be built!") ENDIF() @@ -329,6 +359,12 @@ install(FILES costmap_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +IF(costmap_2d_FOUND) +install(FILES + costmap_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +ENDIF(costmap_2d_FOUND) ############# ## Testing ## diff --git a/package.xml b/package.xml index a784ad11..a84b234d 100644 --- a/package.xml +++ b/package.xml @@ -37,7 +37,8 @@ class_loader rtabmap move_base_msgs - costmap_2d + + octomap_ros octomap diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index eeee9034..952de56c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -26,8 +26,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "GuiWrapper.h" -#include -#include +#include +#include #include #include diff --git a/src/PreferencesDialogROS.cpp b/src/PreferencesDialogROS.cpp index 1707fce6..4215bc76 100644 --- a/src/PreferencesDialogROS.cpp +++ b/src/PreferencesDialogROS.cpp @@ -27,14 +27,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "PreferencesDialogROS.h" #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp index 5b81b35e..60a0ee16 100644 --- a/src/rviz/MapCloudDisplay.cpp +++ b/src/rviz/MapCloudDisplay.cpp @@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include +#include +#include #include #include From 6f06522a8ce255fa9436db118defaf333803cd1c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 4 May 2016 17:09:55 -0400 Subject: [PATCH 114/119] Quiet Qt5 if not found --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d364a21..c9ba62c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(OpenCV REQUIRED) #Qt stuff # If Qt is here, rtabmapviz will be built # look for Qt5 before Qt4 -FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui) +FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET) IF(Qt5_FOUND) FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui) ENDIF(Qt5_FOUND) @@ -232,7 +232,7 @@ target_link_libraries(rtabmap_ros ) IF(Qt5_FOUND) QT5_USE_MODULES(rtabmap_ros Widgets Core Gui) -ENDIF() +ENDIF(Qt5_FOUND) add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS}) # If octomap is found, add definition From c07183623543dd8351fb1c0e99767f407b41362a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 6 May 2016 12:37:11 -0400 Subject: [PATCH 115/119] Updated package version to 0.11.4 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index a84b234d..c1dd743b 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.10.11 + 0.11.4 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe From ee91cfc04876f270d5dd4f59b3b2f30406811064 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 6 May 2016 13:18:09 -0400 Subject: [PATCH 116/119] package.xml: removed costmap_2d run dependency --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index c1dd743b..7b189801 100644 --- a/package.xml +++ b/package.xml @@ -68,7 +68,7 @@ class_loader rtabmap move_base_msgs - costmap_2d + octomap_ros octomap From 5fec584edacdfa25bce94c3407d102572c423eaa Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 6 May 2016 19:15:24 -0400 Subject: [PATCH 117/119] Fixing boost issue on Qt Moc with MapCloudDisplay.h --- src/rviz/MapCloudDisplay.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/rviz/MapCloudDisplay.h b/src/rviz/MapCloudDisplay.h index f43ff9d6..00e507e2 100644 --- a/src/rviz/MapCloudDisplay.h +++ b/src/rviz/MapCloudDisplay.h @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAP_CLOUD_DISPLAY_H #define MAP_CLOUD_DISPLAY_H +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 + #include #include #include @@ -42,6 +44,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#endif + namespace rviz { class IntProperty; class BoolProperty; From 439eff4b352c4f9749a93a128590f429a2258a26 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 May 2016 11:19:07 -0400 Subject: [PATCH 118/119] Update to link with rtabmap 0.11.5 (RTABMAP_QT_VERSION cmake variable set) --- CMakeLists.txt | 31 ++++++++++++++++++++----------- package.xml | 2 +- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c9ba62c4..50f81b39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,21 +18,30 @@ find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.4 REQUIRED) +find_package(RTABMap 0.11.5 REQUIRED) find_package(OpenCV REQUIRED) #Qt stuff -# If Qt is here, rtabmapviz will be built -# look for Qt5 before Qt4 -FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET) -IF(Qt5_FOUND) - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui) -ENDIF(Qt5_FOUND) - -IF(QT4_FOUND) - INCLUDE(${QT_USE_FILE}) -ENDIF(QT4_FOUND) +# If librtabmap_gui.so is found, rtabmapviz will be built +# If rviz is found, plugins will be built +IF(RTABMAP_GUI OR rviz_FOUND) + IF(RTABMAP_QT_VERSION EQUAL 4) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) + INCLUDE(${QT_USE_FILE}) + ELSE() + IF(RTABMAP_GUI) + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui REQUIRED) + ELSE() + # For rviz plugins, look for Qt5 before Qt4 + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET) + IF(NOT Qt5_FOUND) + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) + INCLUDE(${QT_USE_FILE}) + ENDIF(NOT Qt5_FOUND) + ENDIF() + ENDIF() +ENDIF(RTABMAP_GUI OR rviz_FOUND) ## We also use Ogre include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) diff --git a/package.xml b/package.xml index 7b189801..cdc2bef6 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.11.4 + 0.11.5 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe From 804c6627d3a688d735d5fc0e41a8bfbef5897429 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 9 May 2016 16:14:21 -0400 Subject: [PATCH 119/119] removed optional libs from CATKIN_DEPENDS to avoid catkin error (catkin_package() DEPENDS on the catkin package 'costmap_2d' which must therefore be listed as a run dependency in the package.xml) --- CMakeLists.txt | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 50f81b39..93dbb2b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,25 +105,13 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg) ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need - -SET(optional_dependencies "") -IF(costmap_2d_FOUND) -SET(optional_dependencies ${optional_dependencies} costmap_2d ) -ENDIF(costmap_2d_FOUND) -IF(octomap_ros_FOUND) -SET(optional_dependencies ${optional_dependencies} octomap_ros) -ENDIF(octomap_ros_FOUND) -IF(rviz_FOUND) -SET(optional_dependencies ${optional_dependencies} rviz) -ENDIF(rviz_FOUND) - catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_ros CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs visualization_msgs image_transport tf tf_conversions tf2_ros eigen_conversions laser_geometry pcl_conversions pcl_ros nodelet dynamic_reconfigure message_filters class_loader - stereo_msgs move_base_msgs ${optional_dependencies} + stereo_msgs move_base_msgs DEPENDS RTABMap OpenCV )