From 06c1edf42344e32c2ff32df76aa6e31c1f49aad4 Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Mon, 13 Oct 2014 13:00:05 +0900
Subject: [PATCH 1/6] fixed map and costmap misalignement due to wrong frame_id

---
 turtlebot_stdr/launch/turtlebot_in_stdr.launch | 10 +++++++++-
 turtlebot_stdr/nodes/tf_connector.py           | 18 ++++++++++--------
 turtlebot_stdr/robot/turtlebot.yaml            |  4 ++--
 turtlebot_stdr/rviz/robot_navigation.rviz      |  2 +-
 4 files changed, 22 insertions(+), 12 deletions(-)

diff --git a/turtlebot_stdr/launch/turtlebot_in_stdr.launch b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
index cf045db..9be6967 100644
--- a/turtlebot_stdr/launch/turtlebot_in_stdr.launch
+++ b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
@@ -12,7 +12,7 @@
   <arg name="stacks"     default="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons -->
   <arg name="3d_sensor"  default="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro -->
   <arg name="laser_topic" default="robot0/laser_0"/> <!-- default laser topic in stdr for 1 robot -->
-  <arg name="odom_topic" default="robot0/odom"/> <!--env default odom topic in stdr for 1 robot --> 
+  <arg name="odom_topic" default="robot0/odom"/>
   <arg name="odom_frame_id" default="map"/> 
   <arg name="base_frame_id" default="robot0"/>  
   <arg name="global_frame_id" default="world"/> 
@@ -52,6 +52,12 @@
     <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
   </node>
 
+  <!--  ****** Maps *****  -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
+    <param name="frame_id" value="$(arg global_frame_id)"/>
+    <remap from="map" to="new_map"/>
+  </node>
+
   <!--  ************** Navigation  ***************  -->
   <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
    <arg name="odom_topic" value="$(arg odom_topic)"/>
@@ -85,5 +91,7 @@
   <!--  **************** Visualisation ****************  -->
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_stdr)/rviz/robot_navigation.rviz"/>
 
+
+
   
 </launch>
diff --git a/turtlebot_stdr/nodes/tf_connector.py b/turtlebot_stdr/nodes/tf_connector.py
index 5c6d10d..26b831f 100755
--- a/turtlebot_stdr/nodes/tf_connector.py
+++ b/turtlebot_stdr/nodes/tf_connector.py
@@ -2,25 +2,27 @@
 __author__ = 'mehdi tlili'
 import rospy
 from tf2_msgs.msg import TFMessage
+from nav_msgs.msg import OccupancyGrid
 import tf
 
 class Remapper(object):
 
     def __init__(self):
         self.br = tf.TransformBroadcaster()
+        self.map_pub = rospy.Publisher("new_map", OccupancyGrid, queue_size=10)
+        rospy.Subscriber("/tf", TFMessage, self.tf_remapper)
 
     def tf_remapper(self, msg):
 
-            a = 1
-            if msg.transforms[0].header.frame_id == "/robot0":
-                self.br.sendTransform((0, 0, 0),
-                                      tf.transformations.quaternion_from_euler(0, 0, 0),
-                                      rospy.Time.now(),
-                                      "base_footprint",
-                                      "robot0")
+        if msg.transforms[0].header.frame_id == "/robot0":
+            self.br.sendTransform((0, 0, 0),
+                                  tf.transformations.quaternion_from_euler(0, 0, 0),
+                                  rospy.Time.now(),
+                                  "base_footprint",
+                                  "robot0")
+
 
 if __name__ == '__main__':
     rospy.init_node('remapper_nav')
     remapper = Remapper()
-    rospy.Subscriber("/tf", TFMessage, remapper.tf_remapper)
     rospy.spin()
diff --git a/turtlebot_stdr/robot/turtlebot.yaml b/turtlebot_stdr/robot/turtlebot.yaml
index 2585360..40d17a0 100644
--- a/turtlebot_stdr/robot/turtlebot.yaml
+++ b/turtlebot_stdr/robot/turtlebot.yaml
@@ -7,8 +7,8 @@ robot:
             []
     - initial_pose:
         x: 2.0
-        y: 2.0
-        theta: 0
+        y: 6.0
+        theta: 0.0
     - laser:
         laser_specifications:
           max_angle: 0.5
diff --git a/turtlebot_stdr/rviz/robot_navigation.rviz b/turtlebot_stdr/rviz/robot_navigation.rviz
index 9d1f680..354722d 100644
--- a/turtlebot_stdr/rviz/robot_navigation.rviz
+++ b/turtlebot_stdr/rviz/robot_navigation.rviz
@@ -363,7 +363,7 @@ Visualization Manager:
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /map
+      Topic: /new_map
       Value: true
     - Class: rviz/Group
       Displays:

From 1e2603b1a01f86b6271aa706de6ba57c26d17871 Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Mon, 13 Oct 2014 16:34:21 +0900
Subject: [PATCH 2/6] Update tf_connector.py

---
 turtlebot_stdr/nodes/tf_connector.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/turtlebot_stdr/nodes/tf_connector.py b/turtlebot_stdr/nodes/tf_connector.py
index 26b831f..2a9996b 100755
--- a/turtlebot_stdr/nodes/tf_connector.py
+++ b/turtlebot_stdr/nodes/tf_connector.py
@@ -2,7 +2,6 @@
 __author__ = 'mehdi tlili'
 import rospy
 from tf2_msgs.msg import TFMessage
-from nav_msgs.msg import OccupancyGrid
 import tf
 
 class Remapper(object):

From d3778a898b74c322d591b2598c586d2be7d29db9 Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Mon, 13 Oct 2014 16:34:38 +0900
Subject: [PATCH 3/6] Update turtlebot.yaml

---
 turtlebot_stdr/robot/turtlebot.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/turtlebot_stdr/robot/turtlebot.yaml b/turtlebot_stdr/robot/turtlebot.yaml
index 40d17a0..5e81443 100644
--- a/turtlebot_stdr/robot/turtlebot.yaml
+++ b/turtlebot_stdr/robot/turtlebot.yaml
@@ -7,7 +7,7 @@ robot:
             []
     - initial_pose:
         x: 2.0
-        y: 6.0
+        y: 2.0
         theta: 0.0
     - laser:
         laser_specifications:

From 3ce39eab71a151f54efb9311de3c818e185b3a0b Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Tue, 14 Oct 2014 11:13:15 +0900
Subject: [PATCH 4/6] renamed new_map to rviz_map, a map with the right global
 frame id

---
 turtlebot_stdr/launch/turtlebot_in_stdr.launch | 2 +-
 turtlebot_stdr/rviz/robot_navigation.rviz      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/turtlebot_stdr/launch/turtlebot_in_stdr.launch b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
index 9be6967..ae7b4ca 100644
--- a/turtlebot_stdr/launch/turtlebot_in_stdr.launch
+++ b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
@@ -55,7 +55,7 @@
   <!--  ****** Maps *****  -->
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
     <param name="frame_id" value="$(arg global_frame_id)"/>
-    <remap from="map" to="new_map"/>
+    <remap from="map" to="rviz_map"/>
   </node>
 
   <!--  ************** Navigation  ***************  -->
diff --git a/turtlebot_stdr/rviz/robot_navigation.rviz b/turtlebot_stdr/rviz/robot_navigation.rviz
index 354722d..1be1904 100644
--- a/turtlebot_stdr/rviz/robot_navigation.rviz
+++ b/turtlebot_stdr/rviz/robot_navigation.rviz
@@ -363,7 +363,7 @@ Visualization Manager:
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /new_map
+      Topic: /rviz_map
       Value: true
     - Class: rviz/Group
       Displays:

From 33a5070bc3433dacd5b6ae72cd24e0b375e7f1e7 Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Mon, 10 Nov 2014 18:07:13 +0900
Subject: [PATCH 5/6] Use only one map topic and remove unused sensors from
 rviz

---
 .../launch/turtlebot_in_stdr.launch           |  8 ---
 turtlebot_stdr/nodes/tf_connector.py          |  1 -
 turtlebot_stdr/rviz/robot_navigation.rviz     | 61 +------------------
 3 files changed, 2 insertions(+), 68 deletions(-)

diff --git a/turtlebot_stdr/launch/turtlebot_in_stdr.launch b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
index ae7b4ca..8d7f8c4 100644
--- a/turtlebot_stdr/launch/turtlebot_in_stdr.launch
+++ b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
@@ -1,9 +1,7 @@
 <!-- 
   Turtlebot navigation simulation:
   - stdr
-  - map_server
   - move_base
-  - static map
   - amcl
   - rviz view
  -->
@@ -52,12 +50,6 @@
     <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
   </node>
 
-  <!--  ****** Maps *****  -->
-  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
-    <param name="frame_id" value="$(arg global_frame_id)"/>
-    <remap from="map" to="rviz_map"/>
-  </node>
-
   <!--  ************** Navigation  ***************  -->
   <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
    <arg name="odom_topic" value="$(arg odom_topic)"/>
diff --git a/turtlebot_stdr/nodes/tf_connector.py b/turtlebot_stdr/nodes/tf_connector.py
index 2a9996b..099fd71 100755
--- a/turtlebot_stdr/nodes/tf_connector.py
+++ b/turtlebot_stdr/nodes/tf_connector.py
@@ -8,7 +8,6 @@ class Remapper(object):
 
     def __init__(self):
         self.br = tf.TransformBroadcaster()
-        self.map_pub = rospy.Publisher("new_map", OccupancyGrid, queue_size=10)
         rospy.Subscriber("/tf", TFMessage, self.tf_remapper)
 
     def tf_remapper(self, msg):
diff --git a/turtlebot_stdr/rviz/robot_navigation.rviz b/turtlebot_stdr/rviz/robot_navigation.rviz
index 1be1904..ae1177b 100644
--- a/turtlebot_stdr/rviz/robot_navigation.rviz
+++ b/turtlebot_stdr/rviz/robot_navigation.rviz
@@ -288,64 +288,7 @@ Visualization Manager:
       Max Intensity: 285
       Min Color: 0; 0; 0
       Min Intensity: 2
-      Name: LaserScan (ir sensors)
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.05
-      Style: Flat Squares
-      Topic: /ir_scan
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      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: LaserScan (virtual sensor)
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.02
-      Style: Flat Squares
-      Topic: /virtual_sensor_scan
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      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: PointCloud (bumpers)
       Position Transformer: XYZ
       Queue Size: 10
@@ -363,7 +306,7 @@ Visualization Manager:
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /rviz_map
+      Topic: /map
       Value: true
     - Class: rviz/Group
       Displays:

From 27670532551ad7e31be68790180af61a5ce491e5 Mon Sep 17 00:00:00 2001
From: Mehdi Tlili <mehdi.tlili@gmail.com>
Date: Mon, 24 Nov 2014 16:48:02 +0900
Subject: [PATCH 6/6] Use default map topic name map after changing stdr's
 internal map server's topic

---
 turtlebot_stdr/launch/turtlebot_in_stdr.launch | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/turtlebot_stdr/launch/turtlebot_in_stdr.launch b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
index 8d7f8c4..5f71112 100644
--- a/turtlebot_stdr/launch/turtlebot_in_stdr.launch
+++ b/turtlebot_stdr/launch/turtlebot_in_stdr.launch
@@ -3,6 +3,7 @@
   - stdr
   - move_base
   - amcl
+  - map_server
   - rviz view
  -->
 <launch>
@@ -50,6 +51,12 @@
     <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
   </node>
 
+  <!-- ****** Maps ***** --> 	
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"> 	
+   <param name="frame_id" value="$(arg global_frame_id)"/> 	
+  </node> 	
+
+
   <!--  ************** Navigation  ***************  -->
   <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
    <arg name="odom_topic" value="$(arg odom_topic)"/>