Edge reconfigure integration for the new map type

topological_navigation/scripts/localisation.py

@@ -108,7 +108,7 @@ def __init__(self, name, wtags, use_tmap2):
         self.with_tags = wtags
         self.use_tmap2 = use_tmap2
         if self.use_tmap2:
-            rospy.loginfo("TOPOLOGICAL LOCALISATION IS USING THE NEW MAP TYPE")
+            rospy.logwarn("TOPOLOGICAL LOCALISATION IS USING THE NEW MAP TYPE")
 
         self.subscribers=[]
         self.wp_pub = rospy.Publisher('closest_node', String, latch=True, queue_size=1)
@@ -517,12 +517,10 @@ def point_in_poly_from_tmap2(self,node,pose):
 if __name__ == '__main__':
     rospy.init_node('topological_localisation')
     wtags=True
-    use_tmap2 = False
+    use_tmap2 = rospy.get_param("~use_tmap2", False)  
     argc = len(sys.argv)
     if argc > 1:
         if '-notags' in sys.argv:
             wtags = False
-        if '-use_tmap2' in sys.argv:
-            use_tmap2 = True
     server = TopologicalNavLoc(rospy.get_name(), wtags, use_tmap2)
 ###################################################################################################################    

topological_navigation/scripts/navigation.py

@@ -5,7 +5,6 @@
 import sys
 import json
 
-
 import calendar
 from datetime import datetime
 
@@ -14,7 +13,7 @@
 from strands_navigation_msgs.msg import NavStatistics
 from strands_navigation_msgs.msg import CurrentEdge
 
-from strands_navigation_msgs.srv import ReconfAtEdges
+from geometry_msgs.msg import Pose
 
 from actionlib_msgs.msg import *
 from move_base_msgs.msg import *
@@ -28,6 +27,8 @@
 from topological_navigation.route_search import *
 from topological_navigation.route_search2 import *
 
+from topological_navigation.EdgeReconfigureManager import EdgeReconfigureManager
+
 from execute_policy_server import PolicyExecutionServer
 
 import topological_navigation.msg
@@ -64,8 +65,6 @@
  Class for Topological Navigation
 
 """
-
-
 class TopologicalNavServer(object):
     _feedback = topological_navigation.msg.GotoNodeFeedback()
     _result = topological_navigation.msg.GotoNodeResult()
@@ -92,7 +91,7 @@ def __init__(self, name, mode, use_tmap2):
 
         self.use_tmap2 = use_tmap2
         if self.use_tmap2:
-            rospy.loginfo("TOPOLOGICAL LOCALISATION IS USING THE NEW MAP TYPE")
+            rospy.logwarn("TOPOLOGICAL NAVIGATION IS USING THE NEW MAP TYPE")
 
         move_base_actions = [
             "move_base",
@@ -167,9 +166,7 @@ def __init__(self, name, mode, use_tmap2):
         # Check if using edge recofigure server
         self.edge_reconfigure = rospy.get_param("~reconfigure_edges", False)
         if self.edge_reconfigure:
-            self.current_edge_group = "none"
-            rospy.loginfo("Using edge reconfigure ...")
-            self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
+            self.edgeReconfigureManager = EdgeReconfigureManager()
 
         rospy.loginfo("All Done ...")
         rospy.spin()
@@ -226,7 +223,7 @@ def reconfigure_movebase_params(self, params):
         translation = DYNPARAM_MAPPING[key]
         for k, v in params.iteritems():
             if k in translation:
-                if rospy.has_param(translation[k]):
+                if rospy.has_param(self.move_base_planner + "/" + translation[k]):
                     translated_params[translation[k]] = v
                 else:
                     rospy.logwarn(
@@ -531,38 +528,6 @@ def navigate_tmap2(self, target):
                 self._result.success = False
                 self._as.set_preempted(self._result)
 
-    def edgeReconfigureManager(self, edge_id):
-        """
-        edgeReconfigureManager
-
-        Checks if an edge requires reconfiguration of the
-        """
-
-        #        print self.edge_groups
-
-        edge_group = "none"
-        for group in self.edge_groups:
-            print "Check Edge: ", edge_id, "in ", group
-            if edge_id in self.edge_groups[group]["edges"]:
-                edge_group = group
-                break
-
-        print "current group: ", self.current_edge_group
-        print "edge group: ", edge_group
-
-        if edge_group is not self.current_edge_group:  # and edge_group != 'none':
-            print "RECONFIGURING EDGE: ", edge_id
-            print "TO ", edge_group
-            try:
-                rospy.wait_for_service("reconf_at_edges", timeout=3)
-                reconf_at_edges = rospy.ServiceProxy("reconf_at_edges", ReconfAtEdges)
-                resp1 = reconf_at_edges(edge_id)
-                print resp1.success
-                if resp1.success:  # set current_edge_group only if successful
-                    self.current_edge_group = edge_group
-            except rospy.ServiceException, e:
-                rospy.logerr("Service call failed: %s" % e)
-        print "-------"
 
     """
      Follow Route
@@ -657,7 +622,7 @@ def followRoute(self, route, target):
 
             # If we are using edge reconfigure
             if self.edge_reconfigure:
-                self.edgeReconfigureManager(cedg.edge_id)
+                self.edgeReconfigureManager.srv_reconfigure(cedg.edge_id)
 
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
@@ -755,7 +720,7 @@ def followRoute_tmap2(self, route, target):
         Orig = route.source[0]
         Targ = target
         self._target = Targ
-
+        
         self.init_reconfigure()
 
         rospy.loginfo("%d Nodes on route" % nnodes)
@@ -835,10 +800,6 @@ def followRoute_tmap2(self, route, target):
             rospy.loginfo("Current edge: %s" % current_edge)
             self.cur_edge.publish(current_edge)
 
-            # If we are using edge reconfigure
-            if self.edge_reconfigure:
-                self.edgeReconfigureManager(cedg["edge_id"])
-
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
                 cedg["node"],
@@ -868,16 +829,27 @@ def followRoute_tmap2(self, route, target):
                 route.source[rindex], cedg["node"], self.topol_map, cedg["edge_id"]
             )
             dt_text = self.stat.get_start_time_str()
-            inf = MonitoredNavigationGoal()
-            inf.target_pose.pose.position.x = cnode["pose"]["position"]["x"]
-            inf.target_pose.pose.position.y = cnode["pose"]["position"]["y"]
-            inf.target_pose.pose.position.z = cnode["pose"]["position"]["z"]
-            inf.target_pose.pose.orientation.w = cnode["pose"]["orientation"]["w"]
-            inf.target_pose.pose.orientation.x = cnode["pose"]["orientation"]["x"]
-            inf.target_pose.pose.orientation.y = cnode["pose"]["orientation"]["y"]
-            inf.target_pose.pose.orientation.z = cnode["pose"]["orientation"]["z"]
+            inf = Pose()
+            inf.position.x = cnode["pose"]["position"]["x"]
+            inf.position.y = cnode["pose"]["position"]["y"]
+            inf.position.z = cnode["pose"]["position"]["z"]
+            inf.orientation.w = cnode["pose"]["orientation"]["w"]
+            inf.orientation.x = cnode["pose"]["orientation"]["x"]
+            inf.orientation.y = cnode["pose"]["orientation"]["y"]
+            inf.orientation.z = cnode["pose"]["orientation"]["z"]
+
+            # If we are using edge reconfigure
+            if self.edge_reconfigure:
+                self.edgeReconfigureManager.register_edge(cedg)
+                self.edgeReconfigureManager.initialise()
+                self.edgeReconfigureManager.reconfigure() 
+
             nav_ok, inc = self.monitored_navigation(inf, a)
-            # 5 degrees tolerance   'max_vel_x':0.55,
+
+            if self.edge_reconfigure:
+                self.edgeReconfigureManager._reset()
+                rospy.sleep(rospy.Duration.from_sec(0.3))
+
             params = {"yaw_goal_tolerance": 0.087266, "xy_goal_tolerance": 0.1}
             self.reconfigure_movebase_params(params)
 
@@ -970,7 +942,7 @@ def monitored_navigation(self, gpose, command):
         goal.action_server = command
         goal.target_pose.header.frame_id = "map"
         goal.target_pose.header.stamp = rospy.get_rostime()
-        goal.target_pose.pose = gpose.target_pose.pose
+        goal.target_pose.pose = gpose
 
         self.goal_reached = False
         self.monNavClient.send_goal(goal)
@@ -1012,12 +984,8 @@ def monitored_navigation(self, gpose, command):
 
 
 if __name__ == "__main__":
-    mode = "normal"
-    use_tmap2 = False
-    argc = len(sys.argv)
-    if argc > 1:
-        if "-use_tmap2" in sys.argv:
-            use_tmap2 = True
     rospy.init_node("topological_navigation")
+    mode = "normal"
+    use_tmap2 = rospy.get_param("~use_tmap2", False)
     server = TopologicalNavServer(rospy.get_name(), mode, use_tmap2)
     policy_server = PolicyExecutionServer(use_tmap2)

topological_navigation/src/topological_navigation/EdgeReconfigureManager.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+"""
+Created on Wed Feb 10 15:58:34 2021
+
+@author: Adam Binch ([email protected])
+"""
+#############################################################################################################
+import rospy, dynamic_reconfigure.client
+from strands_navigation_msgs.srv import ReconfAtEdges
+
+
+class EdgeReconfigureManager(object):
+
+
+    def __init__(self):
+
+        rospy.logwarn("Edge Reconfigure Manager: USING EDGE RECONFIGURE ...")
+
+        self.edge = {}
+        self.default_config = {}
+        self.namespaces = []
+
+        self.current_edge_group = "none"
+        self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
+
+
+    def register_edge(self, edge):
+
+        self.edge = edge
+
+        namespaces = []
+        if "config" in edge:
+            namespaces = [param["namespace"] for param in edge["config"]]
+
+        self.namespaces = list(set(namespaces))
+        if self.namespaces:
+            rospy.loginfo("Edge Reconfigure Manager: Processing edge {} ...".format(edge["edge_id"]))
+
+
+    def initialise(self):
+
+        self.default_config = {}                    
+        for namespace in self.namespaces:
+            rospy.loginfo("Edge Reconfigure Manager: Getting default configuration for {}".format(namespace))
+
+            client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
+            try:
+                self.default_config[namespace] = client.get_configuration()
+
+            except rospy.ServiceException as e:
+                rospy.warn("Edge Reconfigure Manager: Caught service exception: {}".format(e))
+
+
+    def reconfigure(self):
+        """
+        If using the new map then edge reconfigure is done using settings in the map.
+        """
+        if "config" in self.edge:
+            for param in self.edge["config"]:
+                if param["namespace"] in self.default_config:
+                    rospy.loginfo("Edge Reconfigure Manager: Setting {} = {}".format("/".join((param["namespace"], param["name"])), param["value"]))                    
+                    self.update(param["namespace"], {param["name"]: param["value"]})
+
+
+    def _reset(self):
+        """
+        Used to reset edge params to their default values when the action has completed (only if using the new map)
+        """
+        for namespace in self.default_config:
+            rospy.loginfo("Edge Reconfigure Manager: Resetting {} to its default configuration".format(namespace))            
+            self.update(namespace, self.default_config[namespace])
+
+
+    def update(self, namespace, params):
+
+        client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
+        try:
+            client.update_configuration(params)
+
+        except rospy.ServiceException as e:
+            rospy.logwarn("Edge Reconfigure Manager: Caught service exception: {}".format(e))
+
+
+    def srv_reconfigure(self, edge_id):
+        """
+        If using the old map then edge reconfigure is done via a service.
+        """
+        edge_group = "none"
+        for group in self.edge_groups:
+            print "Check Edge: ", edge_id, "in ", group
+            if edge_id in self.edge_groups[group]["edges"]:
+                edge_group = group
+                break
+
+        print "current group: ", self.current_edge_group
+        print "edge group: ", edge_group
+
+        if edge_group is not self.current_edge_group:
+            print "RECONFIGURING EDGE: ", edge_id
+            print "TO ", edge_group
+            try:
+                rospy.wait_for_service("reconf_at_edges", timeout=3)
+                reconf_at_edges = rospy.ServiceProxy("reconf_at_edges", ReconfAtEdges)
+                resp1 = reconf_at_edges(edge_id)
+                print resp1.success
+                if resp1.success:
+                    self.current_edge_group = edge_group
+            except rospy.ServiceException, e:
+                rospy.logerr("Service call failed: %s" % e)
+        print "-------"
+############################################################################################################# 

topological_navigation/src/topological_navigation/manager.py

@@ -985,12 +985,11 @@ def tmap_to_tmap2(self):
 
             for edge in node.edges:
 
-                config = {}
-                config["inflation_radius"] = edge.inflation_radius
-                config["top_vel"] = edge.top_vel
-                config["recovery_behaviours_config"] = edge.recovery_behaviours_config
+                config = []
+                #config.append({"namespace":"move_base/DWAPlannerROS", "name":"inflation_radius", "value":edge.inflation_radius})
+                #config.append({"namespace":"move_base/DWAPlannerROS", "name":"top_vel", "value":edge.top_vel})
 
-                manager2.add_edge_to_node(node.name, edge.node, edge.action, edge.edge_id, config)
+                manager2.add_edge_to_node(node.name, edge.node, edge.action, edge.edge_id, config, edge.recovery_behaviours_config)
 
         manager2.update()
 ###################################################################################################################