gazebo function test. successfully implement ceres scan matching function according to relative codes in cartographer (built in release mode)

.gitignore

@@ -50,3 +50,5 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+
+grapher/cmake-build-debug/**

MPC/launch/gazebo.launch

@@ -0,0 +1,14 @@
+<launch>
+	<!-- <arg name="model" />
+	<arg name="gui" default="False" />
+	<param name="robot_description" textfile="$(find MPC)/urdf/diff_model.urdf" />
+	<param name="use_gui" value="$(arg gui)"/>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" /> -->
+
+    <!-- push table_description to factory and spawn robot in gazebo -->
+  <node name="spawn_vehicle" pkg="gazebo" type="spawn_model" args="
+  -urdf -file $(find MPC)/urdf/diff_model.urdf -z 1 -model table_model" 
+  respawn="false" output="screen" />
+</launch>

MPC/launch/gazebo.sh

@@ -0,0 +1,9 @@
+###
+ # @Description: 
+ # @Author: yct
+ # @Date: 2020-04-30 09:40:37
+ # @LastEditTime: 2020-04-30 09:40:55
+ # @LastEditors: yct
+ ###
+
+roslaunch gazebo_ros gazebo.launch

MPC/launch/gazebo_launch/elevator_world.launch

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="extra_gazebo_args" default=""/>
+  <arg name="gui" default="true"/>
+  <arg name="recording" default="false"/>
+  <!-- Note that 'headless' is currently non-functional.  See gazebo_ros_pkgs issue #491 (-r arg does not disable
+       rendering, but instead enables recording). The arg definition has been left here to prevent breaking downstream
+       launch files, but it does nothing. -->
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+  <arg name="physics" default="ode"/>
+  <arg name="verbose" default="true"/>
+  <arg name="output" default="screen"/>
+  <arg name="world_name" default="worlds/elevator_ros.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+
+  <!-- set use_sim_time flag -->
+  <group if="$(arg use_sim_time)">
+    <param name="/use_sim_time" value="true" />
+  </group>
+
+  <!-- set command arguments -->
+  <arg unless="$(arg paused)" name="command_arg1" value=""/>
+  <arg     if="$(arg paused)" name="command_arg1" value="-u"/>
+  <arg unless="$(arg recording)" name="command_arg2" value=""/>
+  <arg     if="$(arg recording)" name="command_arg2" value="-r"/>
+  <arg unless="$(arg verbose)" name="command_arg3" value=""/>
+  <arg     if="$(arg verbose)" name="command_arg3" value="--verbose"/>
+  <arg unless="$(arg debug)" name="script_type" value="gzserver"/>
+  <arg     if="$(arg debug)" name="script_type" value="debug"/>
+
+  <!-- start gazebo server-->
+  <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="$(arg output)"
+  args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
+
+  <!-- start gazebo client -->
+  <group if="$(arg gui)">
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="$(arg output)"/>
+  </group>
+
+</launch>

MPC/launch/gazebo_launch/empty_world.launch

@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="extra_gazebo_args" default=""/>
+  <arg name="gui" default="true"/>
+  <arg name="recording" default="false"/>
+  <!-- Note that 'headless' is currently non-functional.  See gazebo_ros_pkgs issue #491 (-r arg does not disable
+       rendering, but instead enables recording). The arg definition has been left here to prevent breaking downstream
+       launch files, but it does nothing. -->
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+  <arg name="physics" default="ode"/>
+  <arg name="verbose" default="false"/>
+  <arg name="output" default="screen"/>
+  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+  <arg name="respawn_gazebo" default="false"/>
+  <arg name="use_clock_frequency" default="false"/>
+  <arg name="pub_clock_frequency" default="100"/>
+
+  <!-- set use_sim_time flag -->
+  <param name="/use_sim_time" value="$(arg use_sim_time)"/>
+
+  <!-- set command arguments -->
+  <arg unless="$(arg paused)" name="command_arg1" value=""/>
+  <arg     if="$(arg paused)" name="command_arg1" value="-u"/>
+  <arg unless="$(arg recording)" name="command_arg2" value=""/>
+  <arg     if="$(arg recording)" name="command_arg2" value="-r"/>
+  <arg unless="$(arg verbose)" name="command_arg3" value=""/>
+  <arg     if="$(arg verbose)" name="command_arg3" value="--verbose"/>
+  <arg unless="$(arg debug)" name="script_type" value="gzserver"/>
+  <arg     if="$(arg debug)" name="script_type" value="debug"/>
+
+  <!-- start gazebo server-->
+  <group if="$(arg use_clock_frequency)">
+    <param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
+  </group>
+  <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="$(arg output)"
+  args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
+
+  <!-- start gazebo client -->
+  <group if="$(arg gui)">
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="$(arg output)" args="$(arg command_arg3)"/>
+  </group>
+
+</launch>

MPC/launch/gazebo_launch/mud_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="worlds/mud.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/> <!-- Inert - see gazebo_ros_pkgs issue #491 -->
+    <arg name="recording" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+</launch>

MPC/launch/gazebo_launch/range_world.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="extra_gazebo_args" default=""/>
+  <arg name="gui" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="physics" default="ode"/>
+  <arg name="verbose" default="true"/>
+  <arg name="output" default="screen"/>
+  <arg name="world" default="gazebo_ros_range"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find gazebo_plugins)/test/test_worlds/$(arg world).world"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="physics" value="$(arg physics)"/>
+    <arg name="verbose" value="$(arg verbose)"/>
+    <arg name="output" value="$(arg output)"/>
+  </include>
+
+</launch>
+

MPC/launch/gazebo_launch/rubble_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="worlds/rubble.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/> <!-- Inert - see gazebo_ros_pkgs issue #491 -->
+    <arg name="recording" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+</launch>

MPC/launch/gazebo_launch/shapes_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="worlds/shapes.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/> <!-- Inert - see gazebo_ros_pkgs issue #491 -->
+    <arg name="recording" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+</launch>

MPC/launch/gazebo_launch/willowgarage_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="worlds/willowgarage.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/> <!-- Inert - see gazebo_ros_pkgs issue #491 -->
+    <arg name="recording" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+</launch>

gazebo_robot/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

gazebo_robot/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

gazebo_robot/config/robot_arbotix.yaml

@@ -0,0 +1,12 @@
+port: /dev/ttyUSB0
+baud: 115200
+rate: 20
+sync_write: True
+sync_read: True
+read_rate: 20
+write_rate: 20
+ 
+controllers: {
+   #  Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100)
+   base_controller: {type: diff_controller, base_frame_id: base_link, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
+}

gazebo_robot/launch/robot_display.rviz.launch

@@ -0,0 +1,27 @@
+<launch>
+  <param name="/use_sim_time" value="false" />
+ 
+	<!-- Load the URDF/Xacro model of our robot -->
+  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find gazebo_robot)/urdf/robot.urdf.xacro'" />
+	<arg name="gui" default="false" />
+ 
+	<param name="robot_description" command="$(arg urdf_file)" />
+	<param name="use_gui" value="$(arg gui)"/>
+ 
+	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
+    <rosparam file="$(find gazebo_robot)/config/robot_arbotix.yaml" command="load" />
+    <param name="sim" value="true"/>
+  </node>
+ 
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
+	</node>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
+    <param name="publish_frequency" type="double" value="20.0" />
+  </node>
+ 
+	<!-- We need a static transforms for the wheels -->
+  <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" />
+  <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" />
+ 
+	<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find gazebo_robot)/urdf.vcg" />-->
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gazebo_robot)/urdf/disp_robot.rviz" />

gazebo_robot/manifest.xml

@@ -0,0 +1,16 @@
+<package>
+  <description brief="gazebo_robot">
+
+     gazebo_robot
+
+  </description>
+  <author>youchen</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/gazebo_robot</url>
+  <depend package="roscpp"/>
+  <depend package="gazebo_ros"/>
+
+</package>
+
+

gazebo_robot/urdf/gazebo.urdf.xacro

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+ 
+<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
+	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" 
+	xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
+	xmlns:xacro="http://ros.org/wiki/xacro" 
+	name="mecanum_robot_gazebo">
+ 
+<!-- ASUS Xtion PRO camera for simulation -->
+<!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package -->
+<xacro:macro name="mecanum_robot_sim">
+    <gazebo reference="base_link">
+        <material>Gazebo/Blue</material>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+ 
+    <gazebo reference="right_front_wheel">
+        <material>Gazebo/FlatBlack</material>
+	</gazebo>
+ 
+	<gazebo reference="right_back_wheel">
+        <material>Gazebo/FlatBlack</material>
+    </gazebo>
+ 
+    <gazebo reference="left_front_wheel">
+        <material>Gazebo/FlatBlack</material>
+    </gazebo>
+ 
+    <gazebo reference="left_back_wheel">
+        <material>Gazebo/FlatBlack</material>
+    </gazebo>
+ 
+    <gazebo reference="head">
+        <material>Gazebo/White</material>
+    </gazebo>
+ 
+</xacro:macro>
+ 
+</robot>

gazebo_robot/urdf/robot.urdf.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+ 
+<robot name="mecanum_robot"  
+    xmlns:xi="http://www.w3.org/2001/XInclude"
+	xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+    xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+	xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+	xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+    xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+    xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+	xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+    xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+    xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
+	xmlns:xacro="http://ros.org/wiki/xacro">
+ 
+  <include filename="$(find gazebo_robot)/urdf/robot_body.urdf.xacro" />
+ 
+  <!-- Body of mecanum_robot, with plates, standoffs and Create (including sim sensors) -->
+  <mecanum_robot_body/>
+ 
+  <mecanum_robot_sim/>
+ 
+</robot>

gazebo_robot/urdf/robot_body.urdf.xacro

@@ -0,0 +1,192 @@
+<?xml version="1.0"?>
+<robot name="mecanum_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+  <property name="M_PI" value="3.14159"/>
+ 
+  <!-- Macro for mecanum_robot body. Including Gazebo extensions, but does not include lidar -->
+  <include filename="$(find gazebo_robot)/urdf/gazebo.urdf.xacro"/>
+ 
+  <property name="base_x" value="0.33" />
+  <property name="base_y" value="0.33" />
+ 
+  <xacro:macro name="mecanum_robot_body">
+ 
+ 
+	<link name="base_link">
+	<inertial>
+      <origin xyz="0 0 0.055"/>
+      <mass value="100.0" />
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="0.25 .16 .05"/>
+      </geometry>
+	  <origin rpy="0 0 0" xyz="0 0 0.055"/>
+      <material name="blue">
+	  <color rgba="0 0 .8 1"/>
+      </material>
+   </visual>
+   <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.055"/>
+      <geometry>
+        <box size="0.25 .16 .05" />
+      </geometry>
+    </collision>
+  </link>
+ 
+ 
+ <link name="left_front_wheel">
+	<inertial>
+      <origin  xyz="0.08 0.08 0.025"/>
+      <mass value="1" />
+       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <cylinder length=".02" radius="0.025"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
+      <geometry>
+         <cylinder length=".02" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+ 
+  <joint name="left_front_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="left_front_wheel"/>
+    <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+ 
+  <link name="right_front_wheel">
+	<inertial>
+      <origin xyz="0.08 -0.08 0.025"/>
+      <mass value="1" />
+       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <cylinder length=".02" radius="0.025"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
+      <geometry>
+         <cylinder length=".02" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+ 
+  <joint name="right_front_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="right_front_wheel"/>
+    <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+ </joint>
+ 
+ <link name="left_back_wheel">
+    <inertial>
+      <origin xyz="-0.08 0.08 0.025"/>
+      <mass value="1" />
+       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <cylinder length=".02" radius="0.025"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+   </visual>
+   <collision>
+       <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
+      <geometry>
+         <cylinder length=".02" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+ 
+  <joint name="left_back_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="left_back_wheel"/>
+    <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+ 
+  <link name="right_back_wheel">
+	<inertial>
+       <origin xyz="-0.08 -0.08 0.025"/>
+       <mass value="1" />
+       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <cylinder length=".02" radius="0.025"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+   </visual>
+   <collision>
+      <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
+      <geometry>
+         <cylinder length=".02" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+ 
+ 
+  <joint name="right_back_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="right_back_wheel"/>
+    <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+ 
+  <link name="head">
+	<inertial>
+      <origin xyz="0.08 0 0.08"/>
+      <mass value="1" />
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size=".02 .03 .03"/>
+      </geometry>
+	  <material name="white">
+		<color rgba="1 1 1 1"/>
+	  </material>
+     </visual>
+     <collision>
+      <origin xyz="0.08 0 0.08"/>
+      <geometry>
+         <cylinder length=".02" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+ 
+  <joint name="tobox" type="fixed">
+    <parent link="base_link"/>
+    <child link="head"/>
+    <origin xyz="0.08 0 0.08"/>
+  </joint>
+  </xacro:macro>
+ 
+</robot>

grapher/CMakeLists.txt

@@ -63,19 +63,19 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/ndt NDT_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/cfg CFG_SRC )
 set(SRC_LIST  src/steger/detectlines.cpp src/steger/detectlines.h src/steger/Steger.cpp src/steger/Steger.cu)
 
-# cuda_add_executable(grapher_node src/grapher_node.cpp src/scanGrapher.cpp src/locateTask.cpp
-#                       src/kalmanFilter.cpp ${TASK_SRC} ${NDT_SRC} ${SRC_LIST} ${CFG_SRC})
-# target_include_directories(grapher_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
-#                             ${PROTOBUF_INCLUDE_DIRS})
-# target_link_libraries(grapher_node ${catkin_LIBRARIES} ${CUDA_LIBRARIES} ${PROTOBUF_LIBRARIES}
-#             ${PCL_LIBRARIES} ${OpenCV_LIBS} libcsm.so)
+cuda_add_executable(grapher_node src/grapher_node.cpp src/scanGrapher.cpp src/locateTask.cpp
+                      src/kalmanFilter.cpp ${TASK_SRC} ${NDT_SRC} ${SRC_LIST} ${CFG_SRC})
+target_include_directories(grapher_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
+                            ${PROTOBUF_INCLUDE_DIRS})
+target_link_libraries(grapher_node ${catkin_LIBRARIES} ${CUDA_LIBRARIES} ${PROTOBUF_LIBRARIES}
+            ${PCL_LIBRARIES} ${OpenCV_LIBS} libcsm.so)
 ###############
 
 
-# add_executable(location_node src/location_node.cpp src/locateTask.cpp src/kalmanFilter.cpp ${NDT_SRC} 
-#                       ${TASK_SRC} ${CFG_SRC})
-# target_include_directories(location_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
-#                             ${PROTOBUF_INCLUDE_DIRS})
-# target_link_libraries(location_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} 
-#                       libcsm.so ${PROTOBUF_LIBRARIES})
+add_executable(location_node src/location_node.cpp src/locateTask.cpp src/kalmanFilter.cpp ${NDT_SRC} 
+                      ${TASK_SRC} ${CFG_SRC})
+target_include_directories(location_node PRIVATE "${CMAKE_CURRENT_LIST_DIR}/src/TaskQueue/threadpp" 
+                            ${PROTOBUF_INCLUDE_DIRS})
+target_link_libraries(location_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} 
+                      libcsm.so ${PROTOBUF_LIBRARIES})
 

grapher/scripts/demo_sick.launch

@@ -7,7 +7,7 @@ Example launch file: launches the scan matcher with pre-recorded data
   <arg name="use_rviz" default="true" />
   #### set up data playback from bag #############################
 
-  <param name="/use_sim_time" value="true"/>
+  <param name="/use_sim_time" value="false"/>
   <!-- <param name="/stamped_vel" value="$(arg IS_TWISTSTAMPED)"/> -->
 
   <group if="$(arg use_rviz)">
@@ -23,7 +23,10 @@ Example launch file: launches the scan matcher with pre-recorded data
   <!-- <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
     args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /base_link /laser 40" />-->
 
-  #### start the laser scan_matcher ##############################
+<!--  <node pkg="grapher" type="grapher_node"-->
+<!--        name="grapher_node" output="screen">-->
+<!--  </node>-->
+
   <node pkg="grapher" type="location_node"
     name="location_node" output="screen">
   </node>

grapher/src/cfg/grapher_cfg.pb.cc

@@ -1,7 +1,6 @@
 // Generated by the protocol buffer compiler.  DO NOT EDIT!
 // source: grapher_cfg.proto
 
-#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
 #include "grapher_cfg.pb.h"
 
 #include <algorithm>
@@ -15,80 +14,103 @@
 #include <google/protobuf/generated_message_reflection.h>
 #include <google/protobuf/reflection_ops.h>
 #include <google/protobuf/wire_format.h>
+// This is a temporary google only hack
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+#include "third_party/protobuf/version.h"
+#endif
 // @@protoc_insertion_point(includes)
-
 namespace grapher {
 class locatefuncDefaultTypeInternal {
-public:
- ::google::protobuf::internal::ExplicitlyConstructed<locatefunc>
-     _instance;
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<locatefunc>
+      _instance;
 } _locatefunc_default_instance_;
 class grapherCfgDefaultTypeInternal {
-public:
- ::google::protobuf::internal::ExplicitlyConstructed<grapherCfg>
-     _instance;
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<grapherCfg>
+      _instance;
 } _grapherCfg_default_instance_;
-
+}  // namespace grapher
 namespace protobuf_grapher_5fcfg_2eproto {
+void InitDefaultslocatefuncImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
 
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  {
+    void* ptr = &::grapher::_locatefunc_default_instance_;
+    new (ptr) ::grapher::locatefunc();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::grapher::locatefunc::InitAsDefaultInstance();
+}
 
-namespace {
+void InitDefaultslocatefunc() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultslocatefuncImpl);
+}
 
-::google::protobuf::Metadata file_level_metadata[2];
+void InitDefaultsgrapherCfgImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
 
-}  // namespace
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  protobuf_grapher_5fcfg_2eproto::InitDefaultslocatefunc();
+  {
+    void* ptr = &::grapher::_grapherCfg_default_instance_;
+    new (ptr) ::grapher::grapherCfg();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::grapher::grapherCfg::InitAsDefaultInstance();
+}
 
-PROTOBUF_CONSTEXPR_VAR ::google::protobuf::internal::ParseTableField
-    const TableStruct::entries[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  {0, 0, 0, ::google::protobuf::internal::kInvalidMask, 0, 0},
-};
+void InitDefaultsgrapherCfg() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultsgrapherCfgImpl);
+}
 
-PROTOBUF_CONSTEXPR_VAR ::google::protobuf::internal::AuxillaryParseTableField
-    const TableStruct::aux[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  ::google::protobuf::internal::AuxillaryParseTableField(),
-};
-PROTOBUF_CONSTEXPR_VAR ::google::protobuf::internal::ParseTable const
-    TableStruct::schema[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  { NULL, NULL, 0, -1, -1, -1, -1, NULL, false },
-  { NULL, NULL, 0, -1, -1, -1, -1, NULL, false },
-};
+::google::protobuf::Metadata file_level_metadata[2];
 
-const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(locatefunc, _has_bits_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(locatefunc, _internal_metadata_),
+const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::locatefunc, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::locatefunc, _internal_metadata_),
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(locatefunc, type_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(locatefunc, tfheaderid_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(locatefunc, tfid_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::locatefunc, type_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::locatefunc, tfheaderid_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::locatefunc, tfid_),
   0,
   1,
   2,
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grapherCfg, _has_bits_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grapherCfg, _internal_metadata_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::grapherCfg, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::grapherCfg, _internal_metadata_),
   ~0u,  // no _extensions_
   ~0u,  // no _oneof_case_
   ~0u,  // no _weak_field_map_
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grapherCfg, scanid_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grapherCfg, grapher_functions_),
-  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grapherCfg, locate_functions_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::grapherCfg, scanid_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::grapherCfg, grapher_functions_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::grapher::grapherCfg, locate_functions_),
   0,
   ~0u,
   ~0u,
 };
-static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
-  { 0, 8, sizeof(locatefunc)},
-  { 11, 19, sizeof(grapherCfg)},
+static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 8, sizeof(::grapher::locatefunc)},
+  { 11, 19, sizeof(::grapher::grapherCfg)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
-  reinterpret_cast<const ::google::protobuf::Message*>(&_locatefunc_default_instance_),
-  reinterpret_cast<const ::google::protobuf::Message*>(&_grapherCfg_default_instance_),
+  reinterpret_cast<const ::google::protobuf::Message*>(&::grapher::_locatefunc_default_instance_),
+  reinterpret_cast<const ::google::protobuf::Message*>(&::grapher::_grapherCfg_default_instance_),
 };
 
-namespace {
-
 void protobuf_AssignDescriptors() {
   AddDescriptors();
   ::google::protobuf::MessageFactory* factory = NULL;
@@ -102,31 +124,15 @@ void protobuf_AssignDescriptorsOnce() {
   ::google::protobuf::GoogleOnceInit(&once, &protobuf_AssignDescriptors);
 }
 
-void protobuf_RegisterTypes(const ::std::string&) GOOGLE_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_PROTOBUF_ATTRIBUTE_COLD;
 void protobuf_RegisterTypes(const ::std::string&) {
   protobuf_AssignDescriptorsOnce();
   ::google::protobuf::internal::RegisterAllTypes(file_level_metadata, 2);
 }
 
-}  // namespace
-void TableStruct::InitDefaultsImpl() {
-  GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-  ::google::protobuf::internal::InitProtobufDefaults();
-  _locatefunc_default_instance_._instance.DefaultConstruct();
-  ::google::protobuf::internal::OnShutdownDestroyMessage(
-      &_locatefunc_default_instance_);_grapherCfg_default_instance_._instance.DefaultConstruct();
-  ::google::protobuf::internal::OnShutdownDestroyMessage(
-      &_grapherCfg_default_instance_);}
-
-void InitDefaults() {
-  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
-  ::google::protobuf::GoogleOnceInit(&once, &TableStruct::InitDefaultsImpl);
-}
-namespace {
 void AddDescriptorsImpl() {
   InitDefaults();
-  static const char descriptor[] GOOGLE_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  static const char descriptor[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
       "\n\021grapher_cfg.proto\022\007grapher\"<\n\nlocatefu"
       "nc\022\014\n\004type\030\001 \002(\t\022\022\n\ntfHeaderID\030\002 \002(\t\022\014\n\004"
       "tfID\030\003 \002(\t\"{\n\ngrapherCfg\022\016\n\006scanID\030\001 \002(\t"
@@ -139,7 +145,6 @@ void AddDescriptorsImpl() {
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "grapher_cfg.proto", &protobuf_RegisterTypes);
 }
-} // anonymous namespace
 
 void AddDescriptors() {
   static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
@@ -151,12 +156,13 @@ struct StaticDescriptorInitializer {
     AddDescriptors();
   }
 } static_descriptor_initializer;
-
 }  // namespace protobuf_grapher_5fcfg_2eproto
-
+namespace grapher {
 
 // ===================================================================
 
+void locatefunc::InitAsDefaultInstance() {
+}
 #if !defined(_MSC_VER) || _MSC_VER >= 1900
 const int locatefunc::kTypeFieldNumber;
 const int locatefunc::kTfHeaderIDFieldNumber;
@@ -166,7 +172,7 @@ const int locatefunc::kTfIDFieldNumber;
 locatefunc::locatefunc()
   : ::google::protobuf::Message(), _internal_metadata_(NULL) {
   if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
-    protobuf_grapher_5fcfg_2eproto::InitDefaults();
+    ::protobuf_grapher_5fcfg_2eproto::InitDefaultslocatefunc();
   }
   SharedCtor();
   // @@protoc_insertion_point(constructor:grapher.locatefunc)
@@ -216,12 +222,12 @@ void locatefunc::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
 const ::google::protobuf::Descriptor* locatefunc::descriptor() {
-  protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
-  return protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+  ::protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
 }
 
 const locatefunc& locatefunc::default_instance() {
-  protobuf_grapher_5fcfg_2eproto::InitDefaults();
+  ::protobuf_grapher_5fcfg_2eproto::InitDefaultslocatefunc();
   return *internal_default_instance();
 }
 
@@ -565,205 +571,14 @@ void locatefunc::InternalSwap(locatefunc* other) {
 
 ::google::protobuf::Metadata locatefunc::GetMetadata() const {
   protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
-  return protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages];
-}
-
-#if PROTOBUF_INLINE_NOT_IN_HEADERS
-// locatefunc
-
-// required string type = 1;
-bool locatefunc::has_type() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-void locatefunc::set_has_type() {
-  _has_bits_[0] |= 0x00000001u;
-}
-void locatefunc::clear_has_type() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-void locatefunc::clear_type() {
-  type_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-  clear_has_type();
-}
-const ::std::string& locatefunc::type() const {
-  // @@protoc_insertion_point(field_get:grapher.locatefunc.type)
-  return type_.GetNoArena();
-}
-void locatefunc::set_type(const ::std::string& value) {
-  set_has_type();
-  type_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
-  // @@protoc_insertion_point(field_set:grapher.locatefunc.type)
-}
-#if LANG_CXX11
-void locatefunc::set_type(::std::string&& value) {
-  set_has_type();
-  type_.SetNoArena(
-    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
-  // @@protoc_insertion_point(field_set_rvalue:grapher.locatefunc.type)
-}
-#endif
-void locatefunc::set_type(const char* value) {
-  GOOGLE_DCHECK(value != NULL);
-  set_has_type();
-  type_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
-  // @@protoc_insertion_point(field_set_char:grapher.locatefunc.type)
-}
-void locatefunc::set_type(const char* value, size_t size) {
-  set_has_type();
-  type_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
-      ::std::string(reinterpret_cast<const char*>(value), size));
-  // @@protoc_insertion_point(field_set_pointer:grapher.locatefunc.type)
-}
-::std::string* locatefunc::mutable_type() {
-  set_has_type();
-  // @@protoc_insertion_point(field_mutable:grapher.locatefunc.type)
-  return type_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-::std::string* locatefunc::release_type() {
-  // @@protoc_insertion_point(field_release:grapher.locatefunc.type)
-  clear_has_type();
-  return type_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-void locatefunc::set_allocated_type(::std::string* type) {
-  if (type != NULL) {
-    set_has_type();
-  } else {
-    clear_has_type();
-  }
-  type_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), type);
-  // @@protoc_insertion_point(field_set_allocated:grapher.locatefunc.type)
-}
-
-// required string tfHeaderID = 2;
-bool locatefunc::has_tfheaderid() const {
-  return (_has_bits_[0] & 0x00000002u) != 0;
-}
-void locatefunc::set_has_tfheaderid() {
-  _has_bits_[0] |= 0x00000002u;
-}
-void locatefunc::clear_has_tfheaderid() {
-  _has_bits_[0] &= ~0x00000002u;
-}
-void locatefunc::clear_tfheaderid() {
-  tfheaderid_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-  clear_has_tfheaderid();
-}
-const ::std::string& locatefunc::tfheaderid() const {
-  // @@protoc_insertion_point(field_get:grapher.locatefunc.tfHeaderID)
-  return tfheaderid_.GetNoArena();
-}
-void locatefunc::set_tfheaderid(const ::std::string& value) {
-  set_has_tfheaderid();
-  tfheaderid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
-  // @@protoc_insertion_point(field_set:grapher.locatefunc.tfHeaderID)
-}
-#if LANG_CXX11
-void locatefunc::set_tfheaderid(::std::string&& value) {
-  set_has_tfheaderid();
-  tfheaderid_.SetNoArena(
-    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
-  // @@protoc_insertion_point(field_set_rvalue:grapher.locatefunc.tfHeaderID)
-}
-#endif
-void locatefunc::set_tfheaderid(const char* value) {
-  GOOGLE_DCHECK(value != NULL);
-  set_has_tfheaderid();
-  tfheaderid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
-  // @@protoc_insertion_point(field_set_char:grapher.locatefunc.tfHeaderID)
-}
-void locatefunc::set_tfheaderid(const char* value, size_t size) {
-  set_has_tfheaderid();
-  tfheaderid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
-      ::std::string(reinterpret_cast<const char*>(value), size));
-  // @@protoc_insertion_point(field_set_pointer:grapher.locatefunc.tfHeaderID)
-}
-::std::string* locatefunc::mutable_tfheaderid() {
-  set_has_tfheaderid();
-  // @@protoc_insertion_point(field_mutable:grapher.locatefunc.tfHeaderID)
-  return tfheaderid_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-::std::string* locatefunc::release_tfheaderid() {
-  // @@protoc_insertion_point(field_release:grapher.locatefunc.tfHeaderID)
-  clear_has_tfheaderid();
-  return tfheaderid_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-void locatefunc::set_allocated_tfheaderid(::std::string* tfheaderid) {
-  if (tfheaderid != NULL) {
-    set_has_tfheaderid();
-  } else {
-    clear_has_tfheaderid();
-  }
-  tfheaderid_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), tfheaderid);
-  // @@protoc_insertion_point(field_set_allocated:grapher.locatefunc.tfHeaderID)
-}
-
-// required string tfID = 3;
-bool locatefunc::has_tfid() const {
-  return (_has_bits_[0] & 0x00000004u) != 0;
-}
-void locatefunc::set_has_tfid() {
-  _has_bits_[0] |= 0x00000004u;
-}
-void locatefunc::clear_has_tfid() {
-  _has_bits_[0] &= ~0x00000004u;
-}
-void locatefunc::clear_tfid() {
-  tfid_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-  clear_has_tfid();
-}
-const ::std::string& locatefunc::tfid() const {
-  // @@protoc_insertion_point(field_get:grapher.locatefunc.tfID)
-  return tfid_.GetNoArena();
-}
-void locatefunc::set_tfid(const ::std::string& value) {
-  set_has_tfid();
-  tfid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
-  // @@protoc_insertion_point(field_set:grapher.locatefunc.tfID)
-}
-#if LANG_CXX11
-void locatefunc::set_tfid(::std::string&& value) {
-  set_has_tfid();
-  tfid_.SetNoArena(
-    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
-  // @@protoc_insertion_point(field_set_rvalue:grapher.locatefunc.tfID)
-}
-#endif
-void locatefunc::set_tfid(const char* value) {
-  GOOGLE_DCHECK(value != NULL);
-  set_has_tfid();
-  tfid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
-  // @@protoc_insertion_point(field_set_char:grapher.locatefunc.tfID)
-}
-void locatefunc::set_tfid(const char* value, size_t size) {
-  set_has_tfid();
-  tfid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
-      ::std::string(reinterpret_cast<const char*>(value), size));
-  // @@protoc_insertion_point(field_set_pointer:grapher.locatefunc.tfID)
-}
-::std::string* locatefunc::mutable_tfid() {
-  set_has_tfid();
-  // @@protoc_insertion_point(field_mutable:grapher.locatefunc.tfID)
-  return tfid_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-::std::string* locatefunc::release_tfid() {
-  // @@protoc_insertion_point(field_release:grapher.locatefunc.tfID)
-  clear_has_tfid();
-  return tfid_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-void locatefunc::set_allocated_tfid(::std::string* tfid) {
-  if (tfid != NULL) {
-    set_has_tfid();
-  } else {
-    clear_has_tfid();
-  }
-  tfid_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), tfid);
-  // @@protoc_insertion_point(field_set_allocated:grapher.locatefunc.tfID)
+  return ::protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages];
 }
 
-#endif  // PROTOBUF_INLINE_NOT_IN_HEADERS
 
 // ===================================================================
 
+void grapherCfg::InitAsDefaultInstance() {
+}
 #if !defined(_MSC_VER) || _MSC_VER >= 1900
 const int grapherCfg::kScanIDFieldNumber;
 const int grapherCfg::kGrapherFunctionsFieldNumber;
@@ -773,7 +588,7 @@ const int grapherCfg::kLocateFunctionsFieldNumber;
 grapherCfg::grapherCfg()
   : ::google::protobuf::Message(), _internal_metadata_(NULL) {
   if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
-    protobuf_grapher_5fcfg_2eproto::InitDefaults();
+    ::protobuf_grapher_5fcfg_2eproto::InitDefaultsgrapherCfg();
   }
   SharedCtor();
   // @@protoc_insertion_point(constructor:grapher.grapherCfg)
@@ -813,12 +628,12 @@ void grapherCfg::SetCachedSize(int size) const {
   GOOGLE_SAFE_CONCURRENT_WRITES_END();
 }
 const ::google::protobuf::Descriptor* grapherCfg::descriptor() {
-  protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
-  return protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+  ::protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
 }
 
 const grapherCfg& grapherCfg::default_instance() {
-  protobuf_grapher_5fcfg_2eproto::InitDefaults();
+  ::protobuf_grapher_5fcfg_2eproto::InitDefaultsgrapherCfg();
   return *internal_default_instance();
 }
 
@@ -838,7 +653,8 @@ void grapherCfg::Clear() {
 
   grapher_functions_.Clear();
   locate_functions_.Clear();
-  if (has_scanid()) {
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
     GOOGLE_DCHECK(!scanid_.IsDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited()));
     (*scanid_.UnsafeRawStringPointer())->clear();
   }
@@ -876,8 +692,7 @@ bool grapherCfg::MergePartialFromCodedStream(
       case 2: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(18u /* 18 & 0xFF */)) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_grapher_functions()));
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessage(input, add_grapher_functions()));
         } else {
           goto handle_unusual;
         }
@@ -888,8 +703,7 @@ bool grapherCfg::MergePartialFromCodedStream(
       case 3: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(26u /* 26 & 0xFF */)) {
-          DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
-                input, add_locate_functions()));
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessage(input, add_locate_functions()));
         } else {
           goto handle_unusual;
         }
@@ -977,7 +791,7 @@ void grapherCfg::SerializeWithCachedSizes(
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->grapher_functions_size()); i < n; i++) {
     target = ::google::protobuf::internal::WireFormatLite::
-      InternalWriteMessageNoVirtualToArray(
+      InternalWriteMessageToArray(
         2, this->grapher_functions(static_cast<int>(i)), deterministic, target);
   }
 
@@ -985,7 +799,7 @@ void grapherCfg::SerializeWithCachedSizes(
   for (unsigned int i = 0,
       n = static_cast<unsigned int>(this->locate_functions_size()); i < n; i++) {
     target = ::google::protobuf::internal::WireFormatLite::
-      InternalWriteMessageNoVirtualToArray(
+      InternalWriteMessageToArray(
         3, this->locate_functions(static_cast<int>(i)), deterministic, target);
   }
 
@@ -1018,7 +832,7 @@ size_t grapherCfg::ByteSizeLong() const {
     total_size += 1UL * count;
     for (unsigned int i = 0; i < count; i++) {
       total_size +=
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        ::google::protobuf::internal::WireFormatLite::MessageSize(
           this->grapher_functions(static_cast<int>(i)));
     }
   }
@@ -1029,7 +843,7 @@ size_t grapherCfg::ByteSizeLong() const {
     total_size += 1UL * count;
     for (unsigned int i = 0; i < count; i++) {
       total_size +=
-        ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
+        ::google::protobuf::internal::WireFormatLite::MessageSize(
           this->locate_functions(static_cast<int>(i)));
     }
   }
@@ -1108,139 +922,11 @@ void grapherCfg::InternalSwap(grapherCfg* other) {
 
 ::google::protobuf::Metadata grapherCfg::GetMetadata() const {
   protobuf_grapher_5fcfg_2eproto::protobuf_AssignDescriptorsOnce();
-  return protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages];
+  return ::protobuf_grapher_5fcfg_2eproto::file_level_metadata[kIndexInFileMessages];
 }
 
-#if PROTOBUF_INLINE_NOT_IN_HEADERS
-// grapherCfg
-
-// required string scanID = 1;
-bool grapherCfg::has_scanid() const {
-  return (_has_bits_[0] & 0x00000001u) != 0;
-}
-void grapherCfg::set_has_scanid() {
-  _has_bits_[0] |= 0x00000001u;
-}
-void grapherCfg::clear_has_scanid() {
-  _has_bits_[0] &= ~0x00000001u;
-}
-void grapherCfg::clear_scanid() {
-  scanid_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-  clear_has_scanid();
-}
-const ::std::string& grapherCfg::scanid() const {
-  // @@protoc_insertion_point(field_get:grapher.grapherCfg.scanID)
-  return scanid_.GetNoArena();
-}
-void grapherCfg::set_scanid(const ::std::string& value) {
-  set_has_scanid();
-  scanid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
-  // @@protoc_insertion_point(field_set:grapher.grapherCfg.scanID)
-}
-#if LANG_CXX11
-void grapherCfg::set_scanid(::std::string&& value) {
-  set_has_scanid();
-  scanid_.SetNoArena(
-    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
-  // @@protoc_insertion_point(field_set_rvalue:grapher.grapherCfg.scanID)
-}
-#endif
-void grapherCfg::set_scanid(const char* value) {
-  GOOGLE_DCHECK(value != NULL);
-  set_has_scanid();
-  scanid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
-  // @@protoc_insertion_point(field_set_char:grapher.grapherCfg.scanID)
-}
-void grapherCfg::set_scanid(const char* value, size_t size) {
-  set_has_scanid();
-  scanid_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
-      ::std::string(reinterpret_cast<const char*>(value), size));
-  // @@protoc_insertion_point(field_set_pointer:grapher.grapherCfg.scanID)
-}
-::std::string* grapherCfg::mutable_scanid() {
-  set_has_scanid();
-  // @@protoc_insertion_point(field_mutable:grapher.grapherCfg.scanID)
-  return scanid_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-::std::string* grapherCfg::release_scanid() {
-  // @@protoc_insertion_point(field_release:grapher.grapherCfg.scanID)
-  clear_has_scanid();
-  return scanid_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
-}
-void grapherCfg::set_allocated_scanid(::std::string* scanid) {
-  if (scanid != NULL) {
-    set_has_scanid();
-  } else {
-    clear_has_scanid();
-  }
-  scanid_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), scanid);
-  // @@protoc_insertion_point(field_set_allocated:grapher.grapherCfg.scanID)
-}
-
-// repeated .grapher.locatefunc grapher_functions = 2;
-int grapherCfg::grapher_functions_size() const {
-  return grapher_functions_.size();
-}
-void grapherCfg::clear_grapher_functions() {
-  grapher_functions_.Clear();
-}
-const ::grapher::locatefunc& grapherCfg::grapher_functions(int index) const {
-  // @@protoc_insertion_point(field_get:grapher.grapherCfg.grapher_functions)
-  return grapher_functions_.Get(index);
-}
-::grapher::locatefunc* grapherCfg::mutable_grapher_functions(int index) {
-  // @@protoc_insertion_point(field_mutable:grapher.grapherCfg.grapher_functions)
-  return grapher_functions_.Mutable(index);
-}
-::grapher::locatefunc* grapherCfg::add_grapher_functions() {
-  // @@protoc_insertion_point(field_add:grapher.grapherCfg.grapher_functions)
-  return grapher_functions_.Add();
-}
-::google::protobuf::RepeatedPtrField< ::grapher::locatefunc >*
-grapherCfg::mutable_grapher_functions() {
-  // @@protoc_insertion_point(field_mutable_list:grapher.grapherCfg.grapher_functions)
-  return &grapher_functions_;
-}
-const ::google::protobuf::RepeatedPtrField< ::grapher::locatefunc >&
-grapherCfg::grapher_functions() const {
-  // @@protoc_insertion_point(field_list:grapher.grapherCfg.grapher_functions)
-  return grapher_functions_;
-}
-
-// repeated .grapher.locatefunc locate_functions = 3;
-int grapherCfg::locate_functions_size() const {
-  return locate_functions_.size();
-}
-void grapherCfg::clear_locate_functions() {
-  locate_functions_.Clear();
-}
-const ::grapher::locatefunc& grapherCfg::locate_functions(int index) const {
-  // @@protoc_insertion_point(field_get:grapher.grapherCfg.locate_functions)
-  return locate_functions_.Get(index);
-}
-::grapher::locatefunc* grapherCfg::mutable_locate_functions(int index) {
-  // @@protoc_insertion_point(field_mutable:grapher.grapherCfg.locate_functions)
-  return locate_functions_.Mutable(index);
-}
-::grapher::locatefunc* grapherCfg::add_locate_functions() {
-  // @@protoc_insertion_point(field_add:grapher.grapherCfg.locate_functions)
-  return locate_functions_.Add();
-}
-::google::protobuf::RepeatedPtrField< ::grapher::locatefunc >*
-grapherCfg::mutable_locate_functions() {
-  // @@protoc_insertion_point(field_mutable_list:grapher.grapherCfg.locate_functions)
-  return &locate_functions_;
-}
-const ::google::protobuf::RepeatedPtrField< ::grapher::locatefunc >&
-grapherCfg::locate_functions() const {
-  // @@protoc_insertion_point(field_list:grapher.grapherCfg.locate_functions)
-  return locate_functions_;
-}
-
-#endif  // PROTOBUF_INLINE_NOT_IN_HEADERS
 
 // @@protoc_insertion_point(namespace_scope)
-
 }  // namespace grapher
 
 // @@protoc_insertion_point(global_scope)

grapher/src/cfg/grapher_cfg.pb.h

@@ -8,12 +8,12 @@
 
 #include <google/protobuf/stubs/common.h>
 
-#if GOOGLE_PROTOBUF_VERSION < 3004000
+#if GOOGLE_PROTOBUF_VERSION < 3005000
 #error This file was generated by a newer version of protoc which is
 #error incompatible with your Protocol Buffer headers.  Please update
 #error your headers.
 #endif
-#if 3004000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#if 3005000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
 #error This file was generated by an older version of protoc which is
 #error incompatible with your Protocol Buffer headers.  Please
 #error regenerate this file with a newer version of protoc.
@@ -30,31 +30,36 @@
 #include <google/protobuf/extension_set.h>  // IWYU pragma: export
 #include <google/protobuf/unknown_field_set.h>
 // @@protoc_insertion_point(includes)
-namespace grapher {
-class grapherCfg;
-class grapherCfgDefaultTypeInternal;
-extern grapherCfgDefaultTypeInternal _grapherCfg_default_instance_;
-class locatefunc;
-class locatefuncDefaultTypeInternal;
-extern locatefuncDefaultTypeInternal _locatefunc_default_instance_;
-}  // namespace grapher
-
-namespace grapher {
 
 namespace protobuf_grapher_5fcfg_2eproto {
-// Internal implementation detail -- do not call these.
+// Internal implementation detail -- do not use these members.
 struct TableStruct {
   static const ::google::protobuf::internal::ParseTableField entries[];
   static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
-  static const ::google::protobuf::internal::ParseTable schema[];
-  static const ::google::protobuf::uint32 offsets[];
+  static const ::google::protobuf::internal::ParseTable schema[2];
   static const ::google::protobuf::internal::FieldMetadata field_metadata[];
   static const ::google::protobuf::internal::SerializationTable serialization_table[];
-  static void InitDefaultsImpl();
+  static const ::google::protobuf::uint32 offsets[];
 };
 void AddDescriptors();
-void InitDefaults();
+void InitDefaultslocatefuncImpl();
+void InitDefaultslocatefunc();
+void InitDefaultsgrapherCfgImpl();
+void InitDefaultsgrapherCfg();
+inline void InitDefaults() {
+  InitDefaultslocatefunc();
+  InitDefaultsgrapherCfg();
+}
 }  // namespace protobuf_grapher_5fcfg_2eproto
+namespace grapher {
+class grapherCfg;
+class grapherCfgDefaultTypeInternal;
+extern grapherCfgDefaultTypeInternal _grapherCfg_default_instance_;
+class locatefunc;
+class locatefuncDefaultTypeInternal;
+extern locatefuncDefaultTypeInternal _locatefunc_default_instance_;
+}  // namespace grapher
+namespace grapher {
 
 // ===================================================================
 
@@ -94,6 +99,7 @@ class locatefunc : public ::google::protobuf::Message /* @@protoc_insertion_poin
   static const ::google::protobuf::Descriptor* descriptor();
   static const locatefunc& default_instance();
 
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
   static inline const locatefunc* internal_default_instance() {
     return reinterpret_cast<const locatefunc*>(
                &_locatefunc_default_instance_);
@@ -209,7 +215,8 @@ class locatefunc : public ::google::protobuf::Message /* @@protoc_insertion_poin
   ::google::protobuf::internal::ArenaStringPtr type_;
   ::google::protobuf::internal::ArenaStringPtr tfheaderid_;
   ::google::protobuf::internal::ArenaStringPtr tfid_;
-  friend struct protobuf_grapher_5fcfg_2eproto::TableStruct;
+  friend struct ::protobuf_grapher_5fcfg_2eproto::TableStruct;
+  friend void ::protobuf_grapher_5fcfg_2eproto::InitDefaultslocatefuncImpl();
 };
 // -------------------------------------------------------------------
 
@@ -249,6 +256,7 @@ class grapherCfg : public ::google::protobuf::Message /* @@protoc_insertion_poin
   static const ::google::protobuf::Descriptor* descriptor();
   static const grapherCfg& default_instance();
 
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
   static inline const grapherCfg* internal_default_instance() {
     return reinterpret_cast<const grapherCfg*>(
                &_grapherCfg_default_instance_);
@@ -351,14 +359,14 @@ class grapherCfg : public ::google::protobuf::Message /* @@protoc_insertion_poin
   ::google::protobuf::RepeatedPtrField< ::grapher::locatefunc > grapher_functions_;
   ::google::protobuf::RepeatedPtrField< ::grapher::locatefunc > locate_functions_;
   ::google::protobuf::internal::ArenaStringPtr scanid_;
-  friend struct protobuf_grapher_5fcfg_2eproto::TableStruct;
+  friend struct ::protobuf_grapher_5fcfg_2eproto::TableStruct;
+  friend void ::protobuf_grapher_5fcfg_2eproto::InitDefaultsgrapherCfgImpl();
 };
 // ===================================================================
 
 
 // ===================================================================
 
-#if !PROTOBUF_INLINE_NOT_IN_HEADERS
 #ifdef __GNUC__
   #pragma GCC diagnostic push
   #pragma GCC diagnostic ignored "-Wstrict-aliasing"
@@ -684,13 +692,11 @@ grapherCfg::locate_functions() const {
 #ifdef __GNUC__
   #pragma GCC diagnostic pop
 #endif  // __GNUC__
-#endif  // !PROTOBUF_INLINE_NOT_IN_HEADERS
 // -------------------------------------------------------------------
 
 
 // @@protoc_insertion_point(namespace_scope)
 
-
 }  // namespace grapher
 
 // @@protoc_insertion_point(global_scope)

grapher/src/grapher_node.cpp

@@ -199,6 +199,7 @@ void scanHandler(const sensor_msgs::LaserScan::ConstPtr& msg)
     //滤波
     if(poses.size()==2)
       lastPose=klmFilter.filter(poses[0],poses[1]);
+//    lastPose = poses[0];
       
     tf::StampedTransform laserOdometryTrans;
     laserOdometryTrans.frame_id_ = "/map";
@@ -280,10 +281,11 @@ void publish()
     }
 }
 
-
+//#include <stdio.h>
 int main(int argc, char** argv)
 {
-    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
+//    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
+    if(false==ReadProtoConfig("/home/youchen/Documents/git_ws/src/grapher/grapher.cfg",grapher_config))
     {
       printf(" config file load failed ... \n");
       return -1;
@@ -308,9 +310,12 @@ int main(int argc, char** argv)
 
     ros::spin();
 
-    pGrapher->saveImage("/home/zx/zzw/map.bmp");
-    pGrapher->saveCloudTxt("/home/zx/zzw/map.txt");
-    pGrapher->saveCloudPcd("/home/zx/zzw/map.pcd");
+//    pGrapher->saveImage("/home/zx/zzw/map.bmp");
+//    pGrapher->saveCloudTxt("/home/zx/zzw/map.txt");
+//    pGrapher->saveCloudPcd("/home/zx/zzw/map.pcd");
+    pGrapher->saveImage("/home/youchen/Documents/git_ws/src/grapher/map.bmp");
+    pGrapher->saveCloudTxt("/home/youchen/Documents/git_ws/src/grapher/map.txt");
+    pGrapher->saveCloudPcd("/home/youchen/Documents/git_ws/src/grapher/map.pcd");
     delete pGrapher;
     
 

grapher/src/location_node.cpp

@@ -180,7 +180,8 @@ void publish()
 
 int main(int argc, char** argv)
 {
-    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
+//    if(false==ReadProtoConfig("/home/zx/zzw/catkin_ws/src/grapher/grapher.cfg",grapher_config))
+    if(false==ReadProtoConfig("/home/youchen/Documents/git_ws/src/grapher/grapher.cfg",grapher_config))
     {
       printf(" config file load failed ... \n");
       return -1;
@@ -194,7 +195,8 @@ int main(int argc, char** argv)
     pubCloudG = nh.advertise<sensor_msgs::PointCloud2>
                                  ("/cloud_global", 2);
 
-    pcl::io::loadPCDFile("/home/zx/zzw/map.pcd", *target_cloud);
+//    pcl::io::loadPCDFile("/home/zx/zzw/map.pcd", *target_cloud);
+    pcl::io::loadPCDFile("/home/youchen/Documents/git_ws/src/grapher/map.pcd", *target_cloud);
     boost::thread* thread_=new boost::thread(publish);
     std::string scanID=grapher_config.scanid();
     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::LaserScan>(scanID, 2,scanHandler);

map_scan_match/200511map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/Documents/git_ws/src/map_scan_match/200511map_origin.pgm
+resolution: 0.050000
+origin: [-11.395653, -10.088187, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

map_scan_match/CMakeLists.txt

@@ -0,0 +1,213 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(map_scan_match)
+
+set(OpenCV_DIR "/opt/ros/kinetic/share/OpenCV-3.3.1-dev/")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+set(CMAKE_BUILD_TYPE "RELEASE")
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  sensor_msgs
+  tf
+  OpenCV REQUIRED
+  Ceres REQUIRED
+  PCL REQUIRED
+  Eigen3 REQUIRED
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ceres_test
+#  CATKIN_DEPENDS roscpp rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ceres_test.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/map_scan_match_node.cpp src/scan_match.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${CERES_LIBRARIES}
+  ${OpenCV_LIBS}
+  ${PCL_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ceres_test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

map_scan_match/map_0.05.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/Documents/git_ws/src/map_scan_match/map_0.05.pgm
+resolution: 0.050000
+origin: [-8.141946, -14.800098, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

map_scan_match/map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/Documents/catkin_ws/map_origin.pgm
+resolution: 0.050000
+origin: [-6.119765, -14.900000, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

map_scan_match/map_scan.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find map_scan_match)/200511map_origin.yaml"/>
+  <node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find map_scan_match)/configuration_files/demo.rviz" />
+  <!--node name="map_scan_matcher" pkg="map_scan_match" type="map_scan_match_node"/-->
+</launch>

map_scan_match/my.yaml

@@ -0,0 +1,24 @@
+%YAML:1.0
+---
+iterationNr: 100
+strings:
+   - "image1.jpg"
+   - Awesomeness
+   - "./src/cv_test/baboon.jpg"
+Mapping:
+   One: 1
+   Two: 2
+R: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: u
+   data: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ]
+T: !!opencv-matrix
+   rows: 3
+   cols: 1
+   dt: d
+   data: [ 0., 0., 0. ]
+MyData:
+   A: 97
+   X: 3.1415926535897931e+00
+   id: mydata1234

map_scan_match/my1.txt

@@ -0,0 +1,201 @@
+-0.000000 -3.655000 0.000000
+0.048339 -3.692684 0.000000
+0.114427 -3.745252 0.000000
+0.147577 -3.756102 0.000000
+0.198614 -3.789799 0.000000
+0.234181 -3.828845 0.000000
+0.269956 -3.860573 0.000000
+0.340254 -3.889144 0.000000
+0.360606 -3.924467 0.000000
+0.416648 -3.964165 0.000000
+0.453942 -3.984224 0.000000
+0.492715 -4.012865 0.000000
+0.553560 -4.067505 0.000000
+0.571302 -4.065051 0.000000
+0.927818 -6.399086 0.000000
+1.015254 -6.410098 0.000000
+1.008332 -6.191429 0.000000
+0.990775 -5.920673 0.000000
+1.048098 -6.099608 0.000000
+1.126970 -6.391403 0.000000
+1.154847 -6.386425 0.000000
+0.432260 -2.332281 0.000000
+0.447730 -2.250903 0.000000
+0.503436 -2.270865 0.000000
+0.566802 -2.360915 0.000000
+0.651444 -2.431236 0.000000
+0.707281 -2.466599 0.000000
+0.756511 -2.513626 0.000000
+0.779603 -2.510749 0.000000
+0.859891 -2.569959 0.000000
+0.932317 -2.632800 0.000000
+0.995984 -2.663898 0.000000
+1.007598 -2.659527 0.000000
+1.055776 -2.714941 0.000000
+1.117812 -2.698655 0.000000
+1.187035 -2.796497 0.000000
+1.240900 -2.853894 0.000000
+1.324561 -2.940436 0.000000
+1.348077 -2.924224 0.000000
+1.403685 -2.910160 0.000000
+1.499351 -3.040403 0.000000
+1.812287 -3.236093 0.000000
+1.826389 -3.228155 0.000000
+2.153728 -3.514590 0.000000
+2.193325 -3.510078 0.000000
+2.264115 -3.588430 0.000000
+2.398771 -3.658760 0.000000
+2.414712 -3.648259 0.000000
+5.949533 -8.188891 0.000000
+5.985207 -8.162853 0.000000
+6.049032 -8.100698 0.000000
+3.172151 -4.209620 0.000000
+3.159619 -4.155142 0.000000
+4.940569 -6.380884 0.000000
+4.988635 -6.328103 0.000000
+4.184523 -5.260708 0.000000
+4.171760 -5.197946 0.000000
+4.149718 -5.124516 0.000000
+4.024619 -4.925927 0.000000
+3.929672 -4.767112 0.000000
+3.974980 -4.737235 0.000000
+3.987212 -4.709924 0.000000
+6.231195 -6.509249 0.000000
+6.259538 -6.481998 0.000000
+6.273806 -6.440299 0.000000
+6.263998 -6.374347 0.000000
+6.928537 -6.868399 0.000000
+6.976984 -6.856326 0.000000
+10.344278 -9.731017 0.000000
+10.386640 -9.685789 0.000000
+10.422929 -9.634947 0.000000
+10.441276 -9.567758 0.000000
+10.466640 -9.507317 0.000000
+10.508022 -9.461557 0.000000
+10.371586 -9.095726 0.000000
+10.442118 -9.077284 0.000000
+10.429354 -8.986595 0.000000
+10.549067 -9.009845 0.000000
+10.630258 -8.999268 0.000000
+8.803346 -7.386959 0.000000
+8.826267 -7.340804 0.000000
+7.436887 -6.130559 0.000000
+7.461243 -6.096154 0.000000
+7.539113 -6.050840 0.000000
+7.553703 -6.008550 0.000000
+7.578277 -5.974296 0.000000
+1.880186 -1.468978 0.000000
+1.812253 -1.403211 0.000000
+1.819152 -1.395900 0.000000
+1.915307 -1.378839 0.000000
+1.969338 -1.404731 0.000000
+2.048129 -1.447491 0.000000
+1.946298 -1.362828 0.000000
+2.066077 -1.419992 0.000000
+2.168929 -1.435600 0.000000
+2.203606 -1.444765 0.000000
+2.315561 -1.503762 0.000000
+2.178628 -1.361375 0.000000
+2.191277 -1.342834 0.000000
+1.458408 -0.867673 0.000000
+1.339829 -0.789230 0.000000
+1.240463 -0.723427 0.000000
+1.187317 -0.685507 0.000000
+1.236489 -0.643685 0.000000
+1.200182 -0.611533 0.000000
+1.239433 -0.604521 0.000000
+1.194001 -0.531612 0.000000
+1.220705 -0.480857 0.000000
+1.233595 -0.461230 0.000000
+1.291965 -0.395002 0.000000
+1.207343 -0.346208 0.000000
+1.181409 -0.327641 0.000000
+1.126245 -0.286037 0.000000
+1.077885 -0.234038 0.000000
+1.144749 -0.160891 0.000000
+1.152952 -0.136467 0.000000
+1.175049 -0.107979 0.000000
+1.238811 -0.021631 0.000000
+1.280403 0.039113 0.000000
+1.302499 0.062556 0.000000
+1.338886 0.117130 0.000000
+1.383250 0.188244 0.000000
+1.434570 0.265874 0.000000
+1.452250 0.295455 0.000000
+1.481044 0.341917 0.000000
+1.522898 0.393839 0.000000
+1.536909 0.419000 0.000000
+1.604436 0.498181 0.000000
+1.630445 0.569367 0.000000
+1.677489 0.618848 0.000000
+1.693533 0.641612 0.000000
+1.749600 0.706873 0.000000
+1.820763 0.772856 0.000000
+1.851369 0.795399 0.000000
+1.952425 0.848926 0.000000
+2.141834 0.942416 0.000000
+2.297572 1.022930 0.000000
+2.339383 1.078455 0.000000
+2.401268 1.158218 0.000000
+2.415066 1.177890 0.000000
+2.458378 1.239108 0.000000
+2.493700 1.284323 0.000000
+2.500490 1.301656 0.000000
+2.575820 1.398539 0.000000
+2.616870 1.450537 0.000000
+2.562082 1.539435 0.000000
+2.535760 1.553897 0.000000
+2.475187 1.592074 0.000000
+2.456182 1.610323 0.000000
+2.406458 1.653892 0.000000
+2.351140 1.692557 0.000000
+2.324183 1.704140 0.000000
+2.276935 1.747131 0.000000
+2.248206 1.756468 0.000000
+2.183522 1.815993 0.000000
+2.157670 1.826578 0.000000
+2.103080 1.860623 0.000000
+2.054647 1.899271 0.000000
+2.034353 1.913702 0.000000
+1.966477 1.966450 0.000000
+1.932893 1.984143 0.000000
+1.878920 2.014866 0.000000
+1.865160 2.053352 0.000000
+1.807282 2.060780 0.000000
+1.727934 2.114853 0.000000
+1.706909 2.126732 0.000000
+1.658283 2.161082 0.000000
+1.608971 2.194323 0.000000
+1.593850 2.213961 0.000000
+1.518392 2.272394 0.000000
+1.432188 2.337075 0.000000
+1.423533 2.345866 0.000000
+1.364615 2.387535 0.000000
+1.309821 2.412342 0.000000
+1.298810 2.417153 0.000000
+1.213433 2.487851 0.000000
+1.148803 2.550188 0.000000
+1.100964 2.562499 0.000000
+1.059295 2.495479 0.000000
+1.017458 2.426301 0.000000
+0.976629 2.357732 0.000000
+0.931875 2.277747 0.000000
+0.850751 2.105627 0.000000
+0.778437 2.109985 0.000000
+0.759656 2.115757 0.000000
+0.682322 2.163977 0.000000
+0.638311 2.189867 0.000000
+0.628199 2.190709 0.000000
+0.552088 2.214210 0.000000
+0.501946 2.264025 0.000000
+0.465946 2.290079 0.000000
+0.403245 2.346605 0.000000
+0.339851 2.496979 0.000000
+0.326864 2.482574 0.000000
+0.254971 2.425636 0.000000
+0.240176 2.384937 0.000000
+0.209350 2.173943 0.000000
+0.181456 2.183473 0.000000
+0.173105 2.199198 0.000000
+0.108791 2.264388 0.000000
+0.030132 2.299803 0.000000

map_scan_match/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>map_scan_match</name>
+  <version>0.0.0</version>
+  <description>The map_scan_match package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/map_scan_match</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

map_scan_match/scan.txt

@@ -0,0 +1,846 @@
+-0.0722 -0.0735 0.0026
+-0.0695 -0.0719 0.0000
+-0.0705 -0.0737 0.0000
+-0.0764 -0.0805 0.0000
+-0.0713 -0.0758 0.0000
+-0.0702 -0.0753 0.0000
+-0.0706 -0.0764 0.0000
+-0.0703 -0.0767 0.0000
+-0.0686 -0.0755 0.0000
+-0.0679 -0.0761 0.0000
+-0.0682 -0.0771 0.0000
+-0.0686 -0.0782 0.0000
+-0.0669 -0.0770 0.0000
+-0.0666 -0.0773 0.0000
+-0.0455 -0.0902 0.0000
+-0.0464 -0.0931 0.0000
+-0.0509 -0.1031 0.0000
+-0.0530 -0.1088 0.0000
+-0.0517 -0.1072 0.0000
+-0.0495 -0.1038 0.0000
+-0.0452 -0.0959 0.0000
+-0.0486 -0.1042 0.0000
+-0.0440 -0.0954 0.0000
+-0.0456 -0.1001 0.0000
+-0.0452 -0.1003 0.0000
+-0.0456 -0.1023 0.0000
+-0.0403 -0.0915 0.0000
+-0.0415 -0.0954 0.0000
+-0.0482 -0.1121 0.0000
+-0.0481 -0.1132 0.0000
+-0.0476 -0.1134 0.0000
+-0.0467 -0.1127 0.0000
+-0.0462 -0.1129 0.0000
+-0.0450 -0.1113 0.0000
+-0.0489 -0.1226 0.0000
+-0.0480 -0.1219 0.0000
+-0.0431 -0.1109 0.0000
+-0.0498 -0.1298 0.0000
+-0.0475 -0.1253 0.0000
+-0.0448 -0.1199 0.0000
+-0.0478 -0.1295 0.0000
+-1.0496 -3.0484 0.0000
+-1.0363 -3.0529 0.0000
+-1.1402 -5.2516 0.0000
+-1.3730 -6.4597 0.0000
+-1.3448 -6.4656 0.0000
+-1.3138 -6.4577 0.0000
+-1.2488 -6.2780 0.0000
+-1.2061 -6.2049 0.0000
+-1.1790 -6.2101 0.0000
+-1.3492 -9.0277 0.0000
+-1.3167 -9.0810 0.0000
+-1.2704 -9.0392 0.0000
+-1.3449 -9.8819 0.0000
+-1.2993 -9.8688 0.0000
+-0.3897 -9.9183 0.0000
+-0.3466 -9.9260 0.0000
+-0.3033 -9.9274 0.0000
+-0.2599 -9.9246 0.0000
+-0.2166 -9.9286 0.0000
+-0.1733 -9.9305 0.0000
+-0.1302 -9.9471 0.0000
+-0.0868 -9.9506 0.0000
+-0.0434 -9.9409 0.0000
+0.0000 -9.9390 0.0000
+0.0433 -9.9319 0.0000
+0.0868 -9.9436 0.0000
+0.1300 -9.9341 0.0000
+0.1732 -9.9215 0.0000
+0.2165 -9.9206 0.0000
+0.2598 -9.9226 0.0000
+0.3022 -9.8924 0.0000
+0.3460 -9.9070 0.0000
+0.3885 -9.8874 0.0000
+0.4316 -9.8856 0.0000
+0.3762 -7.8330 0.0000
+0.4077 -7.7793 0.0000
+0.4418 -7.7805 0.0000
+0.4754 -7.7725 0.0000
+0.6504 -5.9385 0.0000
+0.6661 -5.8462 0.0000
+0.6771 -5.7211 0.0000
+0.6802 -5.5394 0.0000
+0.6912 -5.4332 0.0000
+0.5262 -3.9965 0.0000
+0.5410 -3.9754 0.0000
+0.5584 -3.9730 0.0000
+0.5731 -3.9527 0.0000
+0.5918 -3.9600 0.0000
+0.6103 -3.9653 0.0000
+0.6351 -4.0100 0.0000
+0.7166 -4.4000 0.0000
+0.7315 -4.3712 0.0000
+0.7355 -4.2803 0.0000
+0.7392 -4.1923 0.0000
+0.7401 -4.0926 0.0000
+0.7442 -4.0156 0.0000
+0.7472 -3.9357 0.0000
+0.7539 -3.8784 0.0000
+0.7540 -3.7907 0.0000
+0.7532 -3.7022 0.0000
+0.7573 -3.6411 0.0000
+0.7622 -3.5859 0.0000
+0.7660 -3.5278 0.0000
+0.7662 -3.4561 0.0000
+0.7694 -3.4000 0.0000
+0.7723 -3.3450 0.0000
+0.7745 -3.2890 0.0000
+0.7732 -3.2205 0.0000
+0.7760 -3.1714 0.0000
+0.7804 -3.1302 0.0000
+0.7798 -3.0705 0.0000
+0.7822 -3.0245 0.0000
+0.7824 -2.9717 0.0000
+0.7832 -2.9229 0.0000
+0.7830 -2.8722 0.0000
+0.7897 -2.8475 0.0000
+0.7888 -2.7969 0.0000
+0.7905 -2.7569 0.0000
+0.7936 -2.7227 0.0000
+0.7944 -2.6818 0.0000
+0.7948 -2.6410 0.0000
+0.7967 -2.6059 0.0000
+0.7950 -2.5604 0.0000
+0.7969 -2.5273 0.0000
+0.7981 -2.4934 0.0000
+0.7997 -2.4613 0.0000
+0.8023 -2.4331 0.0000
+0.7980 -2.3850 0.0000
+0.8039 -2.3683 0.0000
+0.8081 -2.3468 0.0000
+0.8058 -2.3074 0.0000
+0.8078 -2.2812 0.0000
+0.8124 -2.2626 0.0000
+0.8085 -2.2214 0.0000
+0.8106 -2.1972 0.0000
+0.8065 -2.1572 0.0000
+0.7429 -1.9610 0.0000
+0.6691 -1.7430 0.0000
+0.6701 -1.7233 0.0000
+0.6711 -1.7036 0.0000
+0.6755 -1.6932 0.0000
+0.6844 -1.6940 0.0000
+0.6884 -1.6826 0.0000
+0.6980 -1.6852 0.0000
+0.6957 -1.6590 0.0000
+0.7029 -1.6560 0.0000
+0.7228 -1.6823 0.0000
+0.7309 -1.6810 0.0000
+0.7398 -1.6814 0.0000
+0.7464 -1.6764 0.0000
+0.7611 -1.6895 0.0000
+0.7854 -1.7235 0.0000
+0.8202 -1.7791 0.0000
+0.8275 -1.7746 0.0000
+0.8288 -1.7574 0.0000
+0.8283 -1.7366 0.0000
+0.8267 -1.7140 0.0000
+0.8268 -1.6951 0.0000
+0.8284 -1.6798 0.0000
+0.8308 -1.6664 0.0000
+0.8318 -1.6502 0.0000
+0.8276 -1.6243 0.0000
+0.8301 -1.6118 0.0000
+0.8339 -1.6019 0.0000
+0.8353 -1.5877 0.0000
+0.8436 -1.5867 0.0000
+0.8515 -1.5847 0.0000
+0.8613 -1.5863 0.0000
+0.8716 -1.5886 0.0000
+0.8824 -1.5918 0.0000
+0.8917 -1.5923 0.0000
+0.8987 -1.5884 0.0000
+0.9071 -1.5871 0.0000
+0.9155 -1.5857 0.0000
+0.9285 -1.5920 0.0000
+0.9364 -1.5897 0.0000
+0.9459 -1.5899 0.0000
+0.9528 -1.5858 0.0000
+0.9670 -1.5936 0.0000
+0.9755 -1.5919 0.0000
+0.9845 -1.5910 0.0000
+0.9984 -1.5977 0.0000
+1.0080 -1.5976 0.0000
+1.0176 -1.5974 0.0000
+1.0279 -1.5980 0.0000
+1.0370 -1.5968 0.0000
+1.0472 -1.5973 0.0000
+1.0581 -1.5986 0.0000
+1.0684 -1.5989 0.0000
+1.0826 -1.6050 0.0000
+1.0896 -1.6003 0.0000
+1.1028 -1.6046 0.0000
+1.1138 -1.6055 0.0000
+1.1213 -1.6014 0.0000
+1.1301 -1.5990 0.0000
+1.1393 -1.5973 0.0000
+1.1498 -1.5972 0.0000
+1.1673 -1.6067 0.0000
+1.1785 -1.6072 0.0000
+1.1873 -1.6045 0.0000
+1.2014 -1.6089 0.0000
+1.2115 -1.6077 0.0000
+1.2239 -1.6095 0.0000
+1.2346 -1.6089 0.0000
+1.2446 -1.6075 0.0000
+1.2590 -1.6115 0.0000
+1.2636 -1.6028 0.0000
+1.2793 -1.6083 0.0000
+1.2907 -1.6081 0.0000
+1.2995 -1.6048 0.0000
+1.3103 -1.6038 0.0000
+1.3129 -1.5926 0.0000
+1.3038 -1.5677 0.0000
+1.3061 -1.5566 0.0000
+1.3065 -1.5433 0.0000
+1.3093 -1.5330 0.0000
+1.3075 -1.5174 0.0000
+1.3088 -1.5056 0.0000
+1.3088 -1.4924 0.0000
+1.3087 -1.4792 0.0000
+1.3098 -1.4675 0.0000
+1.3108 -1.4558 0.0000
+1.3111 -1.4434 0.0000
+1.3120 -1.4318 0.0000
+1.3087 -1.4158 0.0000
+1.3081 -1.4027 0.0000
+1.3142 -1.3970 0.0000
+1.3127 -1.3833 0.0000
+1.3097 -1.3682 0.0000
+1.3143 -1.3610 0.0000
+1.3244 -1.3595 0.0000
+1.3443 -1.3680 0.0000
+1.3580 -1.3699 0.0000
+1.3647 -1.3647 0.0000
+1.3756 -1.3637 0.0000
+1.3901 -1.3661 0.0000
+1.3968 -1.3607 0.0000
+1.4121 -1.3636 0.0000
+1.4245 -1.3637 0.0000
+1.4370 -1.3636 0.0000
+1.4487 -1.3628 0.0000
+1.4664 -1.3674 0.0000
+1.4753 -1.3637 0.0000
+1.4908 -1.3660 0.0000
+1.5108 -1.3723 0.0000
+1.5249 -1.3731 0.0000
+1.5309 -1.3664 0.0000
+1.5428 -1.3650 0.0000
+1.5556 -1.3642 0.0000
+1.5706 -1.3653 0.0000
+1.5826 -1.3636 0.0000
+1.6029 -1.3690 0.0000
+1.6165 -1.3685 0.0000
+1.6317 -1.3691 0.0000
+1.6484 -1.3710 0.0000
+1.6667 -1.3739 0.0000
+1.6936 -1.3837 0.0000
+1.7035 -1.3795 0.0000
+1.7228 -1.3827 0.0000
+1.7405 -1.3845 0.0000
+1.7536 -1.3824 0.0000
+1.7707 -1.3834 0.0000
+1.7877 -1.3842 0.0000
+1.8065 -1.3861 0.0000
+1.8252 -1.3879 0.0000
+1.8440 -1.3896 0.0000
+1.8589 -1.3881 0.0000
+1.8786 -1.3901 0.0000
+1.8968 -1.3908 0.0000
+1.9109 -1.3883 0.0000
+1.9389 -1.3958 0.0000
+1.9441 -1.3867 0.0000
+1.9518 -1.3794 0.0000
+1.9660 -1.3766 0.0000
+1.9810 -1.3743 0.0000
+2.0158 -1.3854 0.0000
+2.0276 -1.3806 0.0000
+2.0626 -1.3913 0.0000
+2.0770 -1.3878 0.0000
+2.1014 -1.3909 0.0000
+2.1200 -1.3899 0.0000
+2.1428 -1.3916 0.0000
+2.1589 -1.3887 0.0000
+2.1903 -1.3954 0.0000
+2.2293 -1.4066 0.0000
+2.2762 -1.4223 0.0000
+2.3342 -1.4445 0.0000
+2.4079 -1.4755 0.0000
+2.7776 -1.6855 0.0000
+2.8184 -1.6934 0.0000
+2.8421 -1.6908 0.0000
+2.8744 -1.6931 0.0000
+2.9154 -1.7002 0.0000
+2.9505 -1.7035 0.0000
+2.9797 -1.7030 0.0000
+3.0062 -1.7008 0.0000
+3.0450 -1.7053 0.0000
+3.0769 -1.7056 0.0000
+3.1150 -1.7090 0.0000
+3.1444 -1.7073 0.0000
+3.1747 -1.7058 0.0000
+3.2201 -1.7122 0.0000
+3.2656 -1.7181 0.0000
+3.2908 -1.7131 0.0000
+3.3045 -1.7019 0.0000
+3.3137 -1.6884 0.0000
+3.3183 -1.6726 0.0000
+3.3220 -1.6563 0.0000
+3.3202 -1.6374 0.0000
+3.3139 -1.6163 0.0000
+3.3200 -1.6014 0.0000
+3.3233 -1.5851 0.0000
+3.3266 -1.5689 0.0000
+3.3280 -1.5519 0.0000
+3.3229 -1.5319 0.0000
+3.3286 -1.5169 0.0000
+3.3225 -1.4967 0.0000
+3.3217 -1.4789 0.0000
+3.3235 -1.4624 0.0000
+3.3188 -1.4431 0.0000
+3.3205 -1.4266 0.0000
+3.3221 -1.4101 0.0000
+3.3227 -1.3933 0.0000
+3.3269 -1.3780 0.0000
+3.3273 -1.3612 0.0000
+3.3184 -1.3407 0.0000
+3.3205 -1.3247 0.0000
+3.3290 -1.3113 0.0000
+3.3291 -1.2946 0.0000
+3.3273 -1.2772 0.0000
+3.3263 -1.2602 0.0000
+3.3252 -1.2432 0.0000
+3.3278 -1.2277 0.0000
+3.3322 -1.2128 0.0000
+3.3280 -1.1949 0.0000
+3.3285 -1.1787 0.0000
+3.3345 -1.1645 0.0000
+3.3226 -1.1440 0.0000
+3.3294 -1.1302 0.0000
+3.3343 -1.1156 0.0000
+3.3420 -1.1020 0.0000
+3.3468 -1.0874 0.0000
+3.3410 -1.0695 0.0000
+3.3390 -1.0528 0.0000
+3.3340 -1.0352 0.0000
+3.3299 -1.0180 0.0000
+3.3343 -1.0035 0.0000
+3.3386 -0.9889 0.0000
+3.3391 -0.9732 0.0000
+3.3413 -0.9581 0.0000
+3.3426 -0.9427 0.0000
+3.3428 -0.9271 0.0000
+3.3401 -0.9106 0.0000
+3.3431 -0.8958 0.0000
+3.3460 -0.8809 0.0000
+3.3488 -0.8661 0.0000
+3.3400 -0.8482 0.0000
+3.3456 -0.8341 0.0000
+3.3511 -0.8200 0.0000
+3.3411 -0.8021 0.0000
+3.3348 -0.7852 0.0000
+3.3392 -0.7709 0.0000
+3.3493 -0.7579 0.0000
+3.3536 -0.7435 0.0000
+3.3519 -0.7278 0.0000
+3.3550 -0.7131 0.0000
+3.3434 -0.6954 0.0000
+3.3396 -0.6794 0.0000
+3.3396 -0.6643 0.0000
+3.3385 -0.6489 0.0000
+3.3393 -0.6340 0.0000
+3.3470 -0.6203 0.0000
+3.3418 -0.6043 0.0000
+3.3464 -0.5901 0.0000
+3.3529 -0.5761 0.0000
+3.3573 -0.5618 0.0000
+3.3558 -0.5465 0.0000
+3.3532 -0.5311 0.0000
+3.3525 -0.5160 0.0000
+3.3557 -0.5015 0.0000
+3.3460 -0.4851 0.0000
+3.3610 -0.4724 0.0000
+3.3699 -0.4586 0.0000
+3.3481 -0.4408 0.0000
+3.3659 -0.4282 0.0000
+3.3677 -0.4135 0.0000
+3.3695 -0.3988 0.0000
+3.3712 -0.3841 0.0000
+3.3738 -0.3695 0.0000
+3.3744 -0.3547 0.0000
+3.3710 -0.3394 0.0000
+3.3704 -0.3245 0.0000
+3.3668 -0.3094 0.0000
+3.3761 -0.2954 0.0000
+3.3724 -0.2802 0.0000
+3.3706 -0.2653 0.0000
+3.3767 -0.2509 0.0000
+3.3778 -0.2362 0.0000
+3.3788 -0.2215 0.0000
+3.3767 -0.2065 0.0000
+3.3766 -0.1917 0.0000
+3.3774 -0.1770 0.0000
+3.3781 -0.1623 0.0000
+3.3768 -0.1474 0.0000
+3.3854 -0.1330 0.0000
+3.3919 -0.1184 0.0000
+3.3874 -0.1035 0.0000
+3.3808 -0.0885 0.0000
+3.3782 -0.0737 0.0000
+3.3785 -0.0590 0.0000
+3.3827 -0.0443 0.0000
+3.3769 -0.0295 0.0000
+3.3850 -0.0148 0.0000
+3.3870 0.0000 0.0000
+3.3910 0.0148 0.0000
+3.3929 0.0296 0.0000
+3.3877 0.0443 0.0000
+3.3875 0.0591 0.0000
+3.3852 0.0739 0.0000
+3.3808 0.0885 0.0000
+3.3684 0.1029 0.0000
+3.1851 0.1112 0.0000
+2.9267 0.1150 0.0000
+2.8843 0.1259 0.0000
+2.8557 0.1372 0.0000
+2.8361 0.1486 0.0000
+2.8304 0.1607 0.0000
+2.8137 0.1721 0.0000
+2.8090 0.1841 0.0000
+2.8121 0.1966 0.0000
+2.8093 0.2088 0.0000
+2.8083 0.2210 0.0000
+2.8113 0.2336 0.0000
+2.8152 0.2463 0.0000
+2.8122 0.2584 0.0000
+2.8080 0.2704 0.0000
+2.8008 0.2820 0.0000
+2.7946 0.2937 0.0000
+2.7933 0.3059 0.0000
+2.7899 0.3179 0.0000
+2.8024 0.3317 0.0000
+2.8000 0.3438 0.0000
+2.7975 0.3559 0.0000
+2.7969 0.3682 0.0000
+2.8022 0.3814 0.0000
+2.8054 0.3943 0.0000
+2.8037 0.4065 0.0000
+2.7880 0.4167 0.0000
+2.7872 0.4290 0.0000
+2.7873 0.4415 0.0000
+2.7873 0.4539 0.0000
+2.7892 0.4668 0.0000
+2.7872 0.4789 0.0000
+2.7909 0.4921 0.0000
+2.8006 0.5064 0.0000
+2.8013 0.5192 0.0000
+2.7990 0.5314 0.0000
+2.7967 0.5436 0.0000
+2.7943 0.5558 0.0000
+2.8006 0.5698 0.0000
+2.8069 0.5838 0.0000
+2.8034 0.5959 0.0000
+2.8076 0.6096 0.0000
+2.8127 0.6236 0.0000
+2.8129 0.6365 0.0000
+2.8140 0.6497 0.0000
+2.8316 0.6667 0.0000
+2.8325 0.6800 0.0000
+2.8742 0.7033 0.0000
+2.9177 0.7275 0.0000
+2.9833 0.7577 0.0000
+3.0129 0.7792 0.0000
+2.7832 0.7327 0.0000
+2.6457 0.7089 0.0000
+2.6049 0.7102 0.0000
+2.5748 0.7141 0.0000
+2.5467 0.7182 0.0000
+2.5618 0.7346 0.0000
+2.5729 0.7499 0.0000
+2.5821 0.7649 0.0000
+2.5740 0.7747 0.0000
+2.5686 0.7853 0.0000
+2.5633 0.7959 0.0000
+2.5722 0.8110 0.0000
+2.5724 0.8234 0.0000
+2.5717 0.8356 0.0000
+2.5756 0.8493 0.0000
+2.5756 0.8618 0.0000
+2.5775 0.8750 0.0000
+2.5737 0.8862 0.0000
+2.5641 0.8954 0.0000
+2.5593 0.9063 0.0000
+2.5694 0.9225 0.0000
+2.5757 0.9375 0.0000
+2.5716 0.9487 0.0000
+2.3349 0.8963 0.0000
+2.3310 0.9065 0.0000
+2.3112 0.9104 0.0000
+2.0963 0.8363 0.0000
+2.0500 0.8283 0.0000
+2.0186 0.8258 0.0000
+1.9984 0.8277 0.0000
+1.9707 0.8264 0.0000
+1.9515 0.8283 0.0000
+1.9285 0.8286 0.0000
+1.9057 0.8286 0.0000
+1.8883 0.8309 0.0000
+1.8746 0.8346 0.0000
+1.8618 0.8387 0.0000
+1.8399 0.8385 0.0000
+1.8190 0.8386 0.0000
+1.8054 0.8419 0.0000
+1.7836 0.8412 0.0000
+1.7664 0.8425 0.0000
+1.7510 0.8446 0.0000
+1.7410 0.8491 0.0000
+1.7301 0.8532 0.0000
+1.7040 0.8496 0.0000
+1.6886 0.8511 0.0000
+1.6689 0.8503 0.0000
+1.6571 0.8535 0.0000
+1.6463 0.8570 0.0000
+1.6328 0.8591 0.0000
+1.6123 0.8573 0.0000
+1.6050 0.8624 0.0000
+1.6003 0.8689 0.0000
+1.6079 0.8821 0.0000
+1.6093 0.8920 0.0000
+1.6063 0.8996 0.0000
+1.6058 0.9085 0.0000
+1.6044 0.9170 0.0000
+1.6108 0.9300 0.0000
+1.6085 0.9380 0.0000
+1.6044 0.9450 0.0000
+1.6037 0.9541 0.0000
+1.6106 0.9678 0.0000
+1.6081 0.9758 0.0000
+1.6106 0.9870 0.0000
+1.6114 0.9972 0.0000
+1.6164 1.0100 0.0000
+1.6153 1.0192 0.0000
+1.6100 1.0257 0.0000
+1.6140 1.0381 0.0000
+1.6203 1.0522 0.0000
+1.6190 1.0615 0.0000
+1.6186 1.0713 0.0000
+1.6180 1.0811 0.0000
+1.6216 1.0938 0.0000
+1.6209 1.1037 0.0000
+1.6186 1.1124 0.0000
+1.6039 1.1126 0.0000
+1.4671 1.0273 0.0000
+1.2478 0.8819 0.0000
+1.1845 0.8449 0.0000
+1.1573 0.8331 0.0000
+1.1391 0.8276 0.0000
+1.1419 0.8373 0.0000
+1.1479 0.8494 0.0000
+1.1498 0.8586 0.0000
+1.1492 0.8660 0.0000
+1.1550 0.8783 0.0000
+1.1543 0.8857 0.0000
+1.1631 0.9006 0.0000
+1.1639 0.9093 0.0000
+1.1623 0.9163 0.0000
+1.1669 0.9282 0.0000
+1.1644 0.9345 0.0000
+1.1618 0.9408 0.0000
+1.1631 0.9503 0.0000
+1.1628 0.9586 0.0000
+1.1610 0.9656 0.0000
+1.1606 0.9738 0.0000
+1.1586 0.9808 0.0000
+1.1665 0.9963 0.0000
+1.1659 1.0046 0.0000
+1.1660 1.0136 0.0000
+1.1714 1.0273 0.0000
+1.1744 1.0390 0.0000
+1.1743 1.0481 0.0000
+1.1742 1.0572 0.0000
+1.1666 1.0596 0.0000
+1.1619 1.0647 0.0000
+1.1690 1.0807 0.0000
+1.1716 1.0926 0.0000
+1.1705 1.1011 0.0000
+1.1729 1.1131 0.0000
+1.1717 1.1216 0.0000
+1.1747 1.1344 0.0000
+1.1697 1.1395 0.0000
+1.1676 1.1474 0.0000
+1.1746 1.1644 0.0000
+1.1717 1.1717 0.0000
+1.1729 1.1832 0.0000
+1.1747 1.1954 0.0000
+1.1786 1.2098 0.0000
+1.1740 1.2157 0.0000
+1.1811 1.2338 0.0000
+1.1778 1.2411 0.0000
+1.1744 1.2484 0.0000
+1.1730 1.2579 0.0000
+1.1798 1.2763 0.0000
+1.1796 1.2873 0.0000
+1.1787 1.2976 0.0000
+1.1770 1.3072 0.0000
+1.1773 1.3190 0.0000
+1.1801 1.3339 0.0000
+1.1769 1.3420 0.0000
+1.1829 1.3607 0.0000
+1.1815 1.3712 0.0000
+1.1820 1.3839 0.0000
+1.1850 1.3998 0.0000
+1.1859 1.4134 0.0000
+1.1862 1.4262 0.0000
+1.1876 1.4406 0.0000
+1.1870 1.4528 0.0000
+1.1831 1.4610 0.0000
+1.1811 1.4716 0.0000
+1.1778 1.4807 0.0000
+1.1893 1.5086 0.0000
+1.1821 1.5130 0.0000
+1.1797 1.5237 0.0000
+1.2145 1.5827 0.0000
+1.2669 1.6660 0.0000
+1.2656 1.6795 0.0000
+1.2553 1.6810 0.0000
+1.2432 1.6801 0.0000
+1.2347 1.6839 0.0000
+1.2255 1.6868 0.0000
+1.2094 1.6800 0.0000
+1.1974 1.6787 0.0000
+1.1878 1.6806 0.0000
+1.1804 1.6858 0.0000
+1.1691 1.6852 0.0000
+1.1577 1.6845 0.0000
+1.1498 1.6887 0.0000
+1.1419 1.6929 0.0000
+1.1300 1.6912 0.0000
+1.1138 1.6828 0.0000
+1.1048 1.6851 0.0000
+1.0964 1.6882 0.0000
+1.0890 1.6930 0.0000
+1.0725 1.6834 0.0000
+1.0683 1.6931 0.0000
+1.0609 1.6978 0.0000
+1.0398 1.6803 0.0000
+1.0325 1.6848 0.0000
+1.0215 1.6833 0.0000
+1.0131 1.6860 0.0000
+1.0057 1.6905 0.0000
+0.9958 1.6905 0.0000
+0.9859 1.6905 0.0000
+0.9795 1.6965 0.0000
+0.9761 1.7077 0.0000
+0.9720 1.7181 0.0000
+0.9719 1.7354 0.0000
+0.9813 1.7702 0.0000
+0.9769 1.7806 0.0000
+0.9662 1.7796 0.0000
+2.3745 4.6104 0.0000
+2.3548 4.6217 0.0000
+2.3032 4.5694 0.0000
+2.2747 4.5624 0.0000
+2.2548 4.5723 0.0000
+2.1801 4.5707 0.0000
+2.1601 4.5802 0.0000
+2.1435 4.5968 0.0000
+2.1184 4.5952 0.0000
+2.0938 4.5944 0.0000
+2.0762 4.6090 0.0000
+2.0544 4.6143 0.0000
+2.0262 4.6049 0.0000
+1.9914 4.5798 0.0000
+1.9710 4.5875 0.0000
+1.9505 4.5952 0.0000
+1.0298 4.6452 0.0000
+1.0106 4.6546 0.0000
+0.9946 4.6795 0.0000
+0.9671 4.6495 0.0000
+0.9486 4.6625 0.0000
+0.9265 4.6577 0.0000
+0.8957 4.6078 0.0000
+0.8755 4.6116 0.0000
+0.8550 4.6134 0.0000
+0.8474 4.6860 0.0000
+1.1499 6.5214 0.0000
+1.1182 6.5076 0.0000
+1.0893 6.5095 0.0000
+1.0612 6.5161 0.0000
+1.0323 6.5178 0.0000
+1.0034 6.5192 0.0000
+0.9745 6.5206 0.0000
+0.9456 6.5218 0.0000
+0.9172 6.5259 0.0000
+0.8881 6.5258 0.0000
+0.8585 6.5207 0.0000
+0.8294 6.5195 0.0000
+0.8003 6.5181 0.0000
+0.7722 6.5245 0.0000
+0.7436 6.5268 0.0000
+0.7144 6.5230 0.0000
+0.6854 6.5211 0.0000
+0.6568 6.5230 0.0000
+0.6284 6.5258 0.0000
+0.6001 6.5305 0.0000
+0.5717 6.5350 0.0000
+0.5498 6.6172 0.0000
+0.6340 8.0551 0.0000
+0.5988 8.0578 0.0000
+0.5055 7.2293 0.0000
+-0.6364 4.6759 0.0000
+-0.6587 4.6869 0.0000
+-0.6771 4.6702 0.0000
+-0.6974 4.6662 0.0000
+-0.7167 4.6562 0.0000
+-0.7370 4.6530 0.0000
+-0.7590 4.6606 0.0000
+-0.7845 4.6878 0.0000
+-0.8059 4.6903 0.0000
+-0.8240 4.6729 0.0000
+-0.8443 4.6693 0.0000
+-0.8662 4.6734 0.0000
+-0.8864 4.6686 0.0000
+-0.9067 4.6647 0.0000
+-0.7552 3.7966 0.0000
+-0.7638 3.7541 0.0000
+-0.7763 3.7321 0.0000
+-1.7400 8.1861 0.0000
+-1.7766 8.1824 0.0000
+-0.7982 3.6006 0.0000
+-1.6724 7.3911 0.0000
+-1.6912 7.3253 0.0000
+-0.7969 3.3844 0.0000
+-0.7722 2.9858 0.0000
+-0.7852 2.9824 0.0000
+-0.7842 2.9268 0.0000
+-0.7970 2.9233 0.0000
+-0.8119 2.9275 0.0000
+-1.8754 6.6496 0.0000
+-1.9091 6.6577 0.0000
+-0.8331 2.8581 0.0000
+-0.8268 2.7911 0.0000
+-2.0180 6.7049 0.0000
+-2.0486 6.7008 0.0000
+-2.0678 6.6594 0.0000
+-0.8246 2.5006 0.0000
+-0.8355 2.4969 0.0000
+-0.8549 2.2864 0.0000
+-0.8592 2.2677 0.0000
+-0.8655 2.2546 0.0000
+-0.0460 0.1184 0.0000
+-0.0440 0.1117 0.0000
+-0.0422 0.1059 0.0000
+-0.0476 0.1178 0.0000
+-0.0488 0.1194 0.0000
+-0.0486 0.1173 0.0000
+-0.0437 0.1042 0.0000
+-0.0461 0.1086 0.0000
+-0.0446 0.1038 0.0000
+-0.0475 0.1091 0.0000
+-0.0471 0.1071 0.0000
+-0.0476 0.1069 0.0000
+-0.0460 0.1021 0.0000
+-0.0431 0.0946 0.0000
+-0.0431 0.0935 0.0000
+-0.0423 0.0906 0.0000
+-0.0431 0.0913 0.0000
+-0.0452 0.0948 0.0000
+-0.0461 0.0955 0.0000
+-0.0447 0.0917 0.0000
+-0.0447 0.0906 0.0000
+-0.0513 0.1029 0.0000
+-0.0554 0.1098 0.0000
+-0.0513 0.1007 0.0000
+-0.0513 0.0996 0.0000
+-0.0531 0.1020 0.0000
+-0.0559 0.1062 0.0000
+-0.0582 0.1095 0.0000
+-0.0559 0.1039 0.0000
+-0.0577 0.1063 0.0000
+-0.0529 0.0964 0.0000
+-0.0596 0.1076 0.0000
+-0.0562 0.1003 0.0000
+-0.0606 0.1071 0.0000
+-0.0615 0.1077 0.0000
+-0.0560 0.0970 0.0000
+-0.0549 0.0942 0.0000
+-0.0589 0.0999 0.0000
+-0.0634 0.1066 0.0000
+-0.0670 0.1114 0.0000
+-0.0607 0.1000 0.0000
+-0.0690 0.1125 0.0000
+-0.2189 0.3537 0.0000
+-0.2321 0.3714 0.0000
+-0.2481 0.3933 0.0000
+-0.0623 0.0978 0.0000
+-0.9051 1.4071 0.0000
+-0.9112 1.4031 0.0000
+-0.9124 1.3916 0.0000
+-0.9112 1.3767 0.0000
+-0.8956 1.3403 0.0000
+-0.8640 1.2809 0.0000
+-0.8515 1.2506 0.0000
+-0.8490 1.2354 0.0000
+-0.8544 1.2316 0.0000
+-0.8581 1.2255 0.0000
+-0.8588 1.2152 0.0000
+-0.8577 1.2024 0.0000
+-0.8571 1.1906 0.0000
+-0.8552 1.1771 0.0000
+-0.8609 1.1742 0.0000
+-0.8619 1.1648 0.0000
+-0.8598 1.1514 0.0000
+-0.8546 1.1341 0.0000
+-0.8619 1.1335 0.0000
+-0.8638 1.1258 0.0000
+-0.8724 1.1267 0.0000
+-0.8810 1.1276 0.0000
+-0.9026 1.1450 0.0000
+-0.9195 1.1559 0.0000
+-0.9239 1.1511 0.0000
+-0.9415 1.1626 0.0000
+-0.9478 1.1600 0.0000
+-0.9548 1.1582 0.0000
+-0.9656 1.1610 0.0000
+-0.9745 1.1613 0.0000
+-0.9828 1.1609 0.0000
+-0.9891 1.1581 0.0000
+-0.9981 1.1583 0.0000
+-1.0057 1.1570 0.0000
+-1.0121 1.1541 0.0000
+-1.0218 1.1549 0.0000
+-1.0308 1.1549 0.0000
+-1.0412 1.1563 0.0000
+-1.0516 1.1577 0.0000
+-1.0559 1.1524 0.0000
+-1.0705 1.1580 0.0000
+-1.0755 1.1533 0.0000
+-0.3159 0.3358 0.0000
+-0.2547 0.2684 0.0000
+-0.2178 0.2275 0.0000
+-0.2133 0.2208 0.0000
+-0.2233 0.2292 0.0000
+-0.2551 0.2596 0.0000
+-0.2901 0.2926 0.0000
+-0.3493 0.3493 0.0000

map_scan_match/scan_line.txt

@@ -0,0 +1,170 @@
+2.62779999 0.72880000 0.00000000
+2.63330007 0.74269998 0.00000000
+2.65499997 0.76130003 0.00000000
+2.65840006 0.77480000 0.00000000
+2.65019989 0.78500003 0.00000000
+2.64100003 0.79479998 0.00000000
+2.63940001 0.80690002 0.00000000
+2.62820005 0.81610000 0.00000000
+2.61800003 0.82539999 0.00000000
+2.60770011 0.83469999 0.00000000
+2.59540009 0.84329998 0.00000000
+2.59080005 0.85430002 0.00000000
+2.56240010 0.85740000 0.00000000
+2.55859995 0.86849999 0.00000000
+2.54629993 0.87680000 0.00000000
+2.54239988 0.88789999 0.00000000
+2.53850007 0.89889997 0.00000000
+2.53080010 0.90869999 0.00000000
+2.52589989 0.91930002 0.00000000
+2.50869989 0.92549998 0.00000000
+2.49900007 0.93440002 0.00000000
+2.48370004 0.94099998 0.00000000
+2.47589993 0.95039999 0.00000000
+2.47449994 0.96230000 0.00000000
+2.46650004 0.97160000 0.00000000
+2.45479989 0.97939998 0.00000000
+2.44959998 0.98970002 0.00000000
+2.43790007 0.99739999 0.00000000
+2.42980003 1.00650001 0.00000000
+2.41989994 1.01470006 0.00000000
+2.41630006 1.02569997 0.00000000
+2.41000009 1.03540003 0.00000000
+2.40089989 1.04390001 0.00000000
+2.39080000 1.05200005 0.00000000
+2.37980008 1.05949998 0.00000000
+2.37150002 1.06830001 0.00000000
+2.36129999 1.07609999 0.00000000
+2.35479999 1.08560002 0.00000000
+2.34010005 1.09119999 0.00000000
+2.32990003 1.09879994 0.00000000
+2.31780005 1.10560000 0.00000000
+2.31660008 1.11740005 0.00000000
+2.30990005 1.12660003 0.00000000
+2.29329991 1.13090003 0.00000000
+2.28830004 1.14090002 0.00000000
+2.27259994 1.14549994 0.00000000
+2.27920008 1.16129994 0.00000000
+2.27049994 1.16939998 0.00000000
+2.26719999 1.18019998 0.00000000
+2.24880004 1.18309999 0.00000000
+2.24620008 1.19430006 0.00000000
+2.22340012 1.19470000 0.00000000
+2.22160006 1.20630002 0.00000000
+2.21900010 1.21739995 0.00000000
+2.21190000 1.22609997 0.00000000
+2.20479989 1.23469996 0.00000000
+2.19589996 1.24240005 0.00000000
+2.17659998 1.24399996 0.00000000
+2.16510010 1.25000000 0.00000000
+2.16129994 1.26040006 0.00000000
+2.15319991 1.26830006 0.00000000
+2.14510012 1.27620006 0.00000000
+2.14120007 1.28659999 0.00000000
+2.13560009 1.29589999 0.00000000
+2.11540008 1.29630005 0.00000000
+2.10630012 1.30340004 0.00000000
+2.10400009 1.31470001 0.00000000
+2.09990001 1.32500005 0.00000000
+2.09330010 1.33360004 0.00000000
+2.08909988 1.34379995 0.00000000
+2.08489990 1.35399997 0.00000000
+2.07229996 1.35870004 0.00000000
+2.06299996 1.36549997 0.00000000
+2.05290008 1.37170005 0.00000000
+2.03940010 1.37559998 0.00000000
+2.03010011 1.38220000 0.00000000
+2.02239990 1.38999999 0.00000000
+2.01220012 1.39590001 0.00000000
+2.00119996 1.40120006 0.00000000
+1.99829996 1.41229999 0.00000000
+1.99539995 1.42330003 0.00000000
+1.98189998 1.42670000 0.00000000
+1.98780000 1.44420004 0.00000000
+1.98140001 1.45280004 0.00000000
+1.97749996 1.46329999 0.00000000
+1.96229994 1.46529996 0.00000000
+1.93509996 1.45819998 0.00000000
+1.92949998 1.46720004 0.00000000
+1.90960002 1.46529996 0.00000000
+1.90160000 1.47239995 0.00000000
+1.90149999 1.48559999 0.00000000
+1.89579999 1.49450004 0.00000000
+1.88999999 1.50339997 0.00000000
+1.88030005 1.50909996 0.00000000
+1.87210000 1.51600003 0.00000000
+1.86399996 1.52289999 0.00000000
+1.85810006 1.53170002 0.00000000
+1.85140002 1.53980005 0.00000000
+1.84459996 1.54779994 0.00000000
+1.82949996 1.54879999 0.00000000
+1.81359994 1.54890001 0.00000000
+1.82500005 1.57249999 0.00000000
+1.81280005 1.57589996 0.00000000
+1.81340003 1.59029996 0.00000000
+1.80799997 1.59959996 0.00000000
+1.80019999 1.60679996 0.00000000
+1.79770005 1.61860001 0.00000000
+1.77499998 1.61230004 0.00000000
+1.76650000 1.61870003 0.00000000
+1.75800002 1.62500000 0.00000000
+1.75090003 1.63269997 0.00000000
+1.75320005 1.64919996 0.00000000
+1.74960005 1.66030002 0.00000000
+1.73800004 1.66380000 0.00000000
+1.72640002 1.66719997 0.00000000
+1.72409999 1.67960000 0.00000000
+1.71679997 1.68710005 0.00000000
+1.70940006 1.69459999 0.00000000
+1.69990003 1.69990003 0.00000000
+1.69459999 1.70940006 0.00000000
+1.68710005 1.71679997 0.00000000
+1.67960000 1.72409999 0.00000000
+1.67200005 1.73150003 0.00000000
+1.66310000 1.73730004 0.00000000
+1.65890002 1.74820006 0.00000000
+1.65540004 1.75969994 0.00000000
+1.64909995 1.76839995 0.00000000
+1.63460004 1.76820004 0.00000000
+1.62609994 1.77460003 0.00000000
+1.62170005 1.78540003 0.00000000
+1.61600006 1.79470003 0.00000000
+1.60810006 1.80170000 0.00000000
+1.59759998 1.80569994 0.00000000
+1.59430003 1.81789994 0.00000000
+1.58500004 1.82340002 0.00000000
+1.57449996 1.82720006 0.00000000
+1.56519997 1.83260000 0.00000000
+1.55850005 1.84089994 0.00000000
+1.55170000 1.84920001 0.00000000
+1.54359996 1.85599995 0.00000000
+1.53359997 1.86039996 0.00000000
+1.52479994 1.86629999 0.00000000
+1.51789999 1.87450004 0.00000000
+1.51040006 1.88189995 0.00000000
+1.51020002 1.89859998 0.00000000
+1.50189996 1.90520000 0.00000000
+1.49300003 1.91090000 0.00000000
+1.48590004 1.91900003 0.00000000
+1.47749996 1.92550004 0.00000000
+1.46840000 1.93110001 0.00000000
+1.45940006 1.93669999 0.00000000
+1.45330000 1.94620001 0.00000000
+1.44780004 1.95659995 0.00000000
+1.43920004 1.96290004 0.00000000
+1.42949998 1.96749997 0.00000000
+1.42209995 1.97539997 0.00000000
+1.41340005 1.98160005 0.00000000
+1.40769994 1.99179995 0.00000000
+1.40470004 2.00609994 0.00000000
+1.39250004 2.00729990 0.00000000
+1.38150001 2.00999999 0.00000000
+1.37549996 2.02020001 0.00000000
+1.36829996 2.02870011 0.00000000
+1.36059999 2.03629994 0.00000000
+1.35109997 2.04139996 0.00000000
+1.34220004 2.04719996 0.00000000
+1.33329999 2.05310011 0.00000000
+1.32700002 2.06310010 0.00000000
+1.30620003 2.05029988 0.00000000
+1.30420005 2.06699991 0.00000000

map_scan_match/src/map_scan_match_node.cpp

@@ -0,0 +1,82 @@
+#include "scan_match.h"
+#include<ceres/ceres.h>
+
+using namespace std;
+using namespace cv;
+
+//构建代价函数结构体，abc为待优化参数，residual为残差。
+struct CURVE_FITTING_COST
+{
+  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
+  template <typename T>
+  //使用仿函数重载
+  bool operator()(const T* const abc,T* residual)const
+  {
+    residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
+    return true;
+  }
+  const double _x,_y;
+};
+
+//主函数
+int main(int argc, char** argv)
+{
+//   //参数初始化设置，abc初始化为0，白噪声方差为1（使用OpenCV的随机数产生器）。
+//   double a=3,b=2,c=1;
+//   double w=1;
+//   RNG rng;
+//   double abc[3]={0,0,0};
+
+// //生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
+//   vector<double> x_data,y_data;
+//   for(int i=0;i<1000;i++)
+//   {
+//     double x=i/1000.0;
+//     x_data.push_back(x);
+//     y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w));
+//   }
+
+// //反复使用AddResidualBlock方法（逐个散点，反复1000次）
+// //将每个点的残差累计求和构建最小二乘优化式
+// //不使用核函数，待优化参数是abc
+//   ceres::Problem problem;
+//   for(int i=0;i<1000;i++)
+//   {
+//     problem.AddResidualBlock(
+//       new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
+//         new CURVE_FITTING_COST(x_data[i],y_data[i])
+//       ),
+//       nullptr,
+//       abc
+//     );
+//   }
+
+// //配置求解器并求解，输出结果
+//   ceres::Solver::Options options;
+//   options.linear_solver_type=ceres::DENSE_QR;
+//   options.minimizer_progress_to_stdout = true;
+//   options.parameter_tolerance = 1e-9;
+//   options.function_tolerance = 1e-9;
+//   ceres::Solver::Summary summary;
+//   ceres::Solve(options,&problem,&summary);
+//   cout<<"a= "<<abc[0]<<endl;
+//   cout<<"b= "<<abc[1]<<endl;
+//   cout<<"c= "<<abc[2]<<endl;
+
+ros::init(argc, argv, "map_scan_matcher");
+Scan_match sm("/home/youchen/Documents/git_ws/src/map_scan_match/200511map_origin.yaml");
+// sm.Solve();
+
+ros::Rate loop_rate(40);
+while(ros::ok()){
+
+  // Eigen::Vector3d trans;
+  // trans << 1.4, -0.1, -0.75;
+  sm.Solve();
+  // std::cout << trans << std::endl;
+  ros::spinOnce();
+  loop_rate.sleep();
+}
+return 0;
+}
+

map_scan_match/src/scan_match.cpp

@@ -0,0 +1,285 @@
+/*
+ * @Description: match scan with grid map
+ * @Author: yct
+ * @Date: 2020-04-03 13:10:37
+ * @LastEditTime: 2020-05-17 23:11:23
+ * @LastEditors: yct
+ */
+
+#include "scan_match.h"
+
+bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+{
+    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    pointcloud->clear();
+    std::ifstream in(filename);
+    if (!in.is_open())
+    {
+        std::cout << "failed to open file " << filename << std::endl;
+        return false;
+    }
+    while (!in.eof())
+    {
+        std::string t_linestr;
+        if (getline(in, t_linestr))
+        {
+            std::vector<std::string> str_vector;
+            std::stringstream ss(t_linestr);
+            std::string num_str;
+            while (getline(ss, num_str, ' '))
+            {
+                str_vector.push_back(num_str);
+            }
+            if (str_vector.size() != 3)
+            {
+                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
+                return false;
+            }
+            pcl::PointXYZ t_cloud;
+            t_cloud.x = stod(str_vector[0]);
+            t_cloud.y = stod(str_vector[1]);
+            t_cloud.z = stod(str_vector[2]);
+            pointcloud->push_back(t_cloud);
+        }
+    }
+    in.close();
+    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
+    return true;
+}
+
+Scan_match::Scan_match(std::string filename)
+{
+    m_scan_subscriber = m_nh.subscribe("scan", 1, &Scan_match::scan_callback, this);
+    m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Scan_match::initial_pose_callback, this);
+    m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
+    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
+    m_trans << 0.0, 0.0, 0.0;
+    if (read_map(filename))
+    {
+        m_probability_array.rows = m_global_map.rows;
+        m_probability_array.cols = m_global_map.cols;
+        for (size_t i = 0; i < m_global_map.rows; i++)
+        {
+            for (size_t j = 0; j < m_global_map.cols; j++)
+            {
+                // 处理原始图片空白区域
+                double value = static_cast<double>(m_global_map.at<float>(i, j));
+                if(value < 0.51 && value > 0.49)
+                    value = 0.9;
+                m_probability_array.m_probability_vector.push_back(value);
+            }  
+        }
+        m_initialized = true;
+        std::cout << "initialized" << std::endl;
+    }
+    else
+    {
+        m_initialized = false;
+    }
+    m_initial_pose_updated = false;
+    m_initial_pose.setIdentity();
+
+}
+
+Scan_match::~Scan_match()
+{
+}
+
+#include <pcl/common/transforms.h>
+bool Scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map)
+{
+    // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    // trans[0] += m_map_origin[0];
+    // trans[1] += m_map_origin[1];
+    // double tr[3] = {0.0, 0.0, 0.0};
+    // std::cout<<"we get points: "<< cloud->size()<<std::endl;
+    ceres::Problem problem;
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<BicubicInterpolationCost, ceres::DYNAMIC, 3>(
+            new BicubicInterpolationCost(m_probability_array, cloud, m_resolution, m_map_origin, 10.0 / sqrt(static_cast<double>(cloud->size()))), cloud->size()),
+        nullptr,
+        trans.data());
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<TranslationCost, 2, 3>(
+            new TranslationCost(Eigen::Vector2d(trans[0], trans[1]), 10.0)),
+        nullptr,
+        trans.data());
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<RotationCost, 1, 3>(
+            new RotationCost(trans[2], 40.0)),
+        nullptr,
+        trans.data());
+
+    ceres::Solver::Options options;
+    options.linear_solver_type = ceres::DENSE_QR;
+    options.minimizer_progress_to_stdout = false;
+    options.max_num_iterations = 20;
+    options.num_threads = 1;
+    // options.parameter_tolerance = 1e-6;
+    // options.function_tolerance = 1e-6;
+    ceres::Solver::Summary summary;
+    ceres::Solve(options, &problem, &summary);
+    std::cout << summary.BriefReport() << std::endl;
+
+    // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    // int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
+    // std::cout << "real solve time us: " << duration << std::endl;
+
+    // 发布点云
+    // trans[0] -= m_map_origin[0];
+    // trans[1] -= m_map_origin[1];
+    std::cout << "final trans: "<< trans.transpose() << std::endl;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
+    transform_1.translation() << trans[0], trans[1], 0.0;
+    transform_1.rotate(Eigen::AngleAxisf(trans[2], Eigen::Vector3f::UnitZ()));
+    pcl::transformPointCloud(*cloud, *temp_cloud, transform_1);
+    sensor_msgs::PointCloud2 output_origin;
+    pcl::toROSMsg(*temp_cloud, output_origin);
+    output_origin.header.frame_id = "map";
+    m_cloud_publisher.publish(output_origin);
+}
+
+bool Scan_match::Solve()
+{
+    std::cout << "-------------- start solve ---------------" << std::endl;
+    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+    Eigen::Vector3d trans;
+    // if (m_initial_pose_updated)
+    // {
+        m_mutex.lock();
+        trans << m_initial_pose.getOrigin().getX(), m_initial_pose.getOrigin().getY(), asin(m_initial_pose.getRotation().getZ()) * 2.0;
+        std::cout << "init trans: "<< trans.transpose() << std::endl;
+        m_mutex.unlock();
+        m_initial_pose_updated = false;
+    // }else{
+        // trans << 1.5, -0.2, -0.75;
+        // trans << 0.7, 0.0, 0.1;
+    // }
+
+    // 点云抽析
+    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
+    approximate_voxel_filter.setLeafSize(m_resolution, m_resolution, m_resolution);
+    approximate_voxel_filter.setInputCloud(m_cloud);
+    approximate_voxel_filter.filter(*m_cloud);
+    std::cout<<"cloud size: "<<m_cloud->size()<<std::endl;
+
+    // // 临时添加，测试carto地图用
+    // trans << 0.0, 0.0, 0.0;
+    // cv::Mat t_map = cv::imread("/home/youchen/Documents/git_ws/src/map_scan_match/my1.jpg", cv::IMREAD_GRAYSCALE);
+    // t_map.convertTo(t_map, CV_32FC1, 1.0 / 255.0);
+    // read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
+
+    Solve(trans, m_cloud, m_global_map);
+    m_initial_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
+    m_initial_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));
+
+    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
+    std::cout << "solve time us: " << duration << std::endl;
+
+    // cv::Mat t_map = cv::imread("./src/map_scan_match/interpolate.jpg", cv::IMREAD_GRAYSCALE);
+    // cv::namedWindow("map", cv::WINDOW_FREERATIO);
+    // cv::imshow("map", t_map);
+    // cv::waitKey();
+}
+
+bool Scan_match::read_map(std::string conf_filename)
+{
+    std::cout << "conf_file: " << conf_filename << std::endl;
+    std::ifstream in(conf_filename);
+    if (!in.is_open())
+    {
+        std::cerr << "Failed to open " << conf_filename << std::endl;
+        return false;
+    }
+    while (!in.eof())
+    {
+        std::string t_line_str;
+        if (getline(in, t_line_str))
+        {
+            std::stringstream ss(t_line_str);
+            std::string t_str;
+            if (getline(ss, t_str, ' '))
+            {
+                if (t_str == "image:")
+                {
+                    m_map_path = t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1);
+                    m_global_map = cv::imread(m_map_path, cv::IMREAD_GRAYSCALE);
+                    // 图像归一化
+                    m_global_map.convertTo(m_global_map, CV_32FC1, 1.0 / 255.0);
+                    // // 值反转
+                    // m_global_map = cv::Mat::ones(m_global_map.size(), m_global_map.type()) - m_global_map;
+                    
+                    // std::cout<<m_global_map(cv::Range(0, 10), cv::Range(0, 10))<<std::endl;
+                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(20, 12))<<std::endl;
+                    // std::cout<<cv::Scalar(m_global_map.at<uchar>(5, 8))<<std::endl;
+                    // cv::namedWindow("origin map", cv::WINDOW_FREERATIO);
+                    // cv::imshow("origin map", m_global_map);
+                }
+                else if (t_str == "resolution:")
+                {
+                    m_resolution = stod(t_line_str.substr(t_str.length() + 1, t_line_str.length() - t_str.length() - 1));
+                }
+                else if (t_str == "origin:")
+                {
+                    std::string t_origin_str = t_line_str.substr(t_str.length() + 2, t_line_str.length() - t_str.length() - 3);
+                    std::stringstream ss_origin(t_origin_str);
+                    std::string t_origin_value_str;
+                    for (size_t i = 0; i < 3; i++)
+                    {
+                        if (getline(ss_origin, t_origin_value_str, ','))
+                        {
+                            m_map_origin[i] = stod(t_origin_value_str);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    // std::cout<<m_map_path<<std::endl;
+    // std::cout<<m_resolution<<std::endl;
+    // std::cout<<m_map_origin<<std::endl;
+    // cv::waitKey();
+    return true;
+}
+
+void Scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
+{
+    if (scan_msg->ranges.size() <= 0)
+        return;
+    if (m_initialized)
+    {
+        std::cout << "scan callback" << std::endl;
+        m_mutex.lock();
+        m_cloud->clear();
+        double l, angle;
+        // std::cout<<"scan callback1"<<std::endl;
+        for (size_t i = 0; i < scan_msg->ranges.size(); i++)
+        {
+            // std::cout<<"scan callback2"<<std::endl;
+            // std::cout<<"scan callback3"<<scan_msg->intensities[0]<<std::endl;
+            l = scan_msg->ranges[i];
+            angle = scan_msg->angle_min + scan_msg->angle_increment * i;
+            // std::cout<<"scan callback4"<<std::endl;
+            m_cloud->push_back(pcl::PointXYZ(l * cos(angle), l * sin(angle), 0.0));
+            // std::cout<<"scan callback5"<<std::endl;
+        }
+        std::cout << "cloud size: " << m_cloud->size() << std::endl;
+        m_mutex.unlock();
+        Solve();
+    }
+}
+
+void Scan_match::initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg)
+{
+    geometry_msgs::Point position = initial_pose_msg->pose.pose.position;
+    geometry_msgs::Quaternion quat = initial_pose_msg->pose.pose.orientation;
+    std::lock_guard<std::mutex> lck(m_mutex);
+    m_initial_pose.setOrigin(tf::Vector3(position.x, position.y, position.z));
+    m_initial_pose.setRotation(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
+    m_initial_pose_updated = true;
+}

map_scan_match/src/scan_match.h

@@ -0,0 +1,342 @@
+/*
+ * @Description: match scan with grid map
+ * @Author: yct
+ * @Date: 2020-04-03 11:50:01
+ * @LastEditTime: 2020-05-17 23:05:08
+ * @LastEditors: yct
+ */
+#ifndef SCAN_MATCH_HH
+#define SCAN_MATCH_HH
+
+#include <algorithm>
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <atomic>
+#include <mutex>
+#include <cmath>
+#include <chrono>
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+
+class prob_array
+{
+public:
+    prob_array():rows(0),cols(0){
+        m_probability_vector.clear();
+    }
+    void operator=(prob_array& other)
+    {
+        this->rows = other.rows;
+        this->cols = other.cols;
+        this->m_probability_vector.clear();
+        for (size_t i = 0; i < other.m_probability_vector.size(); i++)
+        {
+            this->m_probability_vector.push_back(other.m_probability_vector[i]);
+        }
+    }
+    // 列数
+    int rows;
+    // 行数
+    int cols;
+    // 像素值数组，用于计算加速
+    std::vector<double> m_probability_vector;
+};
+
+class Scan_match
+{
+public:
+
+    Scan_match(std::string filename);
+    ~Scan_match();
+    bool read_map(std::string conf_filename);
+
+    bool Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat global_map);
+    bool Solve();
+
+private:
+    void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
+    void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg);
+
+private:
+    // ros句柄
+    ros::NodeHandle m_nh;
+    ros::Subscriber m_scan_subscriber;
+    ros::Subscriber m_initial_pose_subscriber;
+    ros::Publisher m_cloud_publisher;
+    tf::TransformBroadcaster tf_broadcaster_;
+    std::atomic_bool m_initial_pose_updated;
+    tf::Pose m_initial_pose;
+    std::mutex m_mutex;
+
+    // cv全局地图
+    cv::Mat m_global_map;
+    // 图像分辨率
+    double m_resolution;
+    // 全局地图像素值数组，用于加速
+    prob_array m_probability_array;
+    // 当前待匹配点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
+    // 待求变换
+    Eigen::Vector3d m_trans;
+    // 初始化状态
+    std::atomic_bool m_initialized;
+    // 地图路径
+    std::string m_map_path;
+    // 地图中心点
+    Eigen::Vector3d m_map_origin;
+};
+
+class BicubicInterpolationCost
+{
+public:
+    BicubicInterpolationCost(const prob_array& map, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const double resolution, const Eigen::Vector3d map_origin, const double scale)
+    :m_resolution(resolution),
+    m_scale(scale),
+    m_map_origin(map_origin),
+    m_prob_array(map)
+    {
+        // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
+        // m_prob_array = map;
+        // m_resolution = resolution;
+        // m_scale = scale;
+        // m_map_origin = map_origin;
+        m_points.reserve(cloud->size());
+        for (size_t i = 0; i < cloud->size(); i++)
+        {
+            m_points.emplace_back(Eigen::Vector2d(cloud->at(i).x, cloud->at(i).y));
+        }
+        
+        // std::cout<<"*************************************************************************"<<std::endl;
+        // m_grid.reset(new ceres::Grid2D<double, 1>(
+        //     m_grid_values.data(), 0, m_map.rows*3, 0, m_map.cols*3));
+        // m_interpolator.reset(new ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> >(*m_grid));
+
+        // generate_map_img();
+        // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+        // int duration = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
+        // std::cout << "bicubic init time: " << duration << std::endl;
+    }
+
+    void generate_map_img()
+    {
+        cv::Mat interpolate_map = cv::Mat::ones(m_prob_array.rows, m_prob_array.cols, CV_8UC1);
+        const GridArrayAdapter adapter(m_prob_array);
+        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
+        // interpolate_map *= 50.0;
+        std::cout<<"*************************************************************************"<<std::endl;
+        for (size_t i = 0; i < m_prob_array.rows; i++)
+        {
+            for (size_t j = 0; j < m_prob_array.cols; j++)
+            {
+                double f;
+                t_interpolator.Evaluate(double(i+kPadding), double(j+kPadding), &f);
+                // std::cout<<i<<", "<<j<<"--- map value: "<< f <<std::endl;
+                interpolate_map.at<uchar>(i, j) = static_cast<uchar>(f*255.0);
+            }
+        }
+
+        // 将点云映射到图像中
+        Eigen::Matrix<double, 2, 1> translation(0.0, 0.0);
+        Eigen::Rotation2D<double> rotation(0.0);
+        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<double, 3, 3> transform;
+        transform << rotation_matrix, translation, double(0.), double(0.), double(1.);
+        for (size_t i = 0; i < m_points.size(); ++i)
+        {
+            // Note that this is a 2D point. The third component is a scaling factor.
+            const Eigen::Matrix<double, 3, 1> point(
+                double(m_points[i].x()),
+                double(m_points[i].y()),
+                double(1.)
+                );
+            const Eigen::Matrix<double, 3, 1> world = transform * point;
+            int row = (int)(m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5);
+            int col = (int)(( - m_map_origin.x() + world[0] ) / m_resolution + 0.5);
+            interpolate_map.at<uchar>(row, col) = static_cast<uchar>(0);
+        }
+        
+        // cv::imwrite("./src/map_scan_match/interpolate.jpg", interpolate_map);
+        cv::namedWindow("map", cv::WINDOW_FREERATIO);
+        cv::imshow("map", interpolate_map);
+        cv::waitKey();
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
+        // Eigen::Matrix<T, 2, 1> translation(pose[0] - m_map_origin[0], pose[1] - m_map_origin[1]);
+        Eigen::Rotation2D<T> rotation(pose[2]);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<T, 3, 3> transform;
+        transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
+        
+        const GridArrayAdapter adapter(m_prob_array);
+        ceres::BiCubicInterpolator<GridArrayAdapter> t_interpolator(adapter);
+        // const MapLimits &limits = grid_.limits();
+        // std::cout<<"****************************"<<std::endl;
+        // std::cout<<"+++++++ (row, col): "<<m_map.rows<<" "<<m_map.cols<<std::endl;
+        // char res[2048];
+        // memset(res, 0, 2048);
+        // sprintf(res, "residual values:\n");
+        for (size_t i = 0; i < m_points.size(); ++i)
+        {
+            // Note that this is a 2D point. The third component is a scaling factor.
+            const Eigen::Matrix<T, 3, 1> point(
+                T(m_points[i].x()),
+                T(m_points[i].y()),
+                T(1.)
+                );    
+            const Eigen::Matrix<T, 3, 1> world = transform * point;
+            t_interpolator.Evaluate(
+                (T)m_prob_array.rows - ( - m_map_origin.y() + world[1] ) / m_resolution + 0.5 + T(kPadding),
+                ( - m_map_origin.x() + world[0] ) / m_resolution + 0.5 + T(kPadding),
+                &residual[i]);
+            // t_interpolator.Evaluate(
+            //     (T(m_map.rows/2.0) - world[0] / m_resolution) - 0.5 + T(kPadding),
+            //     (T(m_map.cols/2.0) - world[1] / m_resolution) - 0.5 + T(kPadding),
+            //     &residual[i]);
+            residual[i] *= m_scale;
+            // if (i % 10 == 0)
+            // {
+            //     sprintf(res + strlen(res), "\n");
+            // }
+            // sprintf(res + strlen(res), "%.3f ", residual[i]);
+            // std::cout<<i<<"th point: "<<(-world[1]) / m_resolution - 0.5 + T(m_map.rows) - 60.0<<" "<<(width+world[0]) / m_resolution - 0.5 + T(m_map.cols)+75.0<<std::endl;
+            // std::cout<<i<<"th residual: "<<residual[i]<<std::endl;
+            // if(i%40==0)
+            // {
+            //     // std::cout<<"0 point: "<<m_points[i]<<std::endl;
+            //     // std::cout<<world[0]<<", "<<world[1]<<", "<< m_resolution << "." <<std::endl;
+            //     std::cout<<(T(m_map.rows) - (world[1]) / m_resolution) - 0.5<<",, "<<(world[0]) / m_resolution - 0.5<<",,"<<residual[i]<<std::endl;
+            // }
+        }
+        // std::cout<<res<<std::endl;
+        return true;
+    }
+
+    class GridArrayAdapter
+    {
+    public:
+        enum { DATA_DIMENSION = 1 };
+        explicit GridArrayAdapter(const prob_array& map) : m_grid_map(map) {
+        }
+
+        void GetValue(const int row, const int column, double *const value) const
+        {
+            if (m_grid_map.m_probability_vector.empty() || row < kPadding || row >= NumRows() - kPadding || column < kPadding || column >= NumCols() - kPadding)
+            {
+                *value = 0.9;
+                return;
+            }
+            *value = m_grid_map.m_probability_vector[(row - kPadding) * m_grid_map.cols + column - kPadding];
+            // std::cout<<"rc value"<<*value<<std::endl;
+        }
+
+        int NumRows() const
+        {
+            return m_grid_map.rows + 2 * kPadding;
+        }
+
+        int NumCols() const
+        {
+            return m_grid_map.cols + 2 * kPadding;
+        }
+
+    private:
+        const prob_array& m_grid_map;
+    };
+private:
+    double m_scale;
+    // 边界
+    static constexpr int kPadding = INT_MAX / 4;
+    // 概率地图结构体
+    const prob_array& m_prob_array;
+    // 点坐标数组
+    std::vector<Eigen::Vector2d> m_points;
+    // 地图原始分辨率
+    double m_resolution;
+    // 地图原点
+    Eigen::Vector3d m_map_origin;
+    // // ceres二维地图类
+    // std::unique_ptr<ceres::Grid2D<double, 1> > m_grid;
+    // // ceres插值器
+    // std::unique_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<double, 1> > > m_interpolator;
+};
+
+/**
+ * @description: 平移代价函数类
+ */
+class TranslationCost
+{
+public:
+    TranslationCost(Eigen::Vector2d translation, double translation_weight)
+    {
+        m_translation = translation;
+        m_translation_weight = translation_weight;
+    }
+    ~TranslationCost()
+    {
+        
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        residual[0] = m_translation_weight * (pose[0] - m_translation[0]);
+        residual[1] = m_translation_weight * (pose[1] - m_translation[1]);
+        return true;
+    }
+
+private:
+    // 原始平移
+    Eigen::Vector2d m_translation;
+    double m_translation_weight;
+};
+
+/**
+ * @description: 旋转代价函数类
+ */
+class RotationCost
+{
+public:
+    RotationCost(double angle, double rotation_weight)
+    {
+        m_angle = angle;
+        m_rotation_weight = rotation_weight;
+    }
+    ~RotationCost()
+    {
+
+    }
+
+    template <typename T>
+    bool operator()(const T *const pose, T *residual) const
+    {
+        residual[0] = m_rotation_weight * (pose[2] - m_angle);
+        return true;
+    }
+private:
+    // 原始旋转
+    double m_angle;
+    double m_rotation_weight;
+};
+
+
+#endif // !SCAN_MATCH_HH