working on calibration

graph_localization/launch/3d_mcl_ncfm2018.launch

@@ -10,11 +10,11 @@
 <arg name="rviz_enabled" default="false" />
 <arg name="root_id" default="/map_laser2d" />
 
-<!--include file="$(find graph_map)/launch/VelodyneConvertMotionCompensation.launch" >
-  <arg name="robot_id" value="$(arg robot_id)" />
-  <arg name="input_points_topic" value="$(arg robot_id)/sensors/velodyne_packets" />
-  <arg name="output_points_topic" value="$(arg robot_id)/sensors/velodyne_points_unwarped" />
-</include-->
+<include file="$(find graph_map)/launch/VelodyneConvertMotionCompensation.launch" >
+  <arg name="robot_id" value="$(arg  robot_id)" />
+  <arg name="input_points_topic" value="$(arg robot_id)/velodyne_packets" />
+  <arg name="output_points_topic" value="$(arg robot_id)/velodyne_points_unwarped2" />
+</include>
 
 <!-- Include visualization in rviz -->
 <group if="$(arg rviz_enabled)">
@@ -33,8 +33,8 @@
   <param name="dataset" value="ncfm-2018" />
   <!--param name="laser_tf" value="$(arg robot_id)/velodyne_correct" /-->
   <param name="laser_tf" value="$(arg robot_id)/velodyne_link" />
-  <param name="publish_sensor_link" value="false" />
-  <param name="input_cloud_topic" value="$(arg robot_id)/sensors/velodyne_packets" />
+  <param name="publish_sensor_link" value="true" />
+  <param name="input_cloud_topic" value="$(arg robot_id)/velodyne_packets" />
   <param name="pose_estimate_topic" value="$(arg robot_id)/mcl_pose_estimate" />
 
   <!-- Topic of laser scanner -->

graph_localization/launch/visualize.launch

@@ -30,7 +30,10 @@
   </node>
 </group>
 
-
+<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/ncfm.yaml" output="screen">
+  <remap from="/map" to="/maps/map_laser2d"/>
+  <param name="~frame_id" value="map_laser2d"/>
+</node>
 <!--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_ncfm2018.rviz

+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Odometry1/Shape1
+        - /Odometry2/Shape1
+        - /Odometry3/Shape1
+        - /Odometry4/Shape1
+        - /PointCloud21
+        - /PointCloud21/Autocompute Value Bounds1
+        - /Map1
+      Splitter Ratio: 0.5
+    Tree Height: 815
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Orbit1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0.126900002
+        Y: 0.385399997
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        map:
+          Value: true
+        robot4/base_footprint:
+          Value: true
+        robot4/base_fork:
+          Value: true
+        robot4/base_link:
+          Value: true
+        robot4/base_link_ground_truth:
+          Value: true
+        robot4/camera_depth_frame:
+          Value: true
+        robot4/camera_depth_optical_frame:
+          Value: true
+        robot4/camera_link:
+          Value: true
+        robot4/camera_rgb_frame:
+          Value: true
+        robot4/camera_rgb_optical_frame:
+          Value: true
+        robot4/fixed_left_wheel_link:
+          Value: true
+        robot4/fixed_right_wheel_link:
+          Value: true
+        robot4/kinect2_depth_optical_frame:
+          Value: true
+        robot4/kinect2_ir_optical_frame:
+          Value: true
+        robot4/kinect2_link:
+          Value: true
+        robot4/kinect2_rgb_optical_frame:
+          Value: true
+        robot4/kinect_link:
+          Value: true
+        robot4/laser2d_floor:
+          Value: true
+        robot4/laser2d_floor_link:
+          Value: true
+        robot4/laser2d_top:
+          Value: true
+        robot4/laser2d_top_link:
+          Value: true
+        robot4/left_fork:
+          Value: true
+        robot4/odom:
+          Value: true
+        robot4/right_fork:
+          Value: true
+        robot4/robot4/velodyne:
+          Value: true
+        robot4/robot4/velodyne_base_link:
+          Value: true
+        robot4/sd_wheel_link:
+          Value: true
+        robot4/steer_link:
+          Value: true
+        submap_node0:
+          Value: true
+        velodyne:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          robot4/odom:
+            robot4/base_footprint:
+              robot4/base_link:
+                robot4/base_fork:
+                  robot4/left_fork:
+                    {}
+                  robot4/right_fork:
+                    {}
+                robot4/fixed_left_wheel_link:
+                  {}
+                robot4/fixed_right_wheel_link:
+                  {}
+                robot4/kinect2_link:
+                  robot4/kinect2_depth_optical_frame:
+                    {}
+                  robot4/kinect2_rgb_optical_frame:
+                    robot4/kinect2_ir_optical_frame:
+                      {}
+                robot4/kinect_link:
+                  {}
+                robot4/laser2d_floor:
+                  {}
+                robot4/laser2d_floor_link:
+                  {}
+                robot4/laser2d_top:
+                  {}
+                robot4/laser2d_top_link:
+                  {}
+                robot4/robot4/velodyne_base_link:
+                  robot4/robot4/velodyne:
+                    {}
+                robot4/steer_link:
+                  robot4/sd_wheel_link:
+                    {}
+              velodyne:
+                {}
+          submap_node0:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /graphInfo
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 10000
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 255; 0
+        Head Length: 0.100000001
+        Head Radius: 0.100000001
+        Shaft Length: 0.100000001
+        Shaft Radius: 0.00999999978
+        Value: Arrow
+      Topic: /robot1/kmo_navserver/state
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 10000
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.100000001
+        Head Radius: 0.100000001
+        Shaft Length: 0.100000001
+        Shaft Radius: 0.00999999978
+        Value: Arrow
+      Topic: /ndt_mcl_pose
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 0; 255; 255
+        Head Length: 0.100000001
+        Head Radius: 0.100000001
+        Shaft Length: 0.100000001
+        Shaft Radius: 0.00999999978
+        Value: Arrow
+      Topic: /robot1/kmo_navserver/odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.300000012
+      Axes Radius: 0.00999999978
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: PoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Arrow (Flat)
+      Topic: /ndt_mcl_localization_graph/pose_particles
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: false
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.100000001
+        Head Radius: 0.100000001
+        Shaft Length: 0.100000001
+        Shaft Radius: 0.00999999978
+        Value: Arrow
+      Topic: /ndt_mcl_pose
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 5.9265604
+        Min Value: -1.14259744
+        Value: false
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0500000007
+      Style: Points
+      Topic: /cloud_localized
+      Unreliable: false
+      Use Fixed Frame: false
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.400000006
+      Class: ndt_rviz/NDT
+      Color: 204; 51; 204
+      Enabled: true
+      History Length: 1
+      Name: NDT
+      Topic: /graph_map_publisher/NDTOmMap
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 130; 130; 130
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 70
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 22.5021648
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        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: 1.06479573
+      Target Frame: world
+      Value: XYOrbit (rviz)
+      Yaw: 0.494042486
+    Saved:
+      - Class: rviz/Orbit
+        Distance: 39.1665726
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.0599999987
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: false
+        Focal Shape Size: 0.0500000007
+        Invert Z Axis: false
+        Name: Orbit
+        Near Clip Distance: 0.00999999978
+        Pitch: 0.785398185
+        Target Frame: <Fixed Frame>
+        Value: Orbit (rviz)
+        Yaw: 0.785398185
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1028
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001aa000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000000a005600690065007700730100000028000003be000000ad00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000064f000000b0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000047a000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1855
+  X: 65
+  Y: 27

graph_localization/rviz/localization_oru_lab.rviz

@@ -12,7 +12,9 @@ Panels:
         - /Odometry2/Shape1
         - /Odometry3
         - /Odometry3/Shape1
+        - /Map1
         - /PointCloud21
+        - /PointCloud21/Autocompute Value Bounds1
       Splitter Ratio: 0.5
     Tree Height: 815
   - Class: rviz/Selection
@@ -315,34 +317,34 @@ Visualization Manager:
       Topic: /graph_map_publisher/NDTOmMap
       Unreliable: false
       Value: true
-    - Alpha: 0.699999988
+    - Alpha: 0.200000003
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /map
+      Topic: /maps/map_laser2d
       Unreliable: false
       Use Timestamp: false
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
+        Max Value: 6.03039742
+        Min Value: -0.172616005
+        Value: false
       Axis: Z
-      Channel Name: intensity
+      Channel Name: x
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color Transformer: AxisColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
+      Max Intensity: 9.8743124
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: -18.3716335
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -379,7 +381,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: 1.50500131
+      Angle: 1.48000133
       Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
@@ -389,11 +391,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 32.0815163
+      Scale: 54.7555885
       Target Frame: world
       Value: TopDownOrtho (rviz)
-      X: -3.27640963
-      Y: -6.37991905
+      X: -5.34080029
+      Y: -3.81768727
     Saved:
       - Class: rviz/Orbit
         Distance: 39.1665726

graph_map/launch/3d_fuser_ncfm2018.launch

 <?xml version="1.0" encoding="utf-8"?>
 <launch>
   <arg name="run_bag" default="false"/>
-  <arg name="namespace_prefix" default="robot4"/>
+  <arg name="namespace_prefix" default="robot5"/>
   <arg name="output_pointcloud_topic_name" default="/fused_cloud"/>
   <arg name="rviz_enabled" default="false"/>
 
@@ -113,8 +113,8 @@
 
   <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
        <remap from="cloud_in" to="$(arg output_pointcloud_topic_name)"/>
-       <param name="frame_id" value="world"/>
-       <param name="resolution" value="0.15"/>
+       <param name="frame_id" value="map_laser2d"/>
+       <param name="resolution" value="0.1"/>
        <param name="sensor_model/max_range" value="10.0"/>
        <param name="pointcloud_max_z" value="5"/>
        <param name="occupancy_min_z" value="0.1"/>

graph_map/launch/VelodyneConvertMotionCompensation.launch

@@ -6,6 +6,7 @@
 <arg name="output_points_topic" default="/$(arg robot_id)/sensors/velodyne_points_unwarped" />
 
 <!-- Run monte carlo localization -->
+<!--include file="$(find ndt_calibration)/launch/visualize.launch" /-->
 <node name="velodyne_converter" pkg="graph_map" type="velodyne_converter" output="screen">
 
   <!-- select one of the following types-->
@@ -15,7 +16,7 @@
   <!-- Topic of laser scanner -->
   <param name="input_points_topic" value="$(arg input_points_topic)" />
   <param name="output_points_topic" value="$(arg output_points_topic)" />
-  <param name="output_points_link" value="/$(arg robot_id)/velodyne_link" />
+  <param name="output_points_link" value="$(arg robot_id)/velodyne_link" />
   <param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
   <param name="sensor_offset_topic" value="/sensor_pose" />
 
@@ -29,5 +30,17 @@
   <param name="sensor_pose_t" value="0" />
  </node>
 
+
+<!--node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
+     <remap from="cloud_in" to="$(arg output_points_topic)"/>
+     <param name="frame_id" value="map_laser2d"/>
+     <param name="resolution" value="0.10"/>
+     <param name="sensor_model/max_range" value="10.0"/>