i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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 ..
$ make -j4
$ sudo make install
$ sudo ldconfig
```
* RTAB-Map ROS2 package:
```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::fromROSMsg(scanOut, *pclScan);
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;
}
else
@@ -1746,7 +1746,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
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;
}
src/OdometryROS.cpp
+4 -4
View File
@@ -315,7 +315,7 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ || odometry_->canProcessIMU())
if(waitIMUToinit_ || odometry_->canProcessAsyncIMU())
{
int queueSize = 10;
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(!odometry_->canProcessIMU() &&
if(!odometry_->canProcessAsyncIMU() &&
!odometry_->getPose().isIdentity())
{
// 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(),
localTransform);
if(!odometry_->canProcessIMU())
if(!odometry_->canProcessAsyncIMU())
{
if(!odometry_->getPose().isIdentity())
{
@@ -452,7 +452,7 @@ void OdometryROS::processData(const SensorData & data, const rclcpp::Time & stam
Transform groundTruth;
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.");
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::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal).data();
if(filtered_scan_pub_->get_subscription_count())
{
@@ -297,7 +297,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
scan = util3d::laserScan2dFromPointCloud(*pclScan).data();
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_);
maxLaserScans /= scanDownsamplingStep_;
}
scan = util3d::laserScanFromPointCloud(*pclScan);
scan = util3d::laserScanFromPointCloud(*pclScan).data();
if(filtered_scan_pub_->get_subscription_count())
{
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::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal).data();
if(filtered_scan_pub_->get_subscription_count())
{
@@ -440,7 +440,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
scan = util3d::laserScanFromPointCloud(*pclScan).data();
if(filtered_scan_pub_->get_subscription_count())
{