feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,213 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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.
|
||||
*/
|
||||
|
||||
#ifndef ODOMETRYROS_H_
|
||||
#define ODOMETRYROS_H_
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
|
||||
#include <diagnostic_updater/diagnostic_updater.hpp>
|
||||
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/srv/reset_pose.hpp>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include "rtabmap_util/ULogToRosout.h"
|
||||
#include "rtabmap_sync/SyncDiagnostic.h"
|
||||
|
||||
namespace rtabmap {
|
||||
class Odometry;
|
||||
}
|
||||
|
||||
namespace rtabmap_odom {
|
||||
|
||||
class OdometryROS : public rclcpp::Node, public UThread
|
||||
{
|
||||
|
||||
public:
|
||||
explicit OdometryROS(const rclcpp::NodeOptions & options);
|
||||
explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
|
||||
virtual ~OdometryROS();
|
||||
|
||||
void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
|
||||
|
||||
void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
|
||||
void pause(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
|
||||
|
||||
const std::string & frameId() const {return frameId_;}
|
||||
const std::string & odomFrameId() const {return odomFrameId_;}
|
||||
const std::string & guessFrameId() const {return guessFrameId_;}
|
||||
const rtabmap::ParametersMap & parameters() const {return parameters_;}
|
||||
bool isPaused() const {return paused_;}
|
||||
|
||||
protected:
|
||||
void init(bool stereoParams, bool visParams, bool icpParams);
|
||||
rmw_qos_reliability_policy_t qos() const {return qos_;}
|
||||
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
|
||||
|
||||
virtual void flushCallbacks() {};
|
||||
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
|
||||
const double & waitForTransform() const {return waitForTransform_;}
|
||||
rtabmap::Transform velocityGuess() const;
|
||||
double previousStamp() const {return previousStamp_;}
|
||||
virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
|
||||
|
||||
private:
|
||||
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopKill();
|
||||
virtual void updateParameters(rtabmap::ParametersMap &) {}
|
||||
virtual void onOdomInit() {}
|
||||
|
||||
void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
|
||||
|
||||
protected:
|
||||
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
|
||||
void tick(const rclcpp::Time & stamp);
|
||||
|
||||
private:
|
||||
rtabmap::Odometry * odometry_;
|
||||
|
||||
// parameters
|
||||
std::string frameId_;
|
||||
std::string odomFrameId_;
|
||||
std::string groundTruthFrameId_;
|
||||
std::string groundTruthBaseFrameId_;
|
||||
std::string guessFrameId_;
|
||||
double guessMinTranslation_;
|
||||
double guessMinRotation_;
|
||||
double guessMinTime_;
|
||||
bool publishTf_;
|
||||
double waitForTransform_;
|
||||
bool publishNullWhenLost_;
|
||||
bool publishCompressedSensorData_;
|
||||
rmw_qos_reliability_policy_t qos_;
|
||||
rtabmap::ParametersMap parameters_;
|
||||
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoLitePub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalMap_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalScanMap_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLastFrame_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr odomRgbdImagePub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataFeaturesPub_;
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr odomSensorDataCompressedPub_;
|
||||
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetSrv_;
|
||||
rclcpp::Service<rtabmap_msgs::srv::ResetPose>::SharedPtr resetToPoseSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogDebugSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogInfoSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogWarnSrv_;
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogErrorSrv_;
|
||||
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
|
||||
|
||||
// Safe-threading
|
||||
UMutex imuMutex_;
|
||||
UMutex dataMutex_;
|
||||
USemaphore dataReady_;
|
||||
rtabmap::SensorData dataToProcess_;
|
||||
std_msgs::msg::Header dataHeaderToProcess_;
|
||||
bool bufferedDataToProcess_;
|
||||
|
||||
bool paused_;
|
||||
int resetCountdown_;
|
||||
int resetCurrentCount_;
|
||||
bool stereoParams_;
|
||||
bool visParams_;
|
||||
bool icpParams_;
|
||||
rtabmap::Transform guess_;
|
||||
rtabmap::Transform guessPreviousPose_;
|
||||
double previousStamp_;
|
||||
double previousClockTime_;
|
||||
double expectedUpdateRate_;
|
||||
double maxUpdateRate_;
|
||||
double minUpdateRate_;
|
||||
std::string compressionImgFormat_;
|
||||
bool compressionParallelized_;
|
||||
int odomStrategy_;
|
||||
bool waitIMUToinit_;
|
||||
bool alwaysCheckImuTf_;
|
||||
bool imuProcessed_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr> imus_;
|
||||
std::string configPath_;
|
||||
rtabmap::Transform initialPose_;
|
||||
rtabmap::Transform imuLocalTransform_;
|
||||
|
||||
rtabmap_util::ULogToRosout ulogToRosout_;
|
||||
|
||||
class OdomStatusTask : public diagnostic_updater::DiagnosticTask
|
||||
{
|
||||
public:
|
||||
OdomStatusTask();
|
||||
void setStatus(bool isLost, int processedMsgs, int droppedMsgs);
|
||||
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
|
||||
private:
|
||||
bool lost_;
|
||||
bool dataReceived_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
};
|
||||
OdomStatusTask statusDiagnostic_;
|
||||
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
//#include <pluginlib/class_list_macros.h>
|
||||
//#include <pluginlib/class_loader.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
//#include "rtabmap_ros/PluginInterface.h"
|
||||
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class ICPOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
explicit ICPOdometry(const rclcpp::NodeOptions & options);
|
||||
|
||||
virtual ~ICPOdometry();
|
||||
|
||||
private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap &);
|
||||
virtual void onOdomInit();
|
||||
|
||||
void callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg);
|
||||
void callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg);
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
void postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const;
|
||||
|
||||
private:
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_scan_pub_;
|
||||
int scanCloudMaxPoints_;
|
||||
bool scanCloudIs2d_;
|
||||
int scanDownsamplingStep_;
|
||||
double scanRangeMin_;
|
||||
double scanRangeMax_;
|
||||
double scanVoxelSize_;
|
||||
int scanNormalK_;
|
||||
double scanNormalRadius_;
|
||||
double scanNormalGroundUp_;
|
||||
bool deskewing_;
|
||||
bool deskewingSlerp_;
|
||||
//std::vector<std::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
|
||||
//pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
|
||||
bool scanReceived_ = false;
|
||||
bool cloudReceived_ = false;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class RGBDOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
explicit RGBDOdometry(const rclcpp::NodeOptions & options);
|
||||
virtual ~RGBDOdometry();
|
||||
|
||||
private:
|
||||
virtual void updateParameters(rtabmap::ParametersMap & parameters);
|
||||
virtual void onOdomInit();
|
||||
|
||||
void commonCallback(
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
|
||||
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
|
||||
|
||||
void callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr image,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
|
||||
|
||||
void callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
|
||||
|
||||
void callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
|
||||
|
||||
void callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
|
||||
|
||||
void callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
|
||||
|
||||
void callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
|
||||
|
||||
void callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
|
||||
|
||||
void callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
|
||||
private:
|
||||
image_transport::SubscriberFilter image_mono_sub_;
|
||||
image_transport::SubscriberFilter image_depth_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> info_sub_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, 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> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
|
||||
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
|
||||
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
|
||||
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
|
||||
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
|
||||
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
|
||||
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
|
||||
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
|
||||
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
|
||||
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
|
||||
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
bool keepColor_;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <rtabmap_odom/OdometryROS.h>
|
||||
#include <rtabmap_odom/visibility.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <image_transport/subscriber_filter.hpp>
|
||||
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
namespace rtabmap_odom
|
||||
{
|
||||
|
||||
class StereoOdometry : public rtabmap_odom::OdometryROS
|
||||
{
|
||||
public:
|
||||
RTABMAP_ODOM_PUBLIC
|
||||
StereoOdometry(const rclcpp::NodeOptions & options);
|
||||
virtual ~StereoOdometry();
|
||||
|
||||
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,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
|
||||
|
||||
void callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
|
||||
void callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
|
||||
void callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
|
||||
void callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
|
||||
void callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
|
||||
void callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
|
||||
void callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
|
||||
|
||||
|
||||
protected:
|
||||
virtual void flushCallbacks();
|
||||
|
||||
private:
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
|
||||
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
|
||||
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_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_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
|
||||
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
|
||||
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
|
||||
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
|
||||
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
|
||||
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
|
||||
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
|
||||
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
|
||||
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
|
||||
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
|
||||
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
|
||||
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
|
||||
|
||||
int topicQueueSize_;
|
||||
int syncQueueSize_;
|
||||
bool keepColor_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright 2016 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RTABMAP_ODOM__VISIBILITY_CONTROL_H_
|
||||
#define RTABMAP_ODOM__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define RTABMAP_ODOM_EXPORT __attribute__ ((dllexport))
|
||||
#define RTABMAP_ODOM_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define RTABMAP_ODOM_EXPORT __declspec(dllexport)
|
||||
#define RTABMAP_ODOM_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef RTABMAP_ODOM_BUILDING_DLL
|
||||
#define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_EXPORT
|
||||
#else
|
||||
#define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_IMPORT
|
||||
#endif
|
||||
#define RTABMAP_ODOM_PUBLIC_TYPE RTABMAP_ODOM_PUBLIC
|
||||
#define RTABMAP_ODOM_LOCAL
|
||||
#else
|
||||
#define RTABMAP_ODOM_EXPORT __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_ODOM_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define RTABMAP_ODOM_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define RTABMAP_ODOM_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define RTABMAP_ODOM_PUBLIC
|
||||
#define RTABMAP_ODOM_LOCAL
|
||||
#endif
|
||||
#define RTABMAP_ODOM_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // RTABMAP_ODOM__VISIBILITY_CONTROL_H_
|
||||
Reference in New Issue
Block a user