Merge pull request LCAS#12 from heuristicus/multirobot-goto-node

Fix go_to_node not being available with namespaced topological_navigation

topological_navigation/launch/topo_nav_global.launch

@@ -1,9 +1,13 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <launch>
   <arg name="map"/>
+  <!-- This is a setting for the map visualisation. If true, it will provide the green arrows on waypoints to use for
+       navigation from rviz. If false, those will not be available. Setting this to false is required when running
+       multiple instances topological_navigation, otherwise the map visualisation will probably not work -->
+  <arg name="enable_goto_node" default="true"/>
   <node pkg="topological_navigation" type="map_manager.py" name="topological_map_manager" args="$(arg map)" respawn="true"/>
   <node pkg="fremenserver" type="fremenserver" name="fremenserver" respawn="true"/>
-  <node pkg="topological_navigation" type="visualise_map.py" name="visualise_map" args="$(arg map)" respawn="true"/>
+  <node pkg="topological_navigation" type="visualise_map.py" name="visualise_map" args="$(eval map + ' -edit' if not enable_goto_node else '')" output="screen" respawn="true"/>
   <node pkg="topological_navigation" type="travel_time_estimator.py" name="travel_time_estimator"/>
   <node pkg="topological_navigation" type="topological_prediction.py" name="topological_prediction" output="screen" respawn="true"/>
 </launch>

topological_navigation/launch/topo_nav_local.launch

@@ -4,6 +4,7 @@
   <arg name="mon_nav_config_file"  default="" />
   <arg name="topological_navigation_retries" default="3"/>
   <arg name="execute_policy_retries" default="3"/>
+  <arg name="run_goto_node" default="false"/>
   <!-- The planner being used by move_base. STRANDS systems tend to use DWAPlannerROSm Jackal and HSR TrajectoryPlannerROS.  -->
   <arg name="move_base_planner" default="DWAPlannerROS"/>
   <group ns="$(arg robot_id)"> 
@@ -19,5 +20,9 @@
         <param name="retries" type="int" value="$(arg execute_policy_retries)"/>
         <param name="move_base_planner" type="string" value="$(arg move_base_planner)"/>
     </node>
+
+    <node if="$(arg run_goto_node)" pkg="topological_navigation" name="go_to_node" type="go_to_node.py" output="screen">
+        <remap from="/$(arg robot_id)/topological_map" to="/topological_map"/>
+    </node>
   </group>
 </launch>

topological_navigation/scripts/go_to_node.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+import rospy
+from topological_navigation.goto import *
+
+if __name__ == '__main__':
+    rospy.init_node("go_to_node")
+
+    goto_cont = go_to_controllers()
+
+    rospy.spin()

topological_navigation/scripts/navigation.py

@@ -156,7 +156,7 @@ def __init__(self, name, mode) :
         rospy.spin()
 
     def init_reconfigure(self):
-        self.move_base_planner = rospy.get_param('~move_base_planner', 'move_base/DWAPlannerROS')
+        self.move_base_planner = "move_base/" + rospy.get_param('~move_base_planner', 'DWAPlannerROS')
         #Creating Reconfigure Client
         rospy.loginfo("Creating Reconfigure Client")
         self.rcnfclient = dynamic_reconfigure.client.Client(self.move_base_planner)