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

Merged master->ros2, added vlp16 launch example, added d435i stereo launch example

Browse Source
This commit is contained in:
matlabbe
2023-01-16 16:26:53 -08:00
parent 4d86e26f54 507df071e0
commit c204ed6142
33 changed files with 636 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/lidar_deskewing.cpp
+14
View File
@@ -50,6 +50,19 @@ LidarDeskewing::~LidarDeskewing()
void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
{
// make sure the frame of the laser is updated during the whole scan time
rtabmap::Transform tmpT = rtabmap_ros::getTransform(
msg->header.frame_id,
fixedFrameId_,
msg->header.stamp,
rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
*tfBuffer_,
waitForTransformDuration_);
if(tmpT.isNull())
{
return;
}
sensor_msgs::msg::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfBuffer_);
@@ -61,6 +74,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
scanOut.header.frame_id.c_str(), msg->header.frame_id.c_str(), rtabmap_ros::timestampFromROS(msg->header.stamp));
return;
}
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
rtabmap_ros::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
pubScan_->publish(scanOutDeskewed);