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

Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel

Browse Source
This commit is contained in:
matlabbe
2024-05-27 12:47:43 -07:00
parent e874413a5c 0dc233c840
commit 9ab0f93835
14 changed files with 89 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/scripts/yaml_to_camera_info.py
+16 -1
View File
@@ -8,7 +8,7 @@ from sensor_msgs.msg import Image
def yaml_to_CameraInfo(yaml_fname):
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
calib_data = yaml.load(file_handle, Loader=yaml.FullLoader)
msg = CameraInfo()
msg.width = calib_data["image_width"]
@@ -26,7 +26,9 @@ class YamlToCameraInfo(Node):
super().__init__('yaml_to_camera_info')
self.declare_parameter('yaml_path', '')
self.declare_parameter('scale', 1.0)
yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
scale = self.get_parameter('scale').get_parameter_value().double_value
if not yaml_path:
print('yaml_path parameter should be set to path of the calibration file!')
@@ -36,6 +38,19 @@ class YamlToCameraInfo(Node):
self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
if scale!=1.0:
self.camera_info_msg.k[0] = self.camera_info_msg.k[0]*scale
self.camera_info_msg.k[2] = self.camera_info_msg.k[2]*scale
self.camera_info_msg.k[4] = self.camera_info_msg.k[4]*scale
self.camera_info_msg.k[5] = self.camera_info_msg.k[5]*scale
self.camera_info_msg.p[0] = self.camera_info_msg.p[0]*scale
self.camera_info_msg.p[2] = self.camera_info_msg.p[2]*scale
self.camera_info_msg.p[3] = self.camera_info_msg.p[3]*scale
self.camera_info_msg.p[5] = self.camera_info_msg.p[5]*scale
self.camera_info_msg.p[6] = self.camera_info_msg.p[6]*scale
self.camera_info_msg.width = int(self.camera_info_msg.width*scale)
self.camera_info_msg.height = int(self.camera_info_msg.height*scale)
self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
self.subscription = self.create_subscription(
rtabmap_util/src/nodelets/lidar_deskewing.cpp
+1 -1
View File
@@ -51,7 +51,7 @@ 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_conversions::getTransform(
rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
msg->header.frame_id,
fixedFrameId_,
msg->header.stamp,
rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+2 -2
View File
@@ -289,11 +289,11 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
{
// approx sync
cloudDisplacement = rtabmap_conversions::getTransform(
cloudDisplacement = rtabmap_conversions::getMovingTransform(
frameId, //sourceTargetFrame
fixedFrameId_, //fixedFrame
cloudMsgs[i]->header.stamp, //stampSource
cloudMsgs[0]->header.stamp, //stampTarget
cloudMsgs[i]->header.stamp, //stampSource
*tfBuffer_,
waitForTransform_);
}
rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+2 -2
View File
@@ -138,11 +138,11 @@ void PointCloudToDepthImage::callback(
if(!fixedFrameId_.empty())
{
// approx sync
cloudDisplacement = rtabmap_conversions::getTransform(
cloudDisplacement = rtabmap_conversions::getMovingTransform(
pointCloud2Msg->header.frame_id,
fixedFrameId_,
cameraInfoMsg->header.stamp,
pointCloud2Msg->header.stamp,
cameraInfoMsg->header.stamp,
*tfBuffer_,
waitForTransform_);
}