diff --git a/README.md b/README.md index 05bc9f9f..19f91bff 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,13 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current Rolling Build Status + + Docker + + rtabmap_ros + + Docker Pulls + diff --git a/docker/README.md b/docker/README.md index 53af51a2..7774f8a2 100644 --- a/docker/README.md +++ b/docker/README.md @@ -5,49 +5,45 @@ foxy, foxy-latest humble, humble-latest ``` - * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including GTSAM and libpointmatcher dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. + * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. * Launch `rtabmap` from inside the container (**no gui**), saving the database on host `~/.ros/rtabmap.db`: ```bash - $ docker run -it --rm \ + docker run -it --rm \ + --user $UID \ + -e ROS_HOME=/tmp/.ros \ --network=host \ - -v ~/.ros:/root \ + --ipc=host \ + -v ~/.ros:/tmp/.ros \ introlab3it/rtabmap_ros:humble-latest \ - ros2 launch rtabmap_ros rtabmap.launch.py rtabmapviz:=false database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start" + ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=false database_path:=/tmp/.ros/rtabmap.db rtabmap_args:="--delete_db_on_start" ``` - * Launch `rtabmap` from inside the container (**with gui**, if you have a nvidia GPU, follow those [instructions](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2) with nvidia-docker2 using one of the images above instead of the ros image example, or for other GPUs try the intel/AMD [instructions](http://wiki.ros.org/action/fullsearch/docker/Tutorials/Hardware%20Acceleration) to build a `rtabmap_ros3d` image with your GPU driver), saving the database on host `~/.ros/rtabmap.db`: +* Launch `rtabmap_viz` from inside the container, using [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2): - * With [nvidia-docker2 approach](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html), we would have this Dockerfile: - ```docker - FROM introlab3it/rtabmap_ros:humble-latest + ```bash + # those following 3 lines would need to be done only one time + XAUTH=/tmp/.docker.xauth + touch $XAUTH + xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + + docker run -it --rm \ + --privileged \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e XAUTHORITY=$XAUTH \ + --user $UID \ + -e ROS_HOME=/tmp/.ros \ + -v ~/.ros:/tmp/.ros \ + --runtime=nvidia \ + --network=host \ + --ipc=host \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + introlab3it/rtabmap_ros:humble-latest \ + /bin/bash -c "ros2 run rtabmap_viz rtabmap_viz --ros-args -r __ns:=/rtabmap" + ``` - # nvidia-container-runtime - ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} - ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - ``` - Building the image and run it: - ```bash - $ docker build -t rtabmap_ros3d . - - # those following 3 lines would need to be done only one time - $ XAUTH=/tmp/.docker.xauth - $ touch $XAUTH - $ xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - - - $ docker run -it --rm \ - --privileged \ - --env="DISPLAY=$DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - --env="XAUTHORITY=$XAUTH" \ - --volume="$XAUTH:$XAUTH" \ - --runtime=nvidia \ - --network=host \ - -v ~/.ros:/root \ - rtabmap_ros3d \ - ros2 launch rtabmap_ros rtabmap.launch.py database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start" - ``` diff --git a/docker/noetic/latest/Dockerfile b/docker/noetic/latest/Dockerfile index 5ae90e62..523dab67 100644 --- a/docker/noetic/latest/Dockerfile +++ b/docker/noetic/latest/Dockerfile @@ -9,6 +9,10 @@ COPY . catkin_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd catkin_ws && \ + apt update && \ + rosdep install --from-paths src --ignore-src -y && \ + apt remove ros-$ROS_DISTRO-rtabmap* -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \ cd && \ rm -rf catkin_ws diff --git a/rtabmap_demos/CMakeLists.txt b/rtabmap_demos/CMakeLists.txt index 4ef86315..2e7ce52f 100644 --- a/rtabmap_demos/CMakeLists.txt +++ b/rtabmap_demos/CMakeLists.txt @@ -7,4 +7,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/rtabmap_examples/CMakeLists.txt b/rtabmap_examples/CMakeLists.txt index 7d5017d9..a8020127 100644 --- a/rtabmap_examples/CMakeLists.txt +++ b/rtabmap_examples/CMakeLists.txt @@ -7,4 +7,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index d524ed06..e3a4db39 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -342,7 +342,7 @@ def launch_setup(context, *args, **kwargs): ("scan", LaunchConfiguration('scan_topic')), ("scan_cloud", LaunchConfiguration('scan_cloud_topic')), ("odom", LaunchConfiguration('odom_topic'))], - condition=IfCondition(LaunchConfiguration("rtabmapviz")), + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), arguments=[LaunchConfiguration("gui_cfg")], prefix=LaunchConfiguration('launch_prefix'), namespace=LaunchConfiguration('namespace')), @@ -383,14 +383,14 @@ def generate_launch_description(): DeclareLaunchArgument('stereo', default_value='false', description='Use stereo input instead of RGB-D.'), DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), - DeclareLaunchArgument('rtabmapviz', default_value='true', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'), DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'), # Config files DeclareLaunchArgument('cfg', default_value='', description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'), - DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini', description='Configuration path of rtabmapviz.'), + DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini', description='Configuration path of rtabmap_viz.'), DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), DeclareLaunchArgument('frame_id', default_value='base_link', description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'), diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 500f0fb2..a3674fb4 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -263,6 +263,7 @@ private: rtabmap::Transform currentMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_; bool latestNodeWasReached_; + bool pubLocPoseOnlyWhenLocalizing_; bool graphLatched_; rtabmap::ParametersMap parameters_; std::map rtabmapROSStats_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index bc144a55..43d65d44 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -92,6 +92,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : lastPose_(Transform::getIdentity()), lastPoseIntermediate_(false), latestNodeWasReached_(false), + pubLocPoseOnlyWhenLocalizing_(false), graphLatched_(false), frameId_("base_link"), odomFrameId_(""), @@ -189,7 +190,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDefaultAngVariance_ = this->declare_parameter("landmark_angular_variance", landmarkDefaultAngVariance_); landmarkDefaultLinVariance_ = this->declare_parameter("landmark_linear_variance", landmarkDefaultLinVariance_); - + + pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_); waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr); useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_); @@ -226,6 +228,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance); RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); if(this->isSubscribedToStereo()) { RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); @@ -2018,16 +2021,35 @@ void CoreWrapper::process( } else { - if(localizationPosePub_->get_subscription_count() && - !rtabmap_.getStatistics().localizationCovariance().empty()) + if(localizationPosePub_->get_subscription_count()) { - geometry_msgs::msg::PoseWithCovarianceStamped poseMsg; - poseMsg.header.frame_id = mapFrameId_; - poseMsg.header.stamp = stamp; - rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); - const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); - memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double)); - localizationPosePub_->publish(poseMsg); + bool localized = rtabmap_.getStatistics().loopClosureId()!=0 || + rtabmap_.getStatistics().proximityDetectionId()!=0 || + static_cast(uValue(rtabmap_.getStatistics().data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f))!=0; + + if(localized || !pubLocPoseOnlyWhenLocalizing_) + { + geometry_msgs::msg::PoseWithCovarianceStamped poseMsg; + poseMsg.header.frame_id = mapFrameId_; + poseMsg.header.stamp = stamp; + rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); + if(!rtabmap_.getStatistics().localizationCovariance().empty()) + { + const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); + memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double)); + } + else + { + // Not yet localized, publish large covariance + poseMsg.pose.covariance.data()[0] = 9999; + poseMsg.pose.covariance.data()[7] = 9999; + poseMsg.pose.covariance.data()[14] = twoDMapping_?rtabmap::Registration::COVARIANCE_LINEAR_EPSILON:9999; + poseMsg.pose.covariance.data()[21] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999; + poseMsg.pose.covariance.data()[28] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999; + poseMsg.pose.covariance.data()[35] = 9999; + } + localizationPosePub_->publish(poseMsg); + } } std::map filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end()); diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 057a06bb..3e0017d8 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -159,4 +159,4 @@ install(DIRECTORY include/ FILES_MATCHING PATTERN "*.h" ) -ament_package(CONFIG_EXTRAS ${CMAKE_CURRENT_BINARY_DIR}/cmake/extra_configs.cmake) \ No newline at end of file +ament_package(CONFIG_EXTRAS ${CMAKE_CURRENT_BINARY_DIR}/cmake/extra_configs.cmake) diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index 020f082f..cde9111b 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -71,6 +71,7 @@ protected: private: void infoMapCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg, const rtabmap_msgs::msg::MapData::ConstSharedPtr mapMsg); + void infoCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg); void goalPathCallback(const rtabmap_msgs::msg::Goal::ConstSharedPtr goalMsg, const nav_msgs::msg::Path::ConstSharedPtr pathMsg); void goalReachedCallback(const std_msgs::msg::Bool::ConstSharedPtr value); @@ -141,6 +142,7 @@ private: message_filters::Subscriber infoTopic_; message_filters::Subscriber mapDataTopic_; + rclcpp::Subscription::SharedPtr infoOnlyTopic_; message_filters::Subscriber goalTopic_; message_filters::Subscriber pathTopic_; diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index f820c61f..41d1940f 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -112,12 +112,14 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : // To receive odometry events std::string initCachePath; + bool subscribeInfoOnly = false; frameId_ = this->declare_parameter("frame_id", frameId_); this->get_parameter_or("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF, ros2: already declared in CommonDataSubscriber waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_); cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process + subscribeInfoOnly = this->declare_parameter("subscribe_info_only", subscribeInfoOnly); initCachePath = this->declare_parameter("init_cache_path", initCachePath); if(initCachePath.size()) { @@ -142,13 +144,21 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : republishNodeDataPub_ = this->create_publisher("republish_node_data", 1); - infoTopic_.subscribe(this, "info"); - mapDataTopic_.subscribe(this, "mapData"); - infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(this->getQueueSize()), - infoTopic_, - mapDataTopic_); - infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2)); + if(subscribeInfoOnly) + { + RCLCPP_INFO(this->get_logger(), "rtabmap_viz: subscribe_info_only=true"); + infoOnlyTopic_ = this->create_subscription("info", 1, std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1)); + } + else + { + infoTopic_.subscribe(this, "info"); + mapDataTopic_.subscribe(this, "mapData"); + infoMapSync_ = new message_filters::Synchronizer( + MyInfoMapSyncPolicy(this->getQueueSize()), + infoTopic_, + mapDataTopic_); + infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2)); + } goalTopic_.subscribe(this, "goal_node"); pathTopic_.subscribe(this, "global_path"); @@ -198,6 +208,36 @@ void GuiWrapper::infoMapCallback( this->post(new RtabmapEvent(stat)); } +void GuiWrapper::infoCallback( + const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg) +{ + rtabmap::Statistics stat; + + // Info from ROS + rtabmap_conversions::infoFromROS(*infoMsg, stat); + + // mapToOdom can be recovered from statistics + if(stat.data().find(Statistics::kLoopMapToOdom_x()) != stat.data().end() && + stat.data().find(Statistics::kLoopMapToOdom_y()) != stat.data().end() && + stat.data().find(Statistics::kLoopMapToOdom_z()) != stat.data().end() && + stat.data().find(Statistics::kLoopMapToOdom_roll()) != stat.data().end() && + stat.data().find(Statistics::kLoopMapToOdom_pitch()) != stat.data().end() && + stat.data().find(Statistics::kLoopMapToOdom_yaw()) != stat.data().end()) + { + rtabmap::Transform mapToOdom; + mapToOdom = rtabmap::Transform( + stat.data().at(Statistics::kLoopMapToOdom_x()), + stat.data().at(Statistics::kLoopMapToOdom_y()), + stat.data().at(Statistics::kLoopMapToOdom_z()), + stat.data().at(Statistics::kLoopMapToOdom_roll())*M_PI/180.f, + stat.data().at(Statistics::kLoopMapToOdom_pitch())*M_PI/180.f, + stat.data().at(Statistics::kLoopMapToOdom_yaw())*M_PI/180.f); + stat.setMapCorrection(mapToOdom); + } + + this->post(new RtabmapEvent(stat)); +} + void GuiWrapper::goalPathCallback( const rtabmap_msgs::msg::Goal::ConstSharedPtr goalMsg, const nav_msgs::msg::Path::ConstSharedPtr pathMsg)