changed robot

graph_localization/launch/3d_mcl_ncfm2018.launch

@@ -5,9 +5,9 @@
 <arg name="run_bag" default="false" />
 <!-- Absolute Path to map to be used for localization. The map needs to be built by graph_fuser package -->
 <arg name="map_file_path" default="$(find graph_map)/maps/ndt_map.MAP" />
-<arg name="robot_prefix" default="robot4" />
+<arg name="robot_prefix" default="robot5" />
 <arg name="rviz_enabled" default="false" />
-
+ <arg name="use_sim_time" value="true"/>
 <!-- Include visualization in rviz -->
 <group if="$(arg rviz_enabled)">
 <include file="$(find graph_map)/launch/visualize_graph_fuser.launch" >
@@ -52,8 +52,7 @@
   <!--param name="mcl_tf" value="mcl_pose_est" /-->
 
 
-  <!-- MCL parameters -->
-
+<!-- MCL parameters -->
 <param name="enable_localisation" value="true" />
   <param name="resolution_local_factor" value="1.3" />
   <param name="particle_count" value="500" />
@@ -63,15 +62,15 @@
   <param name="z_filter_height" value="1.0" />
 </node>
 
-<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/occupancy_map.yaml" output="screen"/>
+<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/occupancy_map.yaml" output="screen"/> <!--frame_id map_velodyne-->
 
 <group if="$(arg rviz_enabled)">
   <node name="graph_map_publisher" pkg="graph_map" type="graph_map_publisher" output="screen">
     <param name="map_rate" value="1" />
     <param name="map_file" value="$(arg map_file_path)"  />
-    <param name="map_frame" value="/maps/map_2d_laser" />
+    <param name="map_frame" value="/maps/map_laser2d" />
     <param name="map_parent_frame_id" value="/world" />
-    <param name="map_topic" value="/maps/map_2d_laser" />
+    <param name="map_topic" value="/maps/map_laser2d" />
   </node>
 </group>
 
@@ -80,7 +79,7 @@
   <arg name="file_1" default="2018-05-18-09-55-16.bag" />
   <node pkg="rosbag" type="play" name="player" output="screen" args="--clock -r 1.0 -q $(arg path)$(arg file_1)"/>
 </group>
-<node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100" />
-<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100" />
+<node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100"/>
+<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100"/>
 </launch>
 

graph_localization/rviz/localization_oru_lab.rviz

@@ -14,6 +14,7 @@ Panels:
         - /Odometry4/Shape1
         - /PointCloud21
         - /PointCloud21/Autocompute Value Bounds1
+        - /Map1
       Splitter Ratio: 0.5
     Tree Height: 815
   - Class: rviz/Selection
@@ -64,8 +65,6 @@ Visualization Manager:
         All Enabled: false
         map:
           Value: true
-        map_velodyne:
-          Value: true
         robot4/base_footprint:
           Value: true
         robot4/base_fork:
@@ -133,10 +132,6 @@ Visualization Manager:
       Show Names: true
       Tree:
         world:
-          map:
-            {}
-          map_velodyne:
-            {}
           robot4/odom:
             robot4/base_footprint:
               robot4/base_link:
@@ -337,7 +332,7 @@ Visualization Manager:
       Topic: /ndt_mcl_pose
       Unreliable: false
       Value: true
-    - Alpha: 0.800000012
+    - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
         Max Value: 5.9265604
@@ -348,7 +343,7 @@ Visualization Manager:
       Class: rviz/PointCloud2
       Color: 255; 255; 255
       Color Transformer: AxisColor
-      Decay Time: 10
+      Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
@@ -359,7 +354,7 @@ Visualization Manager:
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 1
+      Size (Pixels): 3
       Size (m): 0.0500000007
       Style: Points
       Topic: /cloud_localized
@@ -411,25 +406,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 18.1015873
+      Distance: 22.5021648
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -3.50539112
-        Y: -25.8522511
-        Z: 5.3598078e-06
+        X: 0.926601827
+        Y: -2.5325017
+        Z: 3.01108912e-06
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: -0.0452038944
+      Pitch: 1.06479573
       Target Frame: world
       Value: XYOrbit (rviz)
-      Yaw: 0.784042537
+      Yaw: 0.494042486
     Saved:
       - Class: rviz/Orbit
         Distance: 39.1665726

graph_localization/src/ndt_mcl_localization.cpp

@@ -235,16 +235,15 @@ class localisation_node {
       Initialize(pose_init_geom, ts);
     }
 
-
     tf::StampedTransform transform;
-    double x, y, yaw;
+    //double x, y, yaw;
 
     tf_listener.waitForTransform(odomTF, baseTF, ts, ros::Duration(0.1));
     try{
       tf_listener.lookupTransform(odomTF, baseTF,/*ros::Time(0)*/ts, transform);
-      yaw = tf::getYaw(transform.getRotation());
+    /*  yaw = tf::getYaw(transform.getRotation());
       x = transform.getOrigin().x();
-      y = transform.getOrigin().y();
+      y = transform.getOrigin().y();*/
     }
     catch(tf::TransformException ex){
       ROS_ERROR("%s", ex.what());