Merge branch 'development'

config/adc_common/tf_map_opendrive.yaml

+broadcast:    True
+parent_id:   'map'
+frame_id:    'opendrive'
+period:      100
+x:     0.0
+y:     0.0
+z:     0.0
+roll:  0.0
+pitch: 0.0
+yaw:   0.0

config/adc_rosnav/global_planner/iri_opendrive_params.yaml

 base_global_planner: "iri_opendrive_global_planner/OpendriveGlobalPlanner"
 OpendriveGlobalPlanner:
+  opendrive_frame: "opendrive"
   angle_tol: 0.5
   dist_tol: 0.3
   multi_hyp: True

launch/adc_navigation.launch

@@ -19,6 +19,12 @@
   <arg name="car_y"                       default="0.0"/>
   <arg name="car_yaw"                     default="0.0"/>
 
+  <arg name="calib_tf_map_opendrive"      default="false"/>
+  <arg name="opendrive_x"                 default="0.0"/>
+  <arg name="opendrive_y"                 default="0.0"/>
+  <arg name="opendrive_yaw"               default="0.0"/>
+  <arg name="tf_map_opendrive_file"       default="$(find iri_adc_launch)/config/adc_common/tf_map_opendrive.yaml"/>
+
   <include file="$(find iri_rosnav)/launch/nav.launch">
     <arg name="ns"                      value="$(arg name)"/>
     <arg name="path"                    value="$(find iri_adc_launch)/config/adc_rosnav"/>
@@ -59,4 +65,16 @@
 
   <param name="/$(arg name)/move_base/OpendriveGlobalPlanner/opendrive_file" value="$(arg opendrive_path)/$(arg map_name).xodr" />
 
+  <group if="$(arg calib_tf_map_opendrive)">
+    <include file="$(find iri_dynamic_transform_publisher)/launch/node.launch">
+      <arg name="node_name"       value="$(arg name)_tf_map_opendrive_pub"/>
+      <arg name="params_path"     value="$(arg tf_map_opendrive_file)"/>
+    </include>
+  </group>
+
+  <group unless="$(arg calib_tf_map_opendrive)">
+    <node name="static_tf_map_to_opendrive" pkg="tf" type="static_transform_publisher"
+          args="$(arg opendrive_x) $(arg opendrive_y) 0 $(arg opendrive_yaw) 0 0 map opendrive 100">
+    </node>
+  </group>
 </launch>

rviz/localization_local.rviz

@@ -291,9 +291,9 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: -999999
+          Max Intensity: 0
           Min Color: 0; 0; 0
-          Min Intensity: 999999
+          Min Intensity: 0
           Name: LaserScan
           Position Transformer: XYZ
           Queue Size: 10
@@ -629,6 +629,16 @@ Visualization Manager:
           Unreliable: false
           Use Timestamp: false
           Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: OpendriveMap
+          Topic: /adc_car/move_base/OpendriveGlobalPlanner/opendrive_map
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.10000000149011612
@@ -768,7 +778,7 @@ Window Geometry:
   Height: 872
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   RearImage:
     collapsed: false
   Selection:
@@ -781,4 +791,4 @@ Window Geometry:
     collapsed: false
   Width: 1373
   X: 67
-  Y: 0
+  Y: 27

rviz/mapping.rviz

@@ -492,6 +492,16 @@ Visualization Manager:
           Unreliable: false
           Use Timestamp: false
           Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: false
+          Name: OpendriveMap
+          Topic: /adc_car/move_base/OpendriveGlobalPlanner/opendrive_map
+          Unreliable: false
+          Use Timestamp: false
+          Value: false
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.10000000149011612
@@ -677,7 +687,7 @@ Window Geometry:
   Height: 1025
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   RearImage:
     collapsed: false
   Selection:

rviz/nav.rviz

@@ -629,6 +629,16 @@ Visualization Manager:
           Unreliable: false
           Use Timestamp: false
           Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: OpendriveMap
+          Topic: /adc_car/move_base/OpendriveGlobalPlanner/opendrive_map
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.10000000149011612
@@ -814,7 +824,7 @@ Window Geometry:
   Height: 872
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   RearImage:
     collapsed: false
   Selection:
@@ -827,4 +837,4 @@ Window Geometry:
     collapsed: false
   Width: 1373
   X: 67
-  Y: 0
+  Y: 27