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

[Liying0824]

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

1. The launch files and operation procedures are as follows:
   `<!--
   This launch file is an example for 2D scan matching

It has been parametrized for the PR2 rosbag that can be downloaded at
wget http://pr.willowgarage.com/data/gmapping/small_loop_prf.bag
-->

<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" >
	<param name="max_range" value="5.5" />
</node>

`

2. 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

3. 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_ros/issues/172

4. 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;`
   

5. 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 'PointMatcherSupport::TransformationError' what(): RigidTransformation: Error, rotation matrix is not orthogonal. [dynamic_mapper-1] process has died [pid 11410, exit code -6, cmd /home/liying/icp_ws/devel_isolated/ethzasl_icp_mapper/lib/ethzasl_icp_mapper/dynamic_mapper __name:=dynamic_mapper __log:=/home/liying/.ros/log/47f8b690-678c-11e7-81c4-4ccc6a8d4c37/dynamic_mapper-1.log]. log file: /home/liying/.ros/log/47f8b690-678c-11e7-81c4-4ccc6a8d4c37/dynamic_mapper-1*.log
   Can you give me a hint? Many thanks!