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

Fixed ros2 build with latest rtabmap library (#544)

Browse Source
This commit is contained in:
matlabbe
2021-03-02 16:42:08 +00:00
parent 8ef6cff877
commit 688584db65
4 changed files with 12 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+1
View File
@@ -13,6 +13,7 @@ RTAB-Map's ROS2 package (branch `ros2`). **UNDER CONSTRUCTION**: currently most
$ cmake .. $ cmake ..
$ make -j4 $ make -j4
$ sudo make install $ sudo make install
$ sudo ldconfig
``` ```
* RTAB-Map ROS2 package: * RTAB-Map ROS2 package:
```bash ```bash
src/MsgConversion.cpp
+2 -2
View File
@@ -1738,7 +1738,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true; pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXYI; format = rtabmap::LaserScan::kXYI;
} }
else else
@@ -1746,7 +1746,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true; pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXY; format = rtabmap::LaserScan::kXY;
} }
src/OdometryROS.cpp
+4 -4
View File
@@ -315,7 +315,7 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
odomStrategy_ = 0; odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ || odometry_->canProcessIMU()) if(waitIMUToinit_ || odometry_->canProcessAsyncIMU())
{ {
int queueSize = 10; int queueSize = 10;
queueSize = this->declare_parameter("queue_size", queueSize); queueSize = this->declare_parameter("queue_size", queueSize);
@@ -354,7 +354,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
{ {
if(!this->isPaused()) if(!this->isPaused())
{ {
if(!odometry_->canProcessIMU() && if(!odometry_->canProcessAsyncIMU() &&
!odometry_->getPose().isIdentity()) !odometry_->getPose().isIdentity())
{ {
// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below // For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
@@ -382,7 +382,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
localTransform); localTransform);
if(!odometry_->canProcessIMU()) if(!odometry_->canProcessAsyncIMU())
{ {
if(!odometry_->getPose().isIdentity()) if(!odometry_->getPose().isIdentity())
{ {
@@ -452,7 +452,7 @@ void OdometryROS::processData(const SensorData & data, const rclcpp::Time & stam
Transform groundTruth; Transform groundTruth;
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{ {
if(odometry_->canProcessIMU() && data.imu().empty() && lastImuReceivedStamp_>0.0 && data.stamp() > lastImuReceivedStamp_) if(odometry_->canProcessAsyncIMU() && data.imu().empty() && lastImuReceivedStamp_>0.0 && data.stamp() > lastImuReceivedStamp_)
{ {
//RCLCPP_WARN(this->get_logger(), "Data received is more recent than last imu received, waiting for imu update to process it."); //RCLCPP_WARN(this->get_logger(), "Data received is more recent than last imu received, waiting for imu update to process it.");
if(bufferedData_.isValid()) if(bufferedData_.isValid())
src/nodelets/icp_odometry.cpp
+5 -5
View File
@@ -285,7 +285,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
} }
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); scan = util3d::laserScan2dFromPointCloud(*pclScanNormal).data();
if(filtered_scan_pub_->get_subscription_count()) if(filtered_scan_pub_->get_subscription_count())
{ {
@@ -297,7 +297,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
} }
else else
{ {
scan = util3d::laserScan2dFromPointCloud(*pclScan); scan = util3d::laserScan2dFromPointCloud(*pclScan).data();
if(filtered_scan_pub_->get_subscription_count()) if(filtered_scan_pub_->get_subscription_count())
{ {
@@ -390,7 +390,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_; maxLaserScans /= scanDownsamplingStep_;
} }
scan = util3d::laserScanFromPointCloud(*pclScan); scan = util3d::laserScanFromPointCloud(*pclScan).data();
if(filtered_scan_pub_->get_subscription_count()) if(filtered_scan_pub_->get_subscription_count())
{ {
sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2); sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
@@ -428,7 +428,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_); pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal); scan = util3d::laserScanFromPointCloud(*pclScanNormal).data();
if(filtered_scan_pub_->get_subscription_count()) if(filtered_scan_pub_->get_subscription_count())
{ {
@@ -440,7 +440,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
} }
else else
{ {
scan = util3d::laserScanFromPointCloud(*pclScan); scan = util3d::laserScanFromPointCloud(*pclScan).data();
if(filtered_scan_pub_->get_subscription_count()) if(filtered_scan_pub_->get_subscription_count())
{ {