To use turtlebot_gazebo to build a map with ethzasl_icp_mapping ( by ros-indigo)

edit

Hello, I want to use turtlebot_gazebo to build a map with ethzasl_icp_mapping in simulated environments.

The launch files and operation procedures are as follows:

<node name="dynamic_mapper" type="dynamic_mapper" pkg="ethzasl_icp_mapper" output="screen" >
    <param name="publishMapTf" value="true" /> 
    <param name="subscribe_scan" value="true" />
    <param name="subscribe_cloud" value="false" />
    <param name="useROSLogger" value="false" />
    <param name="icpConfig" value="$(find ethzasl_icp_mapper)/launch/2D_scans/icp.yaml" />
    <param name="inputFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/2D_scans/input_filters.yaml" />
    <param name="mapPostFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/2D_scans/map_post_filters.yaml" />
    <param name="odom_frame" value="odom" />
    <param name="map_frame" value="map" />
<param name="maxAngle" value="0.02" />
<param name="minOverlap" value="0.5" /> 
<param name="maxOverlapToMerge" value="0.9" /> 
<param name="minMapPointCount" value="1000" /> 
<param name="minReadingPointCount" value="150" /> 
<param name="localizing" value="true" /> 
<param name="mapping" value="true" /> 
<param name="inputQueueSize" value="10" /> 
<param name="tfRefreshPeriod" value="0.01" /> 
<param name="priorStatic" value="0.5" />

</node> <node name="occupancy_grid_builder" type="occupancy_grid_builder" pkg="ethzasl_icp_mapper" output="screen" &gt;="" <param="" name="max_range" value="5.5"/> </node>

For now I use keyboard to control the movement of turtlebot. When I control the robot to move forward, The the mapping process in rviz is normal, however, when I control the robot to rotation, the mapping process failed

Error: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "odom" from authority "unknown_publisher" because of an invalid quaternion in the transform (0.000000 0.000000 0.836740 0.547589) at line 253 in /tmp/binarydeb/ros-indigo-tf2-0.5.15/src/buffer_core.cpp
    Refer to the next link, I realize that the quaternion (0.000000 0.000000 0.836740 0.547589) is not in the tolerance margin of 10^-6

https://github.com/introlab/rtabmap_r...

To solve the question of invalid quaternion, I change the code of dynamic_mapper.cpp, before Publish tf, the quaternion corresponding to the rotation matrix (TOdomToMap) is normalized, the codes are as follows:

`double tr = TOdomToMap(0,0) + TOdomToMap(1,1) + TOdomToMap(2,2);
double q0,q1,q2,q3;
q0 = sqrt(tr+1)/2;
q1 = (TOdomToMap(1,2)-TOdomToMap(2,1))/(4q0);
q2 = (TOdomToMap(2,0)-TOdomToMap(0,2))/(4q0);
q3 = (TOdomToMap(0,1)-TOdomToMap(1,0))/(4*q0);

 double d = sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
 if (d != 0)
 {
    q0 = q0/d;
    q1 = q1/d;
    q2 = q2/d;
        q3 = q3/d;
 }
 TOdomToMap(0,0) = 1-2*q2*q2-2*q3*q3;
 TOdomToMap(0,1) = 2*q1*q2+2*q0*q3;
 TOdomToMap(0,2) = 2*q1*q2-2*q0*q2;
 TOdomToMap(1,0) = 2*q1*q2-2*q0*q3;
 TOdomToMap(1,1) = 1-2*q1*q1-2*q3*q3;
 TOdomToMap(1,2) = 2*q2*q3+2*q0*q1;
 TOdomToMap(2,0) = 2*q1*q3+2*q0*q2;
 TOdomToMap(2,1) = 2*q2*q3-2*q0*q1;
 TOdomToMap(2,2) = 1-2*q1*q1-2*q2*q2;`

When I use the modified code of dynamic_mapper.cpp, the error of an invalid quaternion in the transform is not happening. However, when I control the robot to rotation, the mapping process failed, the error infomation is as follows:

terminate called after throwing an instance of ...