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

initial port of rtabmap.launch.py

Browse Source
This commit is contained in:
matlabbe
2021-07-11 19:49:07 -04:00
parent 96c0733b0b
commit 11d8ebc339
9 changed files with 454 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+13 -1
View File
@@ -279,7 +279,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
//parse input arguments
std::vector<std::string> argList = get_node_options().arguments();
std::vector<std::string> tmpList = get_node_options().arguments();
std::vector<std::string> argList;
for(unsigned int i=0; i<tmpList.size(); ++i)
{
// Issue with ros2 launch files in which we cannot pass a
// list of strings as argument (they will appear in same string)
std::list<std::string> v = uSplit(tmpList[i]);
for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
{
argList.push_back(*iter);
}
}
char ** argv = new char*[argList.size()];
bool deleteDbOnStart = false;
for(unsigned int i=0; i<argList.size(); ++i)
src/OdometryROS.cpp
+20 -4
View File
@@ -76,6 +76,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
publishTf_(true),
waitForTransform_(0.1), // 100 ms
publishNullWhenLost_(true),
queueSize_(5),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -122,6 +123,9 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_);
waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
{
@@ -145,6 +149,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time = %f", guessMinTime_);
RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate = %f Hz", expectedUpdateRate_);
RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
RCLCPP_INFO(this->get_logger(), "Odometry: queue_size = %s", queueSize_?"true":"false");
configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
if(configPath_.size() && configPath_.at(0) != '/')
@@ -241,7 +246,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
}
}
std::vector<std::string> argList = this->get_node_options().arguments();
std::vector<std::string> tmpList = this->get_node_options().arguments();
std::vector<std::string> argList;
for(unsigned int i=0; i<tmpList.size(); ++i)
{
// Issue with ros2 launch files in which we cannot pass a
// list of strings as argument (they will appear in same string)
std::list<std::string> v = uSplit(tmpList[i]);
for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
{
argList.push_back(*iter);
}
}
char ** argv = new char*[argList.size()];
for(unsigned int i=0; i<argList.size(); ++i)
{
@@ -315,11 +332,10 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ || odometry_->canProcessAsyncIMU())
{
int queueSize = 10;
queueSize = this->declare_parameter("queue_size", queueSize);
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", queueSize*5, std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", queueSize_*5, std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
}
src/nodelets/rgbd_odometry.cpp
+17 -20
View File
@@ -53,8 +53,7 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
approxSync3_(0),
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
queueSize_(5)
exactSync4_(0)
{
OdometryROS::init(false, true, false);
}
@@ -77,7 +76,6 @@ void RGBDOdometry::onOdomInit()
bool approxSync = true;
bool subscribeRGBD = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
if(rgbdCameras <= 0)
@@ -90,7 +88,6 @@ void RGBDOdometry::onOdomInit()
}
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
@@ -115,7 +112,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -123,7 +120,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -139,7 +136,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -148,7 +145,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -166,7 +163,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -176,7 +173,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -211,12 +208,12 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
@@ -490,20 +487,20 @@ void RGBDOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -512,7 +509,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -521,7 +518,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -531,7 +528,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -541,7 +538,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -552,7 +549,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(queueSize()),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
src/nodelets/stereo_odometry.cpp
+5 -8
View File
@@ -49,8 +49,7 @@ namespace rtabmap_ros
StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
rtabmap_ros::OdometryROS("stereo_odometry", options),
approxSync_(0),
exactSync_(0),
queueSize_(5)
exactSync_(0)
{
OdometryROS::init(true, true, false);
}
@@ -66,11 +65,9 @@ void StereoOdometry::onOdomInit()
bool approxSync = false;
bool subscribeRGBD = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
std::string subscribedTopicsMsg;
@@ -93,12 +90,12 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
@@ -311,13 +308,13 @@ void StereoOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
src/nodelets/stereo_sync.cpp
+2 -2
View File
@@ -75,8 +75,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
image_transport::TransportHints hints(this);
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoLeftSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
cameraInfoRightSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
cameraInfoLeftSub_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
cameraInfoRightSub_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
get_name(),