Skip to content

Commit

Permalink
merged master->ros2
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Jan 4, 2024
1 parent 2f365ab commit 80ae448
Show file tree
Hide file tree
Showing 14 changed files with 493 additions and 131 deletions.
1 change: 1 addition & 0 deletions .github/workflows/ros1.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ jobs:
sudo apt-get update
sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools
sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
sudo pip3 uninstall empy --yes
- name: Setup catkin workspace
run: |
Expand Down
6 changes: 6 additions & 0 deletions rtabmap_conversions/src/MsgConversion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2850,6 +2850,12 @@ bool deskew_impl(
if(offsetTime < 0)
{
UERROR("Input cloud doesn't have \"t\", \"time\", \"stamps\" or \"timestamp\" field!");
std::string fieldsReceived;
for(size_t i=0; i<input.fields.size(); ++i)
{
fieldsReceived += input.fields[i].name + " ";
}
UERROR("Input cloud has these fields: %s", fieldsReceived.c_str());
return false;
}
if(offsetX < 0)
Expand Down
95 changes: 0 additions & 95 deletions rtabmap_demos/launch/demo_unitree_quadruped_robot.launch
View file

This file was deleted.

13 changes: 13 additions & 0 deletions rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,14 @@ class RGBDOdometry : public rtabmap_odom::OdometryROS
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();

Expand All @@ -110,6 +118,7 @@ class RGBDOdometry : public rtabmap_odom::OdometryROS
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_;
Expand All @@ -131,6 +140,10 @@ class RGBDOdometry : public rtabmap_odom::OdometryROS
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 queueSize_;
bool keepColor_;
};
Expand Down
23 changes: 23 additions & 0 deletions rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,19 @@ class StereoOdometry : public rtabmap_odom::OdometryROS
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:
Expand All @@ -101,6 +114,8 @@ class StereoOdometry : public rtabmap_odom::OdometryROS
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_;
Expand All @@ -118,6 +133,14 @@ class StereoOdometry : public rtabmap_odom::OdometryROS
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 queueSize_;
bool keepColor_;
Expand Down
1 change: 1 addition & 0 deletions rtabmap_odom/src/nodelets/icp_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,6 +317,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
*scanMsg,
scanOut,
this->tfBuffer(),
-1.0f,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);

if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
Expand Down
116 changes: 112 additions & 4 deletions rtabmap_odom/src/nodelets/rgbd_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
keepColor_(false)
{
Expand All @@ -75,6 +77,8 @@ RGBDOdometry::~RGBDOdometry()
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
delete approxSync6_;
delete exactSync6_;
}

void RGBDOdometry::onOdomInit()
Expand All @@ -93,10 +97,6 @@ void RGBDOdometry::onOdomInit()
{
rgbdCameras = 1;
}
if(rgbdCameras > 5)
{
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
}
keepColor_ = this->declare_parameter("keep_color", keepColor_);

RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
Expand Down Expand Up @@ -129,6 +129,10 @@ void RGBDOdometry::onOdomInit()
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}

if(rgbdCameras == 2)
{
Expand Down Expand Up @@ -256,6 +260,54 @@ void RGBDOdometry::onOdomInit()
rgbd_image4_sub_.getSubscriber()->get_topic_name(),
rgbd_image5_sub_.getSubscriber()->get_topic_name());
}
else if(rgbdCameras == 6)
{
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str(),
rgbd_image5_sub_.getTopic().c_str(),
rgbd_image6_sub_.getTopic().c_str());
}
else
{
RCLCPP_FATAL(this->get_logger(),
"%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
"internal synchronization interface, set rgbd_cameras=0 and use "
"rgbd_images input topic instead for more cameras (for which "
"rgbdx_sync node can sync up to 8 cameras).",
get_name(), rgbdCameras);
}
}
else if(rgbdCameras == 0)
{
Expand Down Expand Up @@ -649,6 +701,36 @@ void RGBDOdometry::callbackRGBD5(
}
}

void RGBDOdometry::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)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]);
infoMsgs.push_back(image->rgb_camera_info);
infoMsgs.push_back(image2->rgb_camera_info);
infoMsgs.push_back(image3->rgb_camera_info);
infoMsgs.push_back(image4->rgb_camera_info);
infoMsgs.push_back(image5->rgb_camera_info);
infoMsgs.push_back(image6->rgb_camera_info);

this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}

void RGBDOdometry::flushCallbacks()
{
// flush callbacks
Expand Down Expand Up @@ -748,6 +830,32 @@ void RGBDOdometry::flushCallbacks()
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(approxSync6_)
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
if(exactSync6_)
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
}

}
Expand Down
Loading

0 comments on commit 80ae448

Please sign in to comment.