i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 02:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CommonDataSubscriber.h
+5 -18
View File
@@ -75,33 +75,20 @@ public:
protected:
void setupCallbacks(rclcpp::Node & node);
virtual void commonDepthCallback(
virtual void commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan& scanMsg,
const sensor_msgs::msg::PointCloud2& scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
const std::vector<rtabmap_ros::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::msg::GlobalDescriptor>(),
const std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::msg::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::msg::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
virtual void commonStereoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const cv_bridge::CvImageConstPtr& leftImageMsg,
const cv_bridge::CvImageConstPtr& rightImageMsg,
const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
const std::vector<rtabmap_ros::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::msg::GlobalDescriptor>(),
const std::vector<rtabmap_ros::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::msg::KeyPoint>(),
const std::vector<rtabmap_ros::msg::Point3f> & localPoints3d = std::vector<rtabmap_ros::msg::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat()) = 0;
virtual void commonLaserScanCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -114,7 +101,7 @@ protected:
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
void commonSingleDepthCallback(
void commonSingleCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const cv_bridge::CvImageConstPtr & imageMsg,
include/rtabmap_ros/CoreWrapper.h
+6 -16
View File
@@ -112,12 +112,13 @@ private:
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom
virtual void commonDepthCallback(
virtual void commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -125,12 +126,13 @@ private:
const std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::msg::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::msg::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
void commonDepthCallbackImpl(
void commonMultiCameraCallbackImpl(
const std::string & odomFrameId,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -138,20 +140,6 @@ private:
const std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > & localKeyPoints,
const std::vector<std::vector<rtabmap_ros::msg::Point3f> > & localPoints3d,
const std::vector<cv::Mat> & localDescriptors);
virtual void commonStereoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const cv_bridge::CvImageConstPtr& leftImageMsg,
const cv_bridge::CvImageConstPtr& rightImageMsg,
const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
const std::vector<rtabmap_ros::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::msg::GlobalDescriptor>(),
const std::vector<rtabmap_ros::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::msg::KeyPoint>(),
const std::vector<rtabmap_ros::msg::Point3f> & localPoints3d = std::vector<rtabmap_ros::msg::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
virtual void commonLaserScanCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -196,6 +184,7 @@ private:
const rclcpp::Time & stamp,
rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::vector<float> & odomVelocity = std::vector<float>(),
const std::string & odomFrameId = "",
const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(),
@@ -261,6 +250,7 @@ private:
bool paused_;
rtabmap::Transform lastPose_;
rclcpp::Time lastPoseStamp_;
std::vector<float> lastPoseVelocity_;
bool lastPoseIntermediate_;
cv::Mat covariance_;
rtabmap::Transform currentMetricGoal_;
include/rtabmap_ros/GuiWrapper.h
+2 -1
View File
@@ -73,12 +73,13 @@ private:
void goalPathCallback(const rtabmap_ros::msg::Goal::ConstSharedPtr goalMsg, const nav_msgs::msg::Path::ConstSharedPtr pathMsg);
void goalReachedCallback(const std_msgs::msg::Bool::ConstSharedPtr value);
virtual void commonDepthCallback(
virtual void commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scanMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
include/rtabmap_ros/MsgConversion.h
+3
View File
@@ -211,14 +211,17 @@ bool convertRGBDMsgs(
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const std::string & frameId,
const std::string & odomFrameId,
const rclcpp::Time & odomStamp,
cv::Mat & rgb,
cv::Mat & depth,
std::vector<rtabmap::CameraModel> & cameraModels,
std::vector<rtabmap::StereoCameraModel> & stereoCameraModels,
tf2_ros::Buffer & tfBuffer,
double waitForTransform,
bool alreadRectifiedImages,
const std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > & localKeyPointsMsgs = std::vector<std::vector<rtabmap_ros::msg::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::msg::Point3f> > & localPoints3dMsgs = std::vector<std::vector<rtabmap_ros::msg::Point3f> >(),
const std::vector<cv::Mat> & localDescriptorsMsgs = std::vector<cv::Mat>(),
include/rtabmap_ros/point_cloud_aggregator.hpp
+1
View File
@@ -91,6 +91,7 @@ private:
std::string frameId_;
std::string fixedFrameId_;
double waitForTransform_;
bool xyzOutput_;
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
};
include/rtabmap_ros/pointcloud_to_depthimage.hpp
+2
View File
@@ -59,6 +59,8 @@ private:
private:
image_transport::CameraPublisher depthImage16Pub_;
image_transport::CameraPublisher depthImage32Pub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo16Pub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo32Pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
include/rtabmap_ros/rgbd_odometry.hpp
+5
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_ros/msg/rgbd_image.hpp>
#include <rtabmap_ros/msg/rgbd_images.hpp>
#include <cv_bridge/cv_bridge.h>
@@ -66,6 +67,9 @@ private:
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
void callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images);
void callbackRGBD(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image);
@@ -100,6 +104,7 @@ private:
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> info_sub_;
rclcpp::Subscription<rtabmap_ros::msg::RGBDImage>::SharedPtr rgbdSub_;
rclcpp::Subscription<rtabmap_ros::msg::RGBDImages>::SharedPtr rgbdxSub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image3_sub_;
include/rtabmap_ros/stereo_odometry.hpp
+44 -1
View File
@@ -37,8 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_ros/msg/rgbd_image.hpp>
#include <rtabmap_ros/msg/rgbd_images.hpp>
namespace rtabmap_ros
{
@@ -54,6 +56,12 @@ private:
virtual void updateParameters(rtabmap::ParametersMap & parameters);
virtual void onOdomInit();
void commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
void callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
@@ -62,6 +70,21 @@ private:
void callbackRGBD(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image);
void callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images);
void callbackRGBD2(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2);
void callbackRGBD3(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3);
void callbackRGBD4(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4);
protected:
virtual void flushCallbacks();
@@ -71,11 +94,31 @@ private:
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
rclcpp::Subscription<rtabmap_ros::msg::RGBDImage>::SharedPtr rgbdSub_;
rclcpp::Subscription<rtabmap_ros::msg::RGBDImages>::SharedPtr rgbdxSub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_ros::msg::RGBDImage> rgbd_image4_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
rclcpp::Subscription<rtabmap_ros::msg::RGBDImage>::SharedPtr rgbdSub_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyApproxSync2Policy;
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyExactSync2Policy;
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyApproxSync3Policy;
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyExactSync3Policy;
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyApproxSync4Policy;
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
int queueSize_;
bool keepColor_;
};