Current progress on getting stage working with rtabmap.

The tf tree looks correct but for some reason slam_mode_goal does not enter the laserscan callback which prevents move_base from publishing a goal. Additionally, the stage sim topics for the cameras are not publishing so rtabmap will not work. But I do see that stage supports cameras (http://wiki.ros.org/stage_ros#Published_topics and rtv/Stage#34) so theoretically this should be able to work.

src/navigation_stack/launch/move_base.launch

@@ -2,33 +2,46 @@
 <!-- Run move_base node (with TebLocalPlannerROS as the local planner) to execute the navigation stack -->
 
 <launch>
-    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-        <!-- All the configuration parameters for local and global costmaps 
-             and planners live in the corresponding parameter files. -->
-        <rosparam file="$(find navigation_stack)/params/costmap_common_params.yaml" 
-                  command="load" ns="global_costmap" /> 
-        <rosparam file="$(find navigation_stack)/params/costmap_common_params.yaml" 
-                  command="load" ns="local_costmap" />
-        <rosparam file="$(find navigation_stack)/params/local_costmap_params.yaml" 
-                  command="load" />
-        <rosparam file="$(find navigation_stack)/params/global_costmap_params.yaml" 
-                  command="load" /> 
-        <rosparam file="$(find navigation_stack)/params/teb_local_planner_params.yaml" 
-                  command="load" />
+  <arg name="velocity_topic"   default="/cmd_vel" />
+  <arg name="lidar_scan_topic" default="/scan" />
+  <arg name="odom_topic"       default="/odom" />
+  <arg name="map_topic"        default="/map" />
 
-        <!-- Use the standard global planner -->
-        <param name="base_global_planner" value="global_planner/GlobalPlanner" />
-        <param name="planner_frequency" value ="1.0" />
-        <param name="planner_patience" value="5.0" />
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <!-- All the configuration parameters for local and global costmaps 
+         and planners live in the corresponding parameter files. -->
+    <rosparam file="$(find navigation_stack)/params/costmap_common_params.yaml" 
+              command="load" ns="global_costmap" /> 
+    <rosparam file="$(find navigation_stack)/params/costmap_common_params.yaml" 
+              command="load" ns="local_costmap" />
+    <rosparam file="$(find navigation_stack)/params/local_costmap_params.yaml" 
+              command="load" />
+    <rosparam file="$(find navigation_stack)/params/global_costmap_params.yaml" 
+              command="load" /> 
+    <rosparam file="$(find navigation_stack)/params/teb_local_planner_params.yaml" 
+              command="load" />
 
-        <!-- Plugin to the base_local_planner that allows for local planning of 
-             car-like robots -->
-        <param name="base_local_planner" 
-                value="teb_local_planner/TebLocalPlannerROS" />
-        <param name="controller_frequency" value="5.0" />
-        <param name="controller_patience" value="15.0" />
+    <!-- Override topic names -->
+    <!--
+    <param name="map_topic"  value="$(arg map_topic)" />
+    <param name="odom_topic" value="$(arg odom_topic)" />
+    <remap from="cmd_vel"    to="$(arg velocity_topic)"/>
+    <param name="laser_scan_sensor" value="{sensor_frame: base_scan, data_type: LaserScan, topic: $(arg lidar_scan_topic), marking: true, clearing: true}" /> 
+    -->
 
-        <!-- Our carlike robot is not able to rotate in place -->
-        <param name="clearing_rotation_allowed" value="false" />
-    </node>
+    <!-- Use the standard global planner -->
+    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
+    <param name="planner_frequency" value ="1.0" />
+    <param name="planner_patience" value="5.0" />
+
+    <!-- Plugin to the base_local_planner that allows for local planning of 
+         car-like robots -->
+    <param name="base_local_planner" 
+            value="teb_local_planner/TebLocalPlannerROS" />
+    <param name="controller_frequency" value="5.0" />
+    <param name="controller_patience" value="15.0" />
+
+    <!-- Our carlike robot is not able to rotate in place -->
+    <param name="clearing_rotation_allowed" value="false" />
+  </node>
 </launch>

src/navigation_stack/params/costmap_common_params.yaml

@@ -23,8 +23,8 @@ obstacle_layer:
   combination_method: 1
 
   observation_sources: laser_scan_sensor
-  laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan,
-                      topic: top/scan, marking: true, clearing: true}
+  laser_scan_sensor: {sensor_frame: base_laser_link, data_type: LaserScan,
+                      topic: /sim_stage/scan, marking: true, clearing: true}
   #point_cloud_sensor: {sensor_frame: velodyne, data_type: PointCloud2, 
   #                     topic: velodyne_points, marking: true, clearing: true}
 
@@ -35,4 +35,4 @@ inflation_layer:
 
 static_layer:
   enabled: true
-  map_topic: "map"
+  map_topic: "/map"

src/navigation_stack/params/teb_local_planner_params.yaml

@@ -4,7 +4,7 @@
 # TODO(Issue 25): Ensure these params work for both simulation and match the 
 #                 kart's physical params.
 TebLocalPlannerROS:
-  odom_topic: "odom"
+  odom_topic: "/sim_stage/odom"
 
   # Trajectory
   teb_autosize: True

src/rtabmap_manager/launch/rtabmap_manager.launch

@@ -1,12 +1,13 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="depth_cam_node"        default="/zed/zed_node" />
+    <arg name="depth_cam_node"    default="/zed/zed_node" />
 
     <!-- RGB-D related topics -->
-    <arg name="odom_topic"              default="$(arg depth_cam_node)/odom" />
-    <arg name="rgb_topic"               default="$(arg depth_cam_node)/rgb/image_rect_color" />
-    <arg name="depth_topic"             default="$(arg depth_cam_node)/depth/depth_registered" />
-    <arg name="camera_info_topic"       default="$(arg depth_cam_node)/rgb/camera_info" />
+    <arg name="odom_topic"        default="$(arg depth_cam_node)/odom" />
+    <arg name="map_topic"         default="$(arg depth_cam_node)/map" />
+    <arg name="rgb_topic"         default="$(arg depth_cam_node)/rgb/image_rect_color" />
+    <arg name="depth_topic"       default="$(arg depth_cam_node)/depth/depth_registered" />
+    <arg name="camera_info_topic" default="$(arg depth_cam_node)/rgb/camera_info" />
 
     <!-- Localization-only mode -->
     <arg name="localization" default="false"/>
@@ -22,7 +23,7 @@
             <remap from="depth/image"       to="$(arg depth_topic)"/>
             <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
 
-            <remap from="grid_map"          to="map" />
+            <remap from="grid_map"          to="$(arg map_topic)" />
             <remap from="odom"              to="$(arg odom_topic)"/>
 
             <!-- localization mode -->
@@ -37,7 +38,7 @@
             <remap from="depth/image"       to="$(arg depth_topic)"/>
             <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
 
-            <remap from="grid_map"          to="map" />
+            <remap from="grid_map"          to="$(arg map_topic)" />
             <remap from="odom"              to="$(arg odom_topic)"/>
         </node>
     </group>

src/rtabmap_manager/params/rtabmap.yaml

@@ -14,7 +14,7 @@ config_path:        "~/.ros/rtabmap.cfg"
 
 frame_id:       "base_link"
 map_frame_id:   "map"
-odom_frame_id:  "odom"                   # odometry from odom msg to have covariance - Remapped by launch file
+odom_frame_id:  "odom"               # Odometry is taken from TF.
 odom_tf_angular_variance:  0.001     # If TF is used to get odometry, this is the default angular variance
 odom_tf_linear_variance:   0.001     # If TF is used to get odometry, this is the default linear variance
 tf_delay:       0.02

src/sim_stage/launch/stage.launch

@@ -6,41 +6,47 @@
   <!-- Force nodes to use /clock as time source as opposed to "wall-clock". -->
   <param name="/use_sim_time" value="true"/>
 
+	<!-- Topics definitions -->
+	<arg name="sim_stage"         default="/sim_stage" />
+	<arg name="odom_topic"        default="$(arg sim_stage)/odom" />
+	<arg name="map_topic"         default="/map" />
+	<arg name="velocity_topic"    default="/cmd_vel" />
+	<arg name="lidar_scan_topic"  default="$(arg sim_stage)/scan" />
+	<arg name="rgb_topic"         default="$(arg sim_stage)/rgb" />
+	<arg name="depth_topic"       default="$(arg sim_stage)/depth" />
+	<arg name="camera_info_topic" default="$(arg sim_stage)/camera_info" />
+
   <!-- Launch the simulation node.
 		This node published a few key topics and transforms, namely:
 		Topics:
 		* odom
 		* base_scan
 
 		Transforms:
-		* odom -> base_footprint
-		* base_footprint -> base_link
+		* odom -> base_link
 		* base_link -> base_laser
 	-->
-	<node pkg="stage_ros" type="stageros" name="sim_stage"
-		args="$(find sim_stage)/sim_files/track.world">
-		<remap from="base_scan" to="top/scan"/>
-	</node>
-
-  <!-- SLAM: LOCALIZATION & MAPPING 
-  This part of the software stack will not match with what we use 'on the ground'
-  as there is no way to integrate rtabmap with stage to perform SLAM.
-  -->
-  <!-- Map Server: Publishes the /map topic with a static map. -->
-	<node name="map_server" pkg="map_server" type="map_server" 
-		args="$(find sim_stage)/sim_files/track.yaml" output="screen">
-		<param name="frame_id" value="map"/>
-	</node>
-
-  <!-- Localization: Publishes the map->odom transform. -->
-	<node pkg="amcl" type="amcl" name="amcl" output="screen">
-		<rosparam file="$(find sim_stage)/params/amcl_params.yaml" command="load" />
-		<remap from="scan" to="top/scan"/>
+  <group ns="sim_stage">
+    <node pkg="stage_ros" type="stageros" name="sim_stage"
+        args="$(find sim_stage)/sim_files/track.world">
+      <remap from="cmd_vel"     to="$(arg velocity_topic)"/>
+      <remap from="base_scan"   to="$(arg lidar_scan_topic)"/>
+      <remap from="odom"        to="$(arg odom_topic)"/>
+      <remap from="image"       to="$(arg rgb_topic)"/>
+      <remap from="depth"       to="$(arg depth_topic)"/>
+      <remap from="camera_info" to="$(arg camera_info_topic)"/>
+    </node>
+  </group>
 
-    <param name="initial_pose_x" value="4"/>
-    <param name="initial_pose_y" value="1.5"/>
-    <param name="initial_pose_a" value="0"/>
-	</node>   
+  <!-- SLAM: LOCALIZATION & MAPPING -->
+  <include file="$(find rtabmap_manager)/launch/rtabmap_manager.launch">
+    <arg name="depth_cam_node"    value="$(arg sim_stage)" />
+    <arg name="map_topic"         value="$(arg map_topic)" />
+    <arg name="odom_topic"        value="$(arg odom_topic)" />
+    <arg name="rgb_topic"         value="$(arg rgb_topic)" />
+    <arg name="depth_topic"       value="$(arg depth_topic)" />
+    <arg name="camera_info_topic" value="$(arg camera_info_topic)" />
+  </include>
 
   <!-- GOAL SETTING -->
   <!-- Run the goal setting algorithm to navigate without a preliminary map (lap 1).
@@ -49,12 +55,17 @@
     on move_base.
 	-->
   <node pkg="slam_mode_goal" type="slam_mode_goal.py" name="slam_mode_goal" 
-        output="screen" />
+        output="screen" args="$(arg lidar_scan_topic)" />
 
   <!-- ROS NAVIGATION STACK -->
   <!-- Configure and run move_base to set up the ROS Navigation Stack.
     More details on inputs/outputs can be found in move_base.launch -->
-  <include file="$(find navigation_stack)/launch/move_base.launch" />
+  <include file="$(find navigation_stack)/launch/move_base.launch">
+    <arg name="velocity_topic"    value="$(arg velocity_topic)" />
+    <arg name="lidar_scan_topic"  value="$(arg lidar_scan_topic)" />
+    <arg name="odom_topic"        value="$(arg odom_topic)" />
+    <arg name="map_topic"         value="$(arg map_topic)" />
+  </include>
 
   <!-- MICROCONTROLLER COMM -->
   <!-- Run the communication script to enable and start communicating with the 

src/slam_mode_goal/slam_mode_goal.py

@@ -24,6 +24,7 @@
 import actionlib
 import numpy as np
 import time
+import sys
 
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from actionlib_msgs.msg import GoalStatus
@@ -261,16 +262,17 @@ def callback(laserscan):
 laserscan messages it should interrupt and perform the operations
 on the vector field.
 """
-def laserscan_listener():
+def laserscan_listener(scan_topic):
   try:
     rospy.init_node("move_base_sequence", anonymous=True)
-    rospy.Subscriber("top/scan", LaserScan, callback, queue_size=1)
+    rospy.Subscriber(scan_topic, LaserScan, callback, queue_size=1)
     rospy.spin()
   except rospy.ROSInterruptException:
       rospy.loginfo("Navigation Complete.")
 
 
 
 if __name__ == "__main__":
-  laserscan_listener()
+  myargv = rospy.myargv(argv=sys.argv)
+  laserscan_listener(myargv[1]) # Pass the scan topic (may want to improve this later)