From a6921845b614855840f7f4052d7600fdec3a5488 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 May 2026 15:08:21 -0700 Subject: [PATCH] ros1: Migrating tf to tf2 (#1425) * Migrating tf to tf2 * Added ci action to test PR on ros1 * updated dev container with nvidia working * backward compatibility with topics having frame_id with leading slash not allowed with tf2 * backward compatibility of leading slash for other tf2 buffers * updated comment --- .devcontainer/devcontainer.json | 11 +- .github/workflows/noetic-pr.yml | 41 ++++++ rtabmap_conversions/CMakeLists.txt | 4 +- .../rtabmap_conversions/MsgConversion.h | 21 +-- rtabmap_conversions/package.xml | 1 + rtabmap_conversions/src/MsgConversion.cpp | 135 ++++++++---------- rtabmap_costmap_plugins/src/voxel_layer.cpp | 5 +- .../launch/demo_turtlebot3_navigation.launch | 6 +- rtabmap_demos/src/SaveObjectsExample.cpp | 30 ++-- .../src/nodelets/obstacles_detection_old.cpp | 29 ++-- .../include/rtabmap_odom/OdometryROS.h | 8 +- rtabmap_odom/src/OdometryROS.cpp | 11 +- rtabmap_odom/src/nodelets/icp_odometry.cpp | 16 +-- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 2 +- .../src/nodelets/rgbdicp_odometry.cpp | 8 +- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 6 +- .../include/rtabmap_slam/CoreWrapper.h | 6 +- rtabmap_slam/src/CoreWrapper.cpp | 41 +++--- rtabmap_util/CMakeLists.txt | 4 +- rtabmap_util/package.xml | 1 + rtabmap_util/src/nodelets/imu_to_tf.cpp | 28 ++-- rtabmap_util/src/nodelets/lidar_deskewing.cpp | 17 ++- .../src/nodelets/obstacles_detection.cpp | 49 +++---- .../src/nodelets/point_cloud_aggregator.cpp | 15 +- .../src/nodelets/point_cloud_assembler.cpp | 13 +- .../src/nodelets/pointcloud_to_depthimage.cpp | 12 +- rtabmap_viz/CMakeLists.txt | 4 +- rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 6 +- rtabmap_viz/package.xml | 1 + rtabmap_viz/src/GuiWrapper.cpp | 27 ++-- 30 files changed, 311 insertions(+), 247 deletions(-) create mode 100644 .github/workflows/noetic-pr.yml diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 091c51e4..43f6becb 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -23,9 +23,16 @@ "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/catkin_ws", "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace", + "hostRequirements": { + "gpu": "optional" + }, "runArgs": ["--privileged", - "--runtime=nvidia", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer "--env=DISPLAY", "--env=QT_X11_NO_MITSHM=1", - "--volume=/tmp/.X11-unix:/tmp/.X11-unix"] + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.github/workflows/noetic-pr.yml b/.github/workflows/noetic-pr.yml new file mode 100644 index 00000000..b984137f --- /dev/null +++ b/.github/workflows/noetic-pr.yml @@ -0,0 +1,41 @@ +name: noetic-pr + +on: + pull_request: + branches: + - 'master' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + docker: + runs-on: ubuntu-latest + + strategy: + matrix: + docker_tag: [rtabmap_ros_noetic_pr] + include: + - docker_tag: rtabmap_ros_noetic_pr + docker_path: 'noetic/latest' + docker_platforms: | + linux/amd64 + + steps: + - + name: Checkout + uses: actions/checkout@v2 + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + - + name: Build and push + uses: docker/build-push-action@v2 + with: + context: . + push: false + platforms: ${{ matrix.docker_platforms }} + file: ./docker/${{ matrix.docker_path }}/Dockerfile + tags: ${{ matrix.docker_tag }} + diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 17e44954..01882399 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_conversions) find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs - tf tf_conversions eigen_conversions laser_geometry pcl_conversions + tf tf2_ros tf_conversions eigen_conversions laser_geometry pcl_conversions image_geometry rtabmap_msgs ) @@ -13,7 +13,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_conversions CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs - tf tf_conversions eigen_conversions laser_geometry pcl_conversions + tf tf2_ros tf_conversions eigen_conversions laser_geometry pcl_conversions image_geometry rtabmap_msgs DEPENDS RTABMap ) diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 4c3b60bd..ac2777a8 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -29,7 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MSGCONVERSION_H_ #include -#include +#include +#include #include #include #include @@ -135,7 +136,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, const std::string & frameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); void mapDataFromROS( @@ -190,7 +191,7 @@ rtabmap::Landmarks landmarksFromROS( const std::string & frameId, const std::string & odomFrameId, const ros::Time & odomStamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, double defaultLinVariance, double defaultAngVariance); @@ -202,7 +203,7 @@ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); @@ -213,7 +214,7 @@ rtabmap::Transform getMovingTransform( const std::string & fixedFrame, const ros::Time & stampFrom, const ros::Time & stampTo, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); bool convertRGBDMsgs( @@ -228,7 +229,7 @@ bool convertRGBDMsgs( cv::Mat & depth, std::vector & cameraModels, std::vector & stereoCameraModels, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadRectifiedImages, const std::vector > & localKeyPointsMsgs = std::vector >(), @@ -249,7 +250,7 @@ bool convertStereoMsg( cv::Mat & left, cv::Mat & right, rtabmap::StereoCameraModel & stereoModel, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadyRectified); @@ -259,7 +260,7 @@ bool convertScanMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool outputInFrameId = false); @@ -269,7 +270,7 @@ bool convertScan3dMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, int maxPoints = 0, float maxRange = 0.0f, @@ -279,7 +280,7 @@ bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool slerp = false); diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index ebd35cff..93515c62 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -23,6 +23,7 @@ sensor_msgs std_msgs tf + tf2_ros tf_conversions diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index ff155b4f..512384f1 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -928,14 +928,14 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, const std::string & frameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { rtabmap::Transform localTransform = getTransform( frameId, leftCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(localTransform.isNull()) { @@ -946,7 +946,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( leftCamInfo.header.frame_id, rightCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -1885,7 +1885,7 @@ rtabmap::Landmarks landmarksFromROS( const std::string & frameId, const std::string & odomFrameId, const ros::Time & odomStamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, double defaultLinVariance, double defaultAngVariance) @@ -1903,7 +1903,7 @@ rtabmap::Landmarks landmarksFromROS( frameId, iter->second.first.header.frame_id, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(baseToCamera.isNull()) @@ -1923,7 +1923,7 @@ rtabmap::Landmarks landmarksFromROS( odomFrameId, odomStamp, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(!correction.isNull()) { @@ -1952,32 +1952,24 @@ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { // TF ready? rtabmap::Transform transform; try { - if(waitForTransform > 0.0 && !stamp.isZero()) - { - //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) - std::string errorMsg; - if(!listener.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) - { - ROS_WARN("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".", - fromFrameId.c_str(), toFrameId.c_str(), waitForTransform, stamp.toSec(), errorMsg.c_str()); - return transform; - } - } - - tf::StampedTransform tmp; - listener.lookupTransform(fromFrameId, toFrameId, stamp, tmp); - transform = rtabmap_conversions::transformFromTF(tmp); + geometry_msgs::TransformStamped tmp; + tmp = tfBuffer.lookupTransform( + !fromFrameId.empty()&&fromFrameId.at(0)=='/'?fromFrameId.substr(1):fromFrameId, + !toFrameId.empty()&&toFrameId.at(0)=='/'?toFrameId.substr(1):toFrameId, + stamp, + ros::Duration(waitForTransform)); + transform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform); } - catch(tf::TransformException & ex) + catch(tf2::TransformException & ex) { - ROS_WARN("(getting transform %s -> %s) %s", fromFrameId.c_str(), toFrameId.c_str(), ex.what()); + ROS_WARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); } return transform; } @@ -1989,30 +1981,24 @@ rtabmap::Transform getMovingTransform( const std::string & fixedFrame, const ros::Time & stampFrom, const ros::Time & stampTo, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { // TF ready? rtabmap::Transform transform; try { - ros::Time stamp = stampTo>stampFrom?stampTo:stampFrom; - if(waitForTransform > 0.0 && !stamp.isZero()) - { - std::string errorMsg; - if(!listener.waitForTransform(movingFrame, fixedFrame, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) - { - ROS_WARN("Could not get transform from %s to %s accordingly to %s after %f seconds (for stamps=%f -> %f)! Error=\"%s\".", - movingFrame.c_str(), movingFrame.c_str(), fixedFrame.c_str(), waitForTransform, stampTo.toSec(), stampFrom.toSec(), errorMsg.c_str()); - return transform; - } - } - - tf::StampedTransform tmp; - listener.lookupTransform(movingFrame, stampFrom, movingFrame, stampTo, fixedFrame, tmp); - transform = rtabmap_conversions::transformFromTF(tmp); + geometry_msgs::TransformStamped tmp; + tmp = tfBuffer.lookupTransform( + !movingFrame.empty()&&movingFrame.at(0)=='/'?movingFrame.substr(1):movingFrame, + stampFrom, + !movingFrame.empty()&&movingFrame.at(0)=='/'?movingFrame.substr(1):movingFrame, + stampTo, + !fixedFrame.empty()&&fixedFrame.at(0)=='/'?fixedFrame.substr(1):fixedFrame, + ros::Duration(waitForTransform)); + transform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform); } - catch(tf::TransformException & ex) + catch(tf2::TransformException & ex) { ROS_WARN("(getting transform movement of %s according to fixed %s) %s", movingFrame.c_str(), fixedFrame.c_str(), ex.what()); } @@ -2031,7 +2017,7 @@ bool convertRGBDMsgs( cv::Mat & depth, std::vector & cameraModels, std::vector & stereoCameraModels, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadRectifiedImages, const std::vector > & localKeyPointsMsgs, @@ -2157,7 +2143,7 @@ bool convertRGBDMsgs( } // use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb) - rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform); + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec()); @@ -2171,7 +2157,7 @@ bool convertRGBDMsgs( odomFrameId, odomStamp, stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2310,7 +2296,7 @@ bool convertRGBDMsgs( depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2355,7 +2341,7 @@ bool convertRGBDMsgs( cameraInfoMsgs[i].header.frame_id, depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); } if(stereoTransform.isNull() || stereoTransform.x()<=0) @@ -2423,7 +2409,7 @@ bool convertStereoMsg( cv::Mat & left, cv::Mat & right, rtabmap::StereoCameraModel & stereoModel, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadyRectified) { @@ -2474,7 +2460,7 @@ bool convertStereoMsg( right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; } - rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform); + rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { return false; @@ -2487,7 +2473,7 @@ bool convertStereoMsg( odomFrameId, odomStamp, leftImageMsg->header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2507,7 +2493,7 @@ bool convertStereoMsg( rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2537,7 +2523,7 @@ bool convertStereoMsg( leftCamInfoMsg.header.frame_id, rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull() || stereoTransform.x()<=0) { @@ -2575,7 +2561,7 @@ bool convertScanMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool outputInFrameId) { @@ -2603,7 +2589,7 @@ bool convertScanMsg( odomFrameId.empty()?frameId:odomFrameId, scan2dMsg.header.stamp, scan2dMsg.header.stamp + ros::Duration().fromSec(scan2dMsg.ranges.size()*scan2dMsg.time_increment), - listener, + tfBuffer, waitForTransform); if(tmpT.isNull()) { @@ -2614,7 +2600,7 @@ bool convertScanMsg( frameId, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(scanLocalTransform.isNull()) { @@ -2624,14 +2610,14 @@ bool convertScanMsg( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, listener); + projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer); //transform back in laser frame rtabmap::Transform laserToOdom = getTransform( scan2dMsg.header.frame_id, odomFrameId.empty()?frameId:odomFrameId, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(laserToOdom.isNull()) { @@ -2646,7 +2632,7 @@ bool convertScanMsg( odomFrameId, odomStamp, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2734,7 +2720,7 @@ bool convertScan3dMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, int maxPoints, float maxRange, @@ -2743,7 +2729,7 @@ bool convertScan3dMsg( UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height, uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str()); - rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform); + rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfBuffer, waitForTransform); if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg.header.stamp.toSec()); @@ -2758,7 +2744,7 @@ bool convertScan3dMsg( odomFrameId, odomStamp, scan3dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2779,13 +2765,13 @@ bool deskew_impl( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener * listener, + tf2_ros::Buffer * tfBuffer, double waitForTransform, bool slerp, const rtabmap::Transform & velocity, double previousStamp) { - if(listener != 0) + if(tfBuffer != 0) { if(input.header.frame_id.empty()) { @@ -3088,16 +3074,15 @@ bool deskew_impl( } std::string errorMsg; - if(listener != 0 && + if(tfBuffer != 0 && waitForTransform>0.0 && - !listener->waitForTransform( - input.header.frame_id, + !tfBuffer->canTransform( + !input.header.frame_id.empty()&&input.header.frame_id.at(0)=='/'?input.header.frame_id.substr(1):input.header.frame_id, firstStamp, - input.header.frame_id, + !input.header.frame_id.empty()&&input.header.frame_id.at(0)=='/'?input.header.frame_id.substr(1):input.header.frame_id, lastStamp, - fixedFrameId, + !fixedFrameId.empty()&&fixedFrameId.at(0)=='/'?fixedFrameId.substr(1):fixedFrameId, ros::Duration(waitForTransform), - ros::Duration(0.01), &errorMsg)) { ROS_ERROR("Could not estimate motion of %s accordingly to fixed frame %s between stamps %f and %f! (%s)", @@ -3114,21 +3099,21 @@ bool deskew_impl( double scanTime = 0; if(slerp) { - if(listener != 0) + if(tfBuffer != 0) { firstPose = rtabmap_conversions::getMovingTransform( input.header.frame_id, fixedFrameId, input.header.stamp, firstStamp, - *listener, + *tfBuffer, 0); lastPose = rtabmap_conversions::getMovingTransform( input.header.frame_id, fixedFrameId, input.header.stamp, lastStamp, - *listener, + *tfBuffer, 0); } else @@ -3233,7 +3218,7 @@ bool deskew_impl( fixedFrameId, output.header.stamp, stamp, - *listener, + *tfBuffer, 0); if(transform.isNull()) { @@ -3327,7 +3312,7 @@ bool deskew_impl( fixedFrameId, output.header.stamp, stamp, - *listener, + *tfBuffer, 0); if(transform.isNull()) { @@ -3376,11 +3361,11 @@ bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool slerp) { - return deskew_impl(input, output, fixedFrameId, &listener, waitForTransform, slerp, rtabmap::Transform(), 0); + return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0); } bool deskew( diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp index 35574349..495bb2ba 100644 --- a/rtabmap_costmap_plugins/src/voxel_layer.cpp +++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp @@ -406,7 +406,10 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) { geometry_msgs::TransformStamped transformStamped; #ifdef COSTMAP_2D_POINTCLOUD2 - transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0)); + transformStamped = tf_->lookupTransform( + !global_frame_.empty()&&global_frame_.at(0)=='/'?global_frame_.substr(1):global_frame_, + !robot_base_frame_.empty()&&robot_base_frame_.at(0)=='/'?robot_base_frame_.substr(1):robot_base_frame_, + ros::Time(0)); #else tf::StampedTransform stampedTransform; tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform); diff --git a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch index b5c9e1a1..7bb41425 100644 --- a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch +++ b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch @@ -1,10 +1,10 @@