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

data_player: fixed wrong laser scan TF published (#276)

Browse Source
This commit is contained in:
matlabbe
2018-09-28 15:20:55 -04:00
parent 8d1664857b
commit cac47900cc
2 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_robot_mapping.launch
+6 -2
View File
@@ -26,7 +26,11 @@
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/> <param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/az3/base_controller/odom"/> <!-- As /az3/base_controller/odom topic doesn't provide covariances, we use TF to get odom and we fix the covariance -->
<param name="odom_frame_id" type="string" value="odom"/>
<param name="odom_tf_linear_variance" type="double" value="0.001"/>
<param name="odom_tf_angular_variance" type="double" value="0.001"/>
<remap from="scan" to="/jn0/base_scan"/> <remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/> <remap from="rgb/image" to="/data_throttled_image"/>
@@ -42,7 +46,7 @@
<param name="RGBD/ProximityByTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM --> <param name="RGBD/ProximityByTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!-- Do also proximity detection by space by merging close scans together. --> <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!-- Do also proximity detection by space by merging close scans together. -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP --> <param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance --> <param name="Vis/MinInliers" type="string" value="12"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated --> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
<param name="RGBD/OptimizeMaxError" type="string" value="2.0"/> <!-- Reject any loop closure causing large errors (>2x link's covariance) in the map --> <param name="RGBD/OptimizeMaxError" type="string" value="2.0"/> <!-- Reject any loop closure causing large errors (>2x link's covariance) in the map -->
<param name="Reg/Force3DoF" type="string" value="true"/> <!-- 2D SLAM --> <param name="Reg/Force3DoF" type="string" value="true"/> <!-- 2D SLAM -->
src/DbPlayerNode.cpp
+2 -3
View File
@@ -109,8 +109,7 @@ int main(int argc, char** argv)
pnh.param("start_id", startId, startId); pnh.param("start_id", startId, startId);
// based on URG-04LX // based on URG-04LX
double scanHeight, scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax; double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
pnh.param<double>("scan_height", scanHeight, 0.3);
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI / 2.0); pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI / 2.0);
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI / 2.0); pnh.param<double>("scan_angle_max", scanAngleMax, M_PI / 2.0);
pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 360.0); pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 360.0);
@@ -305,7 +304,7 @@ int main(int argc, char** argv)
baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId; baseToLaserScan.header.frame_id = frameId;
baseToLaserScan.header.stamp = time; baseToLaserScan.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform); rtabmap_ros::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan); tfBroadcaster.sendTransform(baseToLaserScan);
} }
} }