From ad4c4c121c64050f85160692abdf2119fb174fb1 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Tue, 2 Feb 2021 11:33:29 +0000
Subject: [PATCH 01/17] Some fixes: The monitored navigation function in
 `navigation.py` expects a geom msgs Pose object rather than a monitored nav
 goal object (stops nav breaking when using the old map format). Navigation
 now reconfigures move base tolerances according to the values specified in
 the tmap.

---
 .../scripts/localisation.py                   |  2 +-
 topological_navigation/scripts/navigation.py  | 24 ++++++++++---------
 2 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/topological_navigation/scripts/localisation.py b/topological_navigation/scripts/localisation.py
index b6f30a54..ee67b775 100755
--- a/topological_navigation/scripts/localisation.py
+++ b/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)
diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index da895cce..666352d3 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -14,6 +14,8 @@
 from strands_navigation_msgs.msg import NavStatistics
 from strands_navigation_msgs.msg import CurrentEdge
 
+from geometry_msgs.msg import Pose
+
 from strands_navigation_msgs.srv import ReconfAtEdges
 
 from actionlib_msgs.msg import *
@@ -92,7 +94,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",
@@ -226,7 +228,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(
@@ -868,14 +870,14 @@ 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"]
             nav_ok, inc = self.monitored_navigation(inf, a)
             # 5 degrees tolerance   'max_vel_x':0.55,
             params = {"yaw_goal_tolerance": 0.087266, "xy_goal_tolerance": 0.1}
@@ -970,7 +972,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)

From 1b709def15f05e2b2beda8bbdda43bafaf545906 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Tue, 2 Feb 2021 16:56:33 +0000
Subject: [PATCH 02/17] The arg `use_tmap2` (used by localisation and
 navigation) is now a rosparam

---
 topological_navigation/scripts/localisation.py | 4 +---
 topological_navigation/scripts/navigation.py   | 8 ++------
 2 files changed, 3 insertions(+), 9 deletions(-)

diff --git a/topological_navigation/scripts/localisation.py b/topological_navigation/scripts/localisation.py
index ee67b775..5217f375 100755
--- a/topological_navigation/scripts/localisation.py
+++ b/topological_navigation/scripts/localisation.py
@@ -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)
 ###################################################################################################################    
\ No newline at end of file
diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index 666352d3..8b8fad80 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -1014,12 +1014,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)

From 5e0bac2509a3706393e5bc5bbdc43b07bf8bc960 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Thu, 4 Feb 2021 09:00:43 +0000
Subject: [PATCH 03/17] map manager 2 coverts new format maps (broadcast on the
 topic `/topological_map_2`) to the old format (broadcast on the topic
 `/topological_map`). This allows nodes/actions that rely on the old map
 format to function whilst using/testing features from the new map.

---
 .../src/topological_navigation/manager2.py    | 69 ++++++++++++++++++-
 1 file changed, 68 insertions(+), 1 deletion(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index ceeb09b1..5e749993 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -11,6 +11,7 @@
 import re, uuid, copy, os
 
 import strands_navigation_msgs.srv
+from strands_navigation_msgs.msg import *
 import topological_navigation_msgs.srv
 import std_msgs.msg
 
@@ -57,7 +58,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.filename = filename
         if not self.filename:
             self.filename = os.path.join(self.cache_dir, self.name + ".yaml")
-            
+        
         self.load = load            
         if self.load:
             self.load_map(self.filename)
@@ -78,6 +79,11 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.broadcaster = tf2_ros.StaticTransformBroadcaster()
         self.broadcast_transform()
         
+        self.points_pub = rospy.Publisher('/topological_map', strands_navigation_msgs.msg.TopologicalMap, latch=True, queue_size=1)
+        if self.loaded:
+            self.tmap2_to_tmap()
+            self.points_pub.publish(self.points)
+        
         # Services that retrieve information from the map
         self.get_map_srv=rospy.Service('/topological_map_manager2/get_topological_map', Trigger, self.get_topological_map_cb)
         self.switch_map_srv=rospy.Service('/topological_map_manager2/switch_topological_map', topological_navigation_msgs.srv.WriteTopologicalMap, self.switch_topological_map_cb)
@@ -107,6 +113,7 @@ def get_time(self):
 
 
     def load_map(self, filename):
+        self.loaded = False
         
         try:
             with open(filename, "r") as f:
@@ -129,6 +136,8 @@ def load_map(self, filename):
             rospy.logerr(e1.format(map_type, dict))
             self.tmap2 = {}
             return
+        
+        self.loaded = True
             
         self.name = self.tmap2["name"]
         self.metric_map = self.tmap2["metric_map"]
@@ -164,6 +173,10 @@ def update(self, update_time=True):
         self.names = self.create_list_of_nodes()
         self.map_check()
         
+        if self.loaded:
+            self.tmap2_to_tmap()
+            self.points_pub.publish(self.points)
+        
         
     def broadcast_transform(self):
         
@@ -876,4 +889,58 @@ def map_check(self):
             if destination not in names:
                 rospy.logwarn("edge with origin '{}' has a destination '{}' that does not exist".format(origin, destination))
                 self.map_ok = False
+                
+                
+    def tmap2_to_tmap(self):
+        
+        points = strands_navigation_msgs.msg.TopologicalMap()
+        
+        point_set = self.tmap2["pointset"]
+        points.name = point_set
+        points.pointset = point_set
+        points.map = self.tmap2["metric_map"]
+        
+        for node in self.tmap2["nodes"]:
+            msg = strands_navigation_msgs.msg.TopologicalNode()
+            msg.name = node["node"]["name"]
+            msg.map = self.tmap2["metric_map"]
+            msg.pointset = point_set
+            
+            msg.pose.position.x = node["node"]["pose"]["position"]["x"]
+            msg.pose.position.y = node["node"]["pose"]["position"]["y"]
+            msg.pose.position.z = node["node"]["pose"]["position"]["z"]
+            
+            msg.pose.orientation.x = node["node"]["pose"]["orientation"]["x"]
+            msg.pose.orientation.y = node["node"]["pose"]["orientation"]["y"]
+            msg.pose.orientation.z = node["node"]["pose"]["orientation"]["z"]
+            msg.pose.orientation.w = node["node"]["pose"]["orientation"]["w"]
+            
+            msg.yaw_goal_tolerance = node["node"]["properties"]["yaw_goal_tolerance"]
+            msg.xy_goal_tolerance = node["node"]["properties"]["xy_goal_tolerance"]
+            
+            msgs_verts = []
+            for v in node["node"]["verts"]:
+                msg_v = strands_navigation_msgs.msg.Vertex()
+                msg_v.x = v["x"]
+                msg_v.y = v["y"]
+                msgs_verts.append(msg_v)
+            msg.verts = msgs_verts
+            
+            msgs_edges = []
+            for e in node["node"]["edges"]:
+                msg_e = strands_navigation_msgs.msg.Edge()
+                msg_e.edge_id = e["edge_id"]
+                msg_e.node = e["node"]
+                msg_e.action = e["action"]
+                msg_e.top_vel = e["config"]["top_vel"]
+                msg_e.map_2d = self.tmap2["metric_map"]
+                msg_e.inflation_radius = e["config"]["inflation_radius"]
+                msg_e.recovery_behaviours_config = e["config"]["recovery_behaviours_config"]
+                msgs_edges.append(msg_e)
+            msg.edges = msgs_edges
+            
+            msg.localise_by_topic = node["node"]["localise_by_topic"]
+            points.nodes.append(msg)
+        
+        self.points = points
 #########################################################################################################
\ No newline at end of file

From 5b3989f83774de748e5f841adf846dc179f96435 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Thu, 4 Feb 2021 09:26:29 +0000
Subject: [PATCH 04/17] `convert_to_legacy` rosparam sets whther the new to old
 format map conversion happens or not

---
 .../src/topological_navigation/manager2.py                   | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index 5e749993..bc075927 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -79,8 +79,9 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.broadcaster = tf2_ros.StaticTransformBroadcaster()
         self.broadcast_transform()
         
+        self.convert_to_legacy = rospy.get_param("~convert_to_legacy", False)
         self.points_pub = rospy.Publisher('/topological_map', strands_navigation_msgs.msg.TopologicalMap, latch=True, queue_size=1)
-        if self.loaded:
+        if self.loaded and self.convert_to_legacy:
             self.tmap2_to_tmap()
             self.points_pub.publish(self.points)
         
@@ -173,7 +174,7 @@ def update(self, update_time=True):
         self.names = self.create_list_of_nodes()
         self.map_check()
         
-        if self.loaded:
+        if self.loaded and self.convert_to_legacy:
             self.tmap2_to_tmap()
             self.points_pub.publish(self.points)
         

From 080a42134878dacfb66b64c12c8d5941a169fab1 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Thu, 4 Feb 2021 09:56:52 +0000
Subject: [PATCH 05/17] Function that does the new to old conversion catches
 exceptions

---
 .../src/topological_navigation/manager2.py    | 94 ++++++++++---------
 1 file changed, 49 insertions(+), 45 deletions(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index bc075927..dc9cee6e 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -896,52 +896,56 @@ def tmap2_to_tmap(self):
         
         points = strands_navigation_msgs.msg.TopologicalMap()
         
-        point_set = self.tmap2["pointset"]
-        points.name = point_set
-        points.pointset = point_set
-        points.map = self.tmap2["metric_map"]
-        
-        for node in self.tmap2["nodes"]:
-            msg = strands_navigation_msgs.msg.TopologicalNode()
-            msg.name = node["node"]["name"]
-            msg.map = self.tmap2["metric_map"]
-            msg.pointset = point_set
-            
-            msg.pose.position.x = node["node"]["pose"]["position"]["x"]
-            msg.pose.position.y = node["node"]["pose"]["position"]["y"]
-            msg.pose.position.z = node["node"]["pose"]["position"]["z"]
-            
-            msg.pose.orientation.x = node["node"]["pose"]["orientation"]["x"]
-            msg.pose.orientation.y = node["node"]["pose"]["orientation"]["y"]
-            msg.pose.orientation.z = node["node"]["pose"]["orientation"]["z"]
-            msg.pose.orientation.w = node["node"]["pose"]["orientation"]["w"]
-            
-            msg.yaw_goal_tolerance = node["node"]["properties"]["yaw_goal_tolerance"]
-            msg.xy_goal_tolerance = node["node"]["properties"]["xy_goal_tolerance"]
-            
-            msgs_verts = []
-            for v in node["node"]["verts"]:
-                msg_v = strands_navigation_msgs.msg.Vertex()
-                msg_v.x = v["x"]
-                msg_v.y = v["y"]
-                msgs_verts.append(msg_v)
-            msg.verts = msgs_verts
-            
-            msgs_edges = []
-            for e in node["node"]["edges"]:
-                msg_e = strands_navigation_msgs.msg.Edge()
-                msg_e.edge_id = e["edge_id"]
-                msg_e.node = e["node"]
-                msg_e.action = e["action"]
-                msg_e.top_vel = e["config"]["top_vel"]
-                msg_e.map_2d = self.tmap2["metric_map"]
-                msg_e.inflation_radius = e["config"]["inflation_radius"]
-                msg_e.recovery_behaviours_config = e["config"]["recovery_behaviours_config"]
-                msgs_edges.append(msg_e)
-            msg.edges = msgs_edges
+        try:
+            point_set = self.pointset
+            points.name = point_set
+            points.pointset = point_set
+            points.map = self.metric_map
             
-            msg.localise_by_topic = node["node"]["localise_by_topic"]
-            points.nodes.append(msg)
+            for node in self.tmap2["nodes"]:
+                msg = strands_navigation_msgs.msg.TopologicalNode()
+                msg.name = node["node"]["name"]
+                msg.map = self.metric_map
+                msg.pointset = point_set
+                
+                msg.pose.position.x = node["node"]["pose"]["position"]["x"]
+                msg.pose.position.y = node["node"]["pose"]["position"]["y"]
+                msg.pose.position.z = node["node"]["pose"]["position"]["z"]
+                
+                msg.pose.orientation.x = node["node"]["pose"]["orientation"]["x"]
+                msg.pose.orientation.y = node["node"]["pose"]["orientation"]["y"]
+                msg.pose.orientation.z = node["node"]["pose"]["orientation"]["z"]
+                msg.pose.orientation.w = node["node"]["pose"]["orientation"]["w"]
+                
+                msg.yaw_goal_tolerance = node["node"]["properties"]["yaw_goal_tolerance"]
+                msg.xy_goal_tolerance = node["node"]["properties"]["xy_goal_tolerance"]
+                
+                msgs_verts = []
+                for v in node["node"]["verts"]:
+                    msg_v = strands_navigation_msgs.msg.Vertex()
+                    msg_v.x = v["x"]
+                    msg_v.y = v["y"]
+                    msgs_verts.append(msg_v)
+                msg.verts = msgs_verts
+                
+                msgs_edges = []
+                for e in node["node"]["edges"]:
+                    msg_e = strands_navigation_msgs.msg.Edge()
+                    msg_e.edge_id = e["edge_id"]
+                    msg_e.node = e["node"]
+                    msg_e.action = e["action"]
+                    msg_e.top_vel = e["config"]["top_vel"]
+                    msg_e.map_2d = self.metric_map
+                    msg_e.inflation_radius = e["config"]["inflation_radius"]
+                    msg_e.recovery_behaviours_config = e["config"]["recovery_behaviours_config"]
+                    msgs_edges.append(msg_e)
+                msg.edges = msgs_edges
+                
+                msg.localise_by_topic = node["node"]["localise_by_topic"]
+                points.nodes.append(msg)
+        
+        except Exception as e:
+            rospy.logerr("cannot convert map to the old format: {}".format(e))
         
         self.points = points
 #########################################################################################################
\ No newline at end of file

From be0280ff90f0eca31654c376d940b8da6c95ef58 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Thu, 4 Feb 2021 10:53:22 +0000
Subject: [PATCH 06/17] fix

---
 topological_navigation/src/topological_navigation/manager2.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index dc9cee6e..2af33bff 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -59,6 +59,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         if not self.filename:
             self.filename = os.path.join(self.cache_dir, self.name + ".yaml")
         
+        self.loaded = False
         self.load = load            
         if self.load:
             self.load_map(self.filename)
@@ -79,7 +80,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.broadcaster = tf2_ros.StaticTransformBroadcaster()
         self.broadcast_transform()
         
-        self.convert_to_legacy = rospy.get_param("~convert_to_legacy", False)
+        self.convert_to_legacy = rospy.get_param("~convert_to_legacy", True)
         self.points_pub = rospy.Publisher('/topological_map', strands_navigation_msgs.msg.TopologicalMap, latch=True, queue_size=1)
         if self.loaded and self.convert_to_legacy:
             self.tmap2_to_tmap()

From 9469801c32783ceccca922f5d3439150d1c3b484 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Thu, 4 Feb 2021 16:43:37 +0000
Subject: [PATCH 07/17] New service for adding/updating edge reconfigure
 parameters.

---
 .../src/topological_navigation/manager2.py    | 39 ++++++++++++++++++-
 topological_navigation_msgs/CMakeLists.txt    |  1 +
 .../srv/UpdateEdgeReconf.srv                  |  7 ++++
 3 files changed, 46 insertions(+), 1 deletion(-)
 create mode 100644 topological_navigation_msgs/srv/UpdateEdgeReconf.srv

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index 2af33bff..399c1e3b 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -108,6 +108,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.add_tag_srv=rospy.Service('/topological_map_manager2/add_tag_to_node', strands_navigation_msgs.srv.AddTag, self.add_tag_cb)
         self.rm_tag_srv=rospy.Service('/topological_map_manager2/rm_tag_from_node', strands_navigation_msgs.srv.AddTag, self.rm_tag_cb)        
         self.update_edge_srv=rospy.Service('/topological_map_manager2/update_edge', strands_navigation_msgs.srv.UpdateEdge, self.update_edge_cb)
+        self.update_edge_reconf_srv=rospy.Service('/topological_map_manager2/update_edge_reconf', topological_navigation_msgs.srv.UpdateEdgeReconf, self.update_edge_reconf_cb)
         
         
     def get_time(self):
@@ -837,6 +838,41 @@ def update_edge(self, edge_id, action, top_vel):
             return False, "no edge found or multiple edges found"
         
         
+    def update_edge_reconf_cb(self, req):
+        """
+        Update edge reconfigure parameters.
+        """
+        return self.update_edge_reconf(req.edge_id, req.param_name, req.param_value, req.value_is_string)
+    
+    
+    def update_edge_reconf(self, edge_id, param_name, param_value, value_is_string):
+        
+        if not value_is_string:
+            param_value = eval(param_value)
+        
+        node_name = edge_id.split('_')[0]
+        num_available, index = self.get_instances_of_node(node_name)
+        
+        if num_available == 1:
+            the_node = copy.deepcopy(self.tmap2["nodes"][index])
+            for edge in the_node["node"]["edges"]:
+                if edge["edge_id"] == edge_id:
+                    if "config" in edge:
+                        edge["config"][param_name] = param_value
+                    else:
+                        edge["config"] = {}
+                        edge["config"][param_name] = param_value
+            
+            self.tmap2["nodes"][index] = the_node
+            self.update()
+            self.write_topological_map(self.filename)
+            
+            return True, "edge action is {} and edge config is {}".format(edge["action"], edge["config"])
+        else:
+            rospy.logerr("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name))
+            return False, "no edge found or multiple edges found"
+        
+        
     def get_instances_of_node(self, node_name):
         
         num_available = 0
@@ -946,7 +982,8 @@ def tmap2_to_tmap(self):
                 points.nodes.append(msg)
         
         except Exception as e:
-            rospy.logerr("cannot convert map to the old format: {}".format(e))
+            rospy.logerr("Cannot convert map to the old format. The conversion requires all fields of the old format map to be set.")
+            points = strands_navigation_msgs.msg.TopologicalMap()
         
         self.points = points
 #########################################################################################################
\ No newline at end of file
diff --git a/topological_navigation_msgs/CMakeLists.txt b/topological_navigation_msgs/CMakeLists.txt
index 0fc9fbcf..11163103 100644
--- a/topological_navigation_msgs/CMakeLists.txt
+++ b/topological_navigation_msgs/CMakeLists.txt
@@ -29,6 +29,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
 add_service_files(
   FILES
   WriteTopologicalMap.srv
+  UpdateEdgeReconf.srv
 )
 
 
diff --git a/topological_navigation_msgs/srv/UpdateEdgeReconf.srv b/topological_navigation_msgs/srv/UpdateEdgeReconf.srv
new file mode 100644
index 00000000..90ea5b4d
--- /dev/null
+++ b/topological_navigation_msgs/srv/UpdateEdgeReconf.srv
@@ -0,0 +1,7 @@
+string edge_id
+string param_name         # namespace of the param is the edge's action
+string param_value        # python uses its eval function to determine the type unless it is a string
+bool value_is_string
+---
+bool success
+string message

From eefcd05b25476c82069b8ccbfa30ace6611daaa3 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Fri, 5 Feb 2021 10:24:18 +0000
Subject: [PATCH 08/17] service `update_edge_reconf` renamed to
 `update_edge_config`

---
 .../src/topological_navigation/manager2.py                | 8 ++++----
 topological_navigation_msgs/CMakeLists.txt                | 2 +-
 .../srv/{UpdateEdgeReconf.srv => UpdateEdgeConfig.srv}    | 0
 3 files changed, 5 insertions(+), 5 deletions(-)
 rename topological_navigation_msgs/srv/{UpdateEdgeReconf.srv => UpdateEdgeConfig.srv} (100%)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index 399c1e3b..f004a6f2 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -108,7 +108,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.add_tag_srv=rospy.Service('/topological_map_manager2/add_tag_to_node', strands_navigation_msgs.srv.AddTag, self.add_tag_cb)
         self.rm_tag_srv=rospy.Service('/topological_map_manager2/rm_tag_from_node', strands_navigation_msgs.srv.AddTag, self.rm_tag_cb)        
         self.update_edge_srv=rospy.Service('/topological_map_manager2/update_edge', strands_navigation_msgs.srv.UpdateEdge, self.update_edge_cb)
-        self.update_edge_reconf_srv=rospy.Service('/topological_map_manager2/update_edge_reconf', topological_navigation_msgs.srv.UpdateEdgeReconf, self.update_edge_reconf_cb)
+        self.update_edge_config_srv=rospy.Service('/topological_map_manager2/update_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.update_edge_config_cb)
         
         
     def get_time(self):
@@ -838,14 +838,14 @@ def update_edge(self, edge_id, action, top_vel):
             return False, "no edge found or multiple edges found"
         
         
-    def update_edge_reconf_cb(self, req):
+    def update_edge_config_cb(self, req):
         """
         Update edge reconfigure parameters.
         """
-        return self.update_edge_reconf(req.edge_id, req.param_name, req.param_value, req.value_is_string)
+        return self.update_edge_config(req.edge_id, req.param_name, req.param_value, req.value_is_string)
     
     
-    def update_edge_reconf(self, edge_id, param_name, param_value, value_is_string):
+    def update_edge_config(self, edge_id, param_name, param_value, value_is_string):
         
         if not value_is_string:
             param_value = eval(param_value)
diff --git a/topological_navigation_msgs/CMakeLists.txt b/topological_navigation_msgs/CMakeLists.txt
index 11163103..696abb4b 100644
--- a/topological_navigation_msgs/CMakeLists.txt
+++ b/topological_navigation_msgs/CMakeLists.txt
@@ -29,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
 add_service_files(
   FILES
   WriteTopologicalMap.srv
-  UpdateEdgeReconf.srv
+  UpdateEdgeConfig.srv
 )
 
 
diff --git a/topological_navigation_msgs/srv/UpdateEdgeReconf.srv b/topological_navigation_msgs/srv/UpdateEdgeConfig.srv
similarity index 100%
rename from topological_navigation_msgs/srv/UpdateEdgeReconf.srv
rename to topological_navigation_msgs/srv/UpdateEdgeConfig.srv

From 0698406962542bbaf4793e6b3a5230306babd913 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Fri, 5 Feb 2021 12:08:33 +0000
Subject: [PATCH 09/17] Service for removing params from an edge's config and a
 fix.

---
 .../src/topological_navigation/manager2.py    | 45 ++++++++++++++++---
 1 file changed, 38 insertions(+), 7 deletions(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index f004a6f2..e93f98f5 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -109,6 +109,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.rm_tag_srv=rospy.Service('/topological_map_manager2/rm_tag_from_node', strands_navigation_msgs.srv.AddTag, self.rm_tag_cb)        
         self.update_edge_srv=rospy.Service('/topological_map_manager2/update_edge', strands_navigation_msgs.srv.UpdateEdge, self.update_edge_cb)
         self.update_edge_config_srv=rospy.Service('/topological_map_manager2/update_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.update_edge_config_cb)
+        self.rm_param_from_edge_config_srv=rospy.Service('/topological_map_manager2/rm_param_from_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.rm_param_from_edge_config_cb)
         
         
     def get_time(self):
@@ -820,13 +821,13 @@ def update_edge(self, edge_id, action, top_vel):
                 if edge["edge_id"] == edge_id:
                     edge["action"] = action or edge["action"]
                     
-                if "config" in edge and "top_vel" in edge["config"]:
-                    edge["config"]["top_vel"] = top_vel or edge["config"]["top_vel"]
-                elif "config" in edge and "top_vel" not in edge["config"]:
-                    edge["config"]["top_vel"] = top_vel
-                else:
-                    edge["config"] = {}
-                    edge["config"]["top_vel"] = top_vel
+                    if "config" in edge and "top_vel" in edge["config"]:
+                        edge["config"]["top_vel"] = top_vel or edge["config"]["top_vel"]
+                    elif "config" in edge and "top_vel" not in edge["config"]:
+                        edge["config"]["top_vel"] = top_vel
+                    else:
+                        edge["config"] = {}
+                        edge["config"]["top_vel"] = top_vel
                     
             self.tmap2["nodes"][index] = the_node
             self.update()
@@ -873,6 +874,36 @@ def update_edge_config(self, edge_id, param_name, param_value, value_is_string):
             return False, "no edge found or multiple edges found"
         
         
+    def rm_param_from_edge_config_cb(self, req):
+        """
+        Remove a param from an edge's config.
+        """
+        return self.rm_param_from_edge_config(req.edge_id, req.param_name)
+    
+    
+    def rm_param_from_edge_config(self, edge_id, param_name):
+        
+        node_name = edge_id.split('_')[0]
+        num_available, index = self.get_instances_of_node(node_name)
+        
+        if num_available == 1:
+            the_node = copy.deepcopy(self.tmap2["nodes"][index])
+            for edge in the_node["node"]["edges"]:
+                if edge["edge_id"] == edge_id:
+                    if "config" in edge and param_name in edge["config"]:
+                        del edge["config"][param_name]
+        
+            self.tmap2["nodes"][index] = the_node
+            self.update()
+            self.write_topological_map(self.filename)
+            
+            print_config = edge["config"] if "config" in edge else {}
+            return True, "edge action is {} and edge config is {}".format(edge["action"], print_config)
+        else:
+            rospy.logerr("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name))
+            return False, "no edge found or multiple edges found"
+        
+        
     def get_instances_of_node(self, node_name):
         
         num_available = 0

From 62fcfd7dfeabadfe18b5a90ebcde5e6f93cc6f1d Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Fri, 5 Feb 2021 17:52:32 +0000
Subject: [PATCH 10/17] Edges config is now a list where each item is a dict
 with the params namespace, name and value. The default config is empty and
 the tmap to tmap2 conversion sets an empty config.

---
 .../src/topological_navigation/manager.py     |  9 ++++----
 .../src/topological_navigation/manager2.py    | 21 +++++++------------
 2 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/topological_navigation/src/topological_navigation/manager.py b/topological_navigation/src/topological_navigation/manager.py
index b0d612ba..63683078 100644
--- a/topological_navigation/src/topological_navigation/manager.py
+++ b/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()
 ###################################################################################################################
\ No newline at end of file
diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index e93f98f5..2a57a86b 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -470,8 +470,9 @@ def add_edge(self, origin, destination, action, edge_id, update=True):
             return False
         
         
-    def add_edge_to_node(self, origin, destination, action="move_base", edge_id="default", config="default", 
-                         action_type="default", goal="default", fail_policy="fail", restrictions=None):
+    def add_edge_to_node(self, origin, destination, action="move_base", edge_id="default", config=[], 
+                         recovery_behaviours_config="", action_type="default", goal="default", fail_policy="fail", 
+                         restrictions=None):
         
         edge = {}
         edge["action"] = action
@@ -482,14 +483,8 @@ def add_edge_to_node(self, origin, destination, action="move_base", edge_id="def
             edge["edge_id"] = edge_id
         
         edge["node"] = destination
-        
-        if config == "default":
-            edge["config"] = {}
-            edge["config"]["inflation_radius"] = 0.0
-            edge["config"]["top_vel"] = 0.55
-            edge["config"]["recovery_behaviours_config"] = ""
-        else:
-            edge["config"] = config
+        edge["config"] = config
+        edge["recovery_behaviours_config"] = recovery_behaviours_config
         
         if action_type == "default":
             edge["action_type"] = self.set_action_type(action)
@@ -1002,10 +997,10 @@ def tmap2_to_tmap(self):
                     msg_e.edge_id = e["edge_id"]
                     msg_e.node = e["node"]
                     msg_e.action = e["action"]
-                    msg_e.top_vel = e["config"]["top_vel"]
+                    msg_e.top_vel = 0.55
                     msg_e.map_2d = self.metric_map
-                    msg_e.inflation_radius = e["config"]["inflation_radius"]
-                    msg_e.recovery_behaviours_config = e["config"]["recovery_behaviours_config"]
+                    msg_e.inflation_radius = 0.0
+                    msg_e.recovery_behaviours_config = e["recovery_behaviours_config"]
                     msgs_edges.append(msg_e)
                 msg.edges = msgs_edges
                 

From eb87d6f72314b206d7d9bca81fc68cf24f6ed74d Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Mon, 8 Feb 2021 11:26:18 +0000
Subject: [PATCH 11/17] Service `update_edge_config` renamed to
 `add_param_to_edge_config` to better reflect what it does. That service and
 `rm_param_from_edge_config` modified to account for the changes in the
 previous commit. Constructing new class `EdgeReconfigureManager` in
 `navigation.py` to handle everything edge reconfigure related.

---
 topological_navigation/scripts/navigation.py  | 171 ++++++++++++++----
 .../src/topological_navigation/manager2.py    |  36 ++--
 .../srv/UpdateEdgeConfig.srv                  |   5 +-
 3 files changed, 161 insertions(+), 51 deletions(-)

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index 8b8fad80..c3533afd 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -5,7 +5,6 @@
 import sys
 import json
 
-
 import calendar
 from datetime import datetime
 
@@ -62,6 +61,110 @@
 }
 
 
+class EdgeReconfigureManager(object):
+    
+    
+    def __init__(self, use_tmap2, tmap):
+        
+        rospy.loginfo("Using edge reconfigure ...")
+        
+        self.use_tmap2 = use_tmap2
+        self.tmap = tmap
+        
+        self.init_tmap1_edge_reconf() if not use_tmap2 else self.init_tmap2_edge_reconf()
+            
+            
+    def init_tmap1_edge_reconf(self):
+        
+        self.current_edge_group = "none"
+        self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
+        
+            
+    def init_tmap2_edge_reconf(self):
+        
+        namespaces = []
+        for node in self.tmap["nodes"]:
+            for edge in node["edges"]:
+                if "config" in edge:
+                    if edge["config"]:
+                        rospy.loginfo("Edge {} config: {}".format(edge["edge_id"], edge["config"]))
+                    for param in edge["config"]:
+                        namespaces.append(param["namespace"])
+
+        self.rcnf_dict = {}        
+        rospy.loginfo("Creating edge reconfigure clients ...")                
+        for namespace in set(namespaces):
+            rospy.loginfo("Creating client for {}".format(namespace))
+            
+            client = dynamic_reconfigure.client.Client(namespace, timeout=5.0)
+            try:
+                default_params = client.get_configuration()
+            except rospy.ServiceException as e:
+                rospy.warn("Cannot do edge reconfigure for {}: {}".format(namespace, e))
+                continue
+            
+            self.rcnf_dict[namespace] = {"client": client, "default_params": default_params}
+        
+            
+    def tmap1_edge_reconf(self, edge_id):
+        """
+        If using the old map 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:  # 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 "-------"
+    
+    
+    def tmap2_edge_reconf(self, edge):
+        """
+        If using the new map then edge reconfigure is done using settings in the map
+        """
+        if "config" in edge:
+            for param in edge["config"]:
+                if param["namespace"] in self.rcnf_dict:
+                    # HANDLE EXCEPTIONS AND PRINT STUFF!
+                    client = self.rcnf_dict[param["namespace"]]["client"]
+                    client.update_configuration({param["name"]: param["value"]})
+                    
+                    
+    def reset_edge_reconf(self, edge):
+        
+        if "config" in edge:        
+            namespaces = [param["namespace"] for param in edge["config"]]
+            
+            for namespace in set(namespaces):
+                # HANDLE EXCEPTIONS AND PRINT STUFF!
+                if namespace in self.rcnf_dict:
+                    client = self.rcnf_dict[namespace]["client"]
+                    client.update_configuration(self.rcnf_dict[namespace]["default_params"])
+                
+            
+        
+            
+            
+                        
+
+
 """
  Class for Topological Navigation
 
@@ -169,8 +272,8 @@ 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:
+            erm = EdgeReconfigureManager(self.use_tmap2, self.lnodes)
             self.current_edge_group = "none"
-            rospy.loginfo("Using edge reconfigure ...")
             self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
 
         rospy.loginfo("All Done ...")
@@ -533,38 +636,38 @@ 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 "-------"
+#    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
diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index 2a57a86b..239b1bcf 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -108,7 +108,7 @@ def __init__(self, name="new_map", metric_map="map_2d", pointset="new_map", tran
         self.add_tag_srv=rospy.Service('/topological_map_manager2/add_tag_to_node', strands_navigation_msgs.srv.AddTag, self.add_tag_cb)
         self.rm_tag_srv=rospy.Service('/topological_map_manager2/rm_tag_from_node', strands_navigation_msgs.srv.AddTag, self.rm_tag_cb)        
         self.update_edge_srv=rospy.Service('/topological_map_manager2/update_edge', strands_navigation_msgs.srv.UpdateEdge, self.update_edge_cb)
-        self.update_edge_config_srv=rospy.Service('/topological_map_manager2/update_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.update_edge_config_cb)
+        self.add_param_to_edge_config_srv=rospy.Service('/topological_map_manager2/add_param_to_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.add_param_to_edge_config_cb)
         self.rm_param_from_edge_config_srv=rospy.Service('/topological_map_manager2/rm_param_from_edge_config', topological_navigation_msgs.srv.UpdateEdgeConfig, self.rm_param_from_edge_config_cb)
         
         
@@ -834,17 +834,20 @@ def update_edge(self, edge_id, action, top_vel):
             return False, "no edge found or multiple edges found"
         
         
-    def update_edge_config_cb(self, req):
+    def add_param_to_edge_config_cb(self, req):
         """
         Update edge reconfigure parameters.
         """
-        return self.update_edge_config(req.edge_id, req.param_name, req.param_value, req.value_is_string)
+        return self.add_param_to_edge_config(req.edge_id, req.namespace, req.name, req.value, req.value_is_string)
     
     
-    def update_edge_config(self, edge_id, param_name, param_value, value_is_string):
+    def add_param_to_edge_config(self, edge_id, namespace, name, value, value_is_string):
+        
+        if not value:
+            return False, "no value provided"
         
         if not value_is_string:
-            param_value = eval(param_value)
+            value = eval(value)
         
         node_name = edge_id.split('_')[0]
         num_available, index = self.get_instances_of_node(node_name)
@@ -853,11 +856,9 @@ def update_edge_config(self, edge_id, param_name, param_value, value_is_string):
             the_node = copy.deepcopy(self.tmap2["nodes"][index])
             for edge in the_node["node"]["edges"]:
                 if edge["edge_id"] == edge_id:
-                    if "config" in edge:
-                        edge["config"][param_name] = param_value
-                    else:
-                        edge["config"] = {}
-                        edge["config"][param_name] = param_value
+                    if "config" not in edge:
+                        edge["config"] = []
+                    edge["config"].append({"namespace":namespace, "name":name, "value":value})
             
             self.tmap2["nodes"][index] = the_node
             self.update()
@@ -873,10 +874,10 @@ def rm_param_from_edge_config_cb(self, req):
         """
         Remove a param from an edge's config.
         """
-        return self.rm_param_from_edge_config(req.edge_id, req.param_name)
+        return self.rm_param_from_edge_config(req.edge_id, req.namespace, req.name)
     
     
-    def rm_param_from_edge_config(self, edge_id, param_name):
+    def rm_param_from_edge_config(self, edge_id, namespace, name):
         
         node_name = edge_id.split('_')[0]
         num_available, index = self.get_instances_of_node(node_name)
@@ -885,9 +886,14 @@ def rm_param_from_edge_config(self, edge_id, param_name):
             the_node = copy.deepcopy(self.tmap2["nodes"][index])
             for edge in the_node["node"]["edges"]:
                 if edge["edge_id"] == edge_id:
-                    if "config" in edge and param_name in edge["config"]:
-                        del edge["config"][param_name]
-        
+                    if "config" in edge:
+                        params_new = []
+                        for param in copy.deepcopy(edge["config"]):
+                            if param["namespace"] == namespace and param["name"] == name:
+                                continue
+                            else:
+                                params_new.append(param)
+                        edge["config"] = params_new
             self.tmap2["nodes"][index] = the_node
             self.update()
             self.write_topological_map(self.filename)
diff --git a/topological_navigation_msgs/srv/UpdateEdgeConfig.srv b/topological_navigation_msgs/srv/UpdateEdgeConfig.srv
index 90ea5b4d..a06c533c 100644
--- a/topological_navigation_msgs/srv/UpdateEdgeConfig.srv
+++ b/topological_navigation_msgs/srv/UpdateEdgeConfig.srv
@@ -1,6 +1,7 @@
 string edge_id
-string param_name         # namespace of the param is the edge's action
-string param_value        # python uses its eval function to determine the type unless it is a string
+string namespace
+string name
+string value        # python uses its eval function to determine the type unless it is a string
 bool value_is_string
 ---
 bool success

From 6f8ae868ed7ac183f7879cd6826289c71a44e995 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Mon, 8 Feb 2021 16:21:21 +0000
Subject: [PATCH 12/17] EdgeReconfigureManager class done. Needs testing.

---
 topological_navigation/scripts/navigation.py | 111 ++++++++-----------
 1 file changed, 44 insertions(+), 67 deletions(-)

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index c3533afd..35ec93b9 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -68,10 +68,12 @@ def __init__(self, use_tmap2, tmap):
         
         rospy.loginfo("Using edge reconfigure ...")
         
-        self.use_tmap2 = use_tmap2
         self.tmap = tmap
-        
         self.init_tmap1_edge_reconf() if not use_tmap2 else self.init_tmap2_edge_reconf()
+        
+        self.reconf_str = "Reconfigured parameters for edge {}. Parameters: {}"
+        self.reconf_err_str = "Could not reconfigure parameters for edge {}. Parameters: {}. Caught service exception: {}"
+        self.reset_err_str = "Could not reset parameters with namespace {} for edge {}. Caught service exception: {}"
             
             
     def init_tmap1_edge_reconf(self):
@@ -85,9 +87,8 @@ def init_tmap2_edge_reconf(self):
         namespaces = []
         for node in self.tmap["nodes"]:
             for edge in node["edges"]:
-                if "config" in edge:
-                    if edge["config"]:
-                        rospy.loginfo("Edge {} config: {}".format(edge["edge_id"], edge["config"]))
+                if "config" in edge and edge["config"]:
+                    rospy.loginfo("Edge {} config: {}".format(edge["edge_id"], edge["config"]))
                     for param in edge["config"]:
                         namespaces.append(param["namespace"])
 
@@ -100,15 +101,15 @@ def init_tmap2_edge_reconf(self):
             try:
                 default_params = client.get_configuration()
             except rospy.ServiceException as e:
-                rospy.warn("Cannot do edge reconfigure for {}: {}".format(namespace, e))
+                rospy.warn("Cannot do edge reconfigure for {}. Caught service exception when initialising: {}".format(namespace, e))
                 continue
             
             self.rcnf_dict[namespace] = {"client": client, "default_params": default_params}
         
-            
+        
     def tmap1_edge_reconf(self, edge_id):
         """
-        If using the old map edge reconfigure is done via a service.
+        If using the old map then edge reconfigure is done via a service.
         """
         edge_group = "none"
         for group in self.edge_groups:
@@ -133,44 +134,51 @@ def tmap1_edge_reconf(self, edge_id):
             except rospy.ServiceException, e:
                 rospy.logerr("Service call failed: %s" % e)
         print "-------"
+        
+        
+    def register_edge(self, edge):
+        self.edge = edge
     
     
-    def tmap2_edge_reconf(self, edge):
+    def tmap2_edge_reconf(self):
         """
-        If using the new map then edge reconfigure is done using settings in the map
+        If using the new map then edge reconfigure is done using settings in the map.
         """
-        if "config" in edge:
-            for param in edge["config"]:
+        if "config" in self.edge:
+            for param in self.edge["config"]:
                 if param["namespace"] in self.rcnf_dict:
-                    # HANDLE EXCEPTIONS AND PRINT STUFF!
                     client = self.rcnf_dict[param["namespace"]]["client"]
-                    client.update_configuration({param["name"]: param["value"]})
                     
+                    try:
+                        client.update_configuration({param["name"]: param["value"]})
+                        rospy.loginfo(self.reconf_str.format(self.edge["edge_id"], self.edge["config"]))
+                    except rospy.ServiceException as e:
+                        rospy.logwarn(self.reconf_err_str.format(self.edge["edge_id"], self.edge["config"], e))
                     
-    def reset_edge_reconf(self, edge):
-        
-        if "config" in edge:        
-            namespaces = [param["namespace"] for param in edge["config"]]
-            
+                    
+    def reset_edge_reconf(self):
+        """
+        Used to reset edge params to their default values when the action has completed (only if using the new map)
+        """
+        if "config" in self.edge:
+            namespaces = [param["namespace"] for param in self.edge["config"]]
             for namespace in set(namespaces):
-                # HANDLE EXCEPTIONS AND PRINT STUFF!
+    
                 if namespace in self.rcnf_dict:
                     client = self.rcnf_dict[namespace]["client"]
-                    client.update_configuration(self.rcnf_dict[namespace]["default_params"])
+                    
+                    try:
+                        client.update_configuration(self.rcnf_dict[namespace]["default_params"])
+                    except rospy.ServiceException as e:
+                        rospy.logwarn(self.reset_err_str.format(namespace, self.edge["edge_id"], e))
+#############################################################################################################                
                 
-            
-        
-            
-            
-                        
-
-
+                
+#############################################################################################################
 """
  Class for Topological Navigation
 
 """
-
-
 class TopologicalNavServer(object):
     _feedback = topological_navigation.msg.GotoNodeFeedback()
     _result = topological_navigation.msg.GotoNodeResult()
@@ -272,9 +280,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:
-            erm = EdgeReconfigureManager(self.use_tmap2, self.lnodes)
-            self.current_edge_group = "none"
-            self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
+            self.edge_reconf_manager = EdgeReconfigureManager(self.use_tmap2, self.lnodes)
 
         rospy.loginfo("All Done ...")
         rospy.spin()
@@ -636,38 +642,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
@@ -762,7 +736,7 @@ def followRoute(self, route, target):
 
             # If we are using edge reconfigure
             if self.edge_reconfigure:
-                self.edgeReconfigureManager(cedg.edge_id)
+                self.edge_reconf_manager.tmap1_edge_reconf(cedg.edge_id)
 
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
@@ -942,7 +916,8 @@ def followRoute_tmap2(self, route, target):
 
             # If we are using edge reconfigure
             if self.edge_reconfigure:
-                self.edgeReconfigureManager(cedg["edge_id"])
+                self.edge_reconf_manager.register_edge(cedg)
+                self.edge_reconf_manager.tmap2_edge_reconf()
 
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
@@ -1026,6 +1001,8 @@ def followRoute_tmap2(self, route, target):
             self.current_action = "none"
             self.next_action = "none"
             rindex = rindex + 1
+            
+            self.edge_reconf_manager.reset_edge_reconf()
 
         self.reset_reconf()
 

From 7b1c2ccf378197610d5ef3f1e84f668dad8da1d4 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Tue, 9 Feb 2021 18:52:01 +0000
Subject: [PATCH 13/17] Lots of fixes

---
 topological_navigation/scripts/navigation.py  | 147 +++++++++---------
 .../src/topological_navigation/manager2.py    |   9 +-
 2 files changed, 81 insertions(+), 75 deletions(-)

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index 35ec93b9..f96534fb 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -64,50 +64,87 @@
 class EdgeReconfigureManager(object):
     
     
-    def __init__(self, use_tmap2, tmap):
+    def __init__(self, use_tmap2):
         
         rospy.loginfo("Using edge reconfigure ...")
         
-        self.tmap = tmap
-        self.init_tmap1_edge_reconf() if not use_tmap2 else self.init_tmap2_edge_reconf()
+        if not use_tmap2:
+            self.current_edge_group = "none"
+            self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
         
-        self.reconf_str = "Reconfigured parameters for edge {}. Parameters: {}"
-        self.reconf_err_str = "Could not reconfigure parameters for edge {}. Parameters: {}. Caught service exception: {}"
-        self.reset_err_str = "Could not reset parameters with namespace {} for edge {}. Caught service exception: {}"
-            
-            
-    def init_tmap1_edge_reconf(self):
+        self.reconf_str = "Reconfigured edge {} parameters: {}"
+        self.reconf_err_str = "Could not reconfigure edge {} parameters: {}. Caught service exception: {}"
+        self.reset_err_str = "Could not reset edge {} parameters with namespace {}. Caught service exception: {}"
         
-        self.current_edge_group = "none"
-        self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
         
-            
-    def init_tmap2_edge_reconf(self):
+    def initialise(self, tmap):
         
         namespaces = []
-        for node in self.tmap["nodes"]:
-            for edge in node["edges"]:
-                if "config" in edge and edge["config"]:
-                    rospy.loginfo("Edge {} config: {}".format(edge["edge_id"], edge["config"]))
+        for node in tmap["nodes"]:
+            for edge in node["node"]["edges"]:
+                if "config" in edge:
                     for param in edge["config"]:
                         namespaces.append(param["namespace"])
 
-        self.rcnf_dict = {}        
-        rospy.loginfo("Creating edge reconfigure clients ...")                
+        self.rcnf_dict = {}                    
         for namespace in set(namespaces):
-            rospy.loginfo("Creating client for {}".format(namespace))
+            rospy.loginfo("Getting default parameters for {}".format(namespace))
             
-            client = dynamic_reconfigure.client.Client(namespace, timeout=5.0)
+            client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
             try:
                 default_params = client.get_configuration()
+                self.rcnf_dict[namespace] = default_params
+                
             except rospy.ServiceException as e:
                 rospy.warn("Cannot do edge reconfigure for {}. Caught service exception when initialising: {}".format(namespace, e))
-                continue
             
-            self.rcnf_dict[namespace] = {"client": client, "default_params": default_params}
+            
+    def register_edge(self, edge):
+        
+        self.edge = edge   
+        self.namespaces = self.get_namespaces()       
+        
         
+    def get_namespaces(self):
         
-    def tmap1_edge_reconf(self, edge_id):
+        namespaces = []
+        if "config" in self.edge:
+            namespaces = [param["namespace"] for param in self.edge["config"]]
+            
+        return list(set(namespaces))
+            
+            
+    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.rcnf_dict:
+                    
+                    client = dynamic_reconfigure.client.Client(param["namespace"], timeout=2.0)
+                    try:
+                        client.update_configuration({param["name"]: param["value"]})
+                        rospy.loginfo(self.reconf_str.format(self.edge["edge_id"], self.edge["config"]))
+                        
+                    except rospy.ServiceException as e:
+                        rospy.logwarn(self.reconf_err_str.format(self.edge["edge_id"], self.edge["config"], e))
+                    
+                    
+    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.rcnf_dict:
+            client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
+            try:
+                client.update_configuration(self.rcnf_dict[namespace])
+                
+            except rospy.ServiceException as e:
+                rospy.logwarn(self.reset_err_str.format(namespace, self.edge["edge_id"], e))
+                    
+                    
+    def srv_reconfigure(self, edge_id):
         """
         If using the old map then edge reconfigure is done via a service.
         """
@@ -134,43 +171,6 @@ def tmap1_edge_reconf(self, edge_id):
             except rospy.ServiceException, e:
                 rospy.logerr("Service call failed: %s" % e)
         print "-------"
-        
-        
-    def register_edge(self, edge):
-        self.edge = edge
-    
-    
-    def tmap2_edge_reconf(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.rcnf_dict:
-                    client = self.rcnf_dict[param["namespace"]]["client"]
-                    
-                    try:
-                        client.update_configuration({param["name"]: param["value"]})
-                        rospy.loginfo(self.reconf_str.format(self.edge["edge_id"], self.edge["config"]))
-                    except rospy.ServiceException as e:
-                        rospy.logwarn(self.reconf_err_str.format(self.edge["edge_id"], self.edge["config"], e))
-                    
-                    
-    def reset_edge_reconf(self):
-        """
-        Used to reset edge params to their default values when the action has completed (only if using the new map)
-        """
-        if "config" in self.edge:
-            namespaces = [param["namespace"] for param in self.edge["config"]]
-            for namespace in set(namespaces):
-    
-                if namespace in self.rcnf_dict:
-                    client = self.rcnf_dict[namespace]["client"]
-                    
-                    try:
-                        client.update_configuration(self.rcnf_dict[namespace]["default_params"])
-                    except rospy.ServiceException as e:
-                        rospy.logwarn(self.reset_err_str.format(namespace, self.edge["edge_id"], e))
 #############################################################################################################                
                 
                 
@@ -280,7 +280,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.edge_reconf_manager = EdgeReconfigureManager(self.use_tmap2, self.lnodes)
+            self.edgeReconfigureManager = EdgeReconfigureManager(self.use_tmap2)
 
         rospy.loginfo("All Done ...")
         rospy.spin()
@@ -736,7 +736,7 @@ def followRoute(self, route, target):
 
             # If we are using edge reconfigure
             if self.edge_reconfigure:
-                self.edge_reconf_manager.tmap1_edge_reconf(cedg.edge_id)
+                self.edgeReconfigureManager.srv_reconfigure(cedg.edge_id)
 
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
@@ -834,7 +834,8 @@ def followRoute_tmap2(self, route, target):
         Orig = route.source[0]
         Targ = target
         self._target = Targ
-
+        
+        self.edgeReconfigureManager.initialise(self.lnodes)
         self.init_reconfigure()
 
         rospy.loginfo("%d Nodes on route" % nnodes)
@@ -914,11 +915,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.edge_reconf_manager.register_edge(cedg)
-                self.edge_reconf_manager.tmap2_edge_reconf()
-
             self._feedback.route = "%s to %s using %s" % (
                 route.source[rindex],
                 cedg["node"],
@@ -956,8 +952,17 @@ def followRoute_tmap2(self, route, target):
             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.reconfigure()
+            
             nav_ok, inc = self.monitored_navigation(inf, a)
-            # 5 degrees tolerance   'max_vel_x':0.55,
+            
+            if self.edge_reconfigure:
+                self.edgeReconfigureManager._reset()
+            
             params = {"yaw_goal_tolerance": 0.087266, "xy_goal_tolerance": 0.1}
             self.reconfigure_movebase_params(params)
 
@@ -1001,8 +1006,6 @@ def followRoute_tmap2(self, route, target):
             self.current_action = "none"
             self.next_action = "none"
             rindex = rindex + 1
-            
-            self.edge_reconf_manager.reset_edge_reconf()
 
         self.reset_reconf()
 
diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index 239b1bcf..f6a14be3 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -854,17 +854,19 @@ def add_param_to_edge_config(self, edge_id, namespace, name, value, value_is_str
         
         if num_available == 1:
             the_node = copy.deepcopy(self.tmap2["nodes"][index])
+            msg = ""
             for edge in the_node["node"]["edges"]:
                 if edge["edge_id"] == edge_id:
                     if "config" not in edge:
                         edge["config"] = []
                     edge["config"].append({"namespace":namespace, "name":name, "value":value})
+                    msg = "edge action is {} and edge config is {}".format(edge["action"], edge["config"])
             
             self.tmap2["nodes"][index] = the_node
             self.update()
             self.write_topological_map(self.filename)
             
-            return True, "edge action is {} and edge config is {}".format(edge["action"], edge["config"])
+            return True, msg
         else:
             rospy.logerr("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name))
             return False, "no edge found or multiple edges found"
@@ -884,6 +886,7 @@ def rm_param_from_edge_config(self, edge_id, namespace, name):
         
         if num_available == 1:
             the_node = copy.deepcopy(self.tmap2["nodes"][index])
+            msg = ""
             for edge in the_node["node"]["edges"]:
                 if edge["edge_id"] == edge_id:
                     if "config" in edge:
@@ -894,12 +897,12 @@ def rm_param_from_edge_config(self, edge_id, namespace, name):
                             else:
                                 params_new.append(param)
                         edge["config"] = params_new
+                        msg = "edge action is {} and edge config is {}".format(edge["action"], edge["config"])
             self.tmap2["nodes"][index] = the_node
             self.update()
             self.write_topological_map(self.filename)
             
-            print_config = edge["config"] if "config" in edge else {}
-            return True, "edge action is {} and edge config is {}".format(edge["action"], print_config)
+            return True, msg
         else:
             rospy.logerr("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name))
             return False, "no edge found or multiple edges found"

From fcc9c1cb42385b1e462debec3b486af2105aecf7 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Wed, 10 Feb 2021 13:07:34 +0000
Subject: [PATCH 14/17] Fixes and improvements to the edge reconfigure manager.

---
 topological_navigation/scripts/navigation.py | 87 +++++++++-----------
 1 file changed, 37 insertions(+), 50 deletions(-)

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index f96534fb..6f85f8bb 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -66,52 +66,38 @@ class EdgeReconfigureManager(object):
     
     def __init__(self, use_tmap2):
         
-        rospy.loginfo("Using edge reconfigure ...")
+        rospy.logwarn("USING EDGE RECONFIGURE ...")
         
         if not use_tmap2:
             self.current_edge_group = "none"
             self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
         
-        self.reconf_str = "Reconfigured edge {} parameters: {}"
-        self.reconf_err_str = "Could not reconfigure edge {} parameters: {}. Caught service exception: {}"
-        self.reset_err_str = "Could not reset edge {} parameters with namespace {}. Caught service exception: {}"
         
+    def register_edge(self, edge):
         
-    def initialise(self, tmap):
+        self.edge = edge
         
         namespaces = []
-        for node in tmap["nodes"]:
-            for edge in node["node"]["edges"]:
-                if "config" in edge:
-                    for param in edge["config"]:
-                        namespaces.append(param["namespace"])
-
-        self.rcnf_dict = {}                    
-        for namespace in set(namespaces):
-            rospy.loginfo("Getting default parameters for {}".format(namespace))
-            
-            client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
-            try:
-                default_params = client.get_configuration()
-                self.rcnf_dict[namespace] = default_params
-                
-            except rospy.ServiceException as e:
-                rospy.warn("Cannot do edge reconfigure for {}. Caught service exception when initialising: {}".format(namespace, e))
+        if "config" in edge:
+            namespaces = [param["namespace"] for param in edge["config"]]
             
-            
-    def register_edge(self, edge):
-        
-        self.edge = edge   
-        self.namespaces = self.get_namespaces()       
+        self.namespaces = list(set(namespaces))
+        if self.namespaces:
+            rospy.loginfo("Edge Reconfigure Manager: Processing edge {} ...".format(edge["edge_id"]))
         
         
-    def get_namespaces(self):
+    def initialise(self):
         
-        namespaces = []
-        if "config" in self.edge:
-            namespaces = [param["namespace"] for param in self.edge["config"]]
+        self.default_config = {}                    
+        for namespace in self.namespaces:
+            rospy.loginfo("Edge Reconfigure Manager: Getting default configuration for {}".format(namespace))
             
-        return list(set(namespaces))
+            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):
@@ -120,28 +106,28 @@ def reconfigure(self):
         """
         if "config" in self.edge:
             for param in self.edge["config"]:
-                if param["namespace"] in self.rcnf_dict:
-                    
-                    client = dynamic_reconfigure.client.Client(param["namespace"], timeout=2.0)
-                    try:
-                        client.update_configuration({param["name"]: param["value"]})
-                        rospy.loginfo(self.reconf_str.format(self.edge["edge_id"], self.edge["config"]))
-                        
-                    except rospy.ServiceException as e:
-                        rospy.logwarn(self.reconf_err_str.format(self.edge["edge_id"], self.edge["config"], e))
+                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.rcnf_dict:
-            client = dynamic_reconfigure.client.Client(namespace, timeout=2.0)
-            try:
-                client.update_configuration(self.rcnf_dict[namespace])
+        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])
                 
-            except rospy.ServiceException as e:
-                rospy.logwarn(self.reset_err_str.format(namespace, self.edge["edge_id"], e))
+                
+    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):
@@ -166,7 +152,7 @@ def srv_reconfigure(self, edge_id):
                 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
+                if resp1.success:
                     self.current_edge_group = edge_group
             except rospy.ServiceException, e:
                 rospy.logerr("Service call failed: %s" % e)
@@ -835,7 +821,6 @@ def followRoute_tmap2(self, route, target):
         Targ = target
         self._target = Targ
         
-        self.edgeReconfigureManager.initialise(self.lnodes)
         self.init_reconfigure()
 
         rospy.loginfo("%d Nodes on route" % nnodes)
@@ -956,12 +941,14 @@ def followRoute_tmap2(self, route, target):
             # If we are using edge reconfigure
             if self.edge_reconfigure:
                 self.edgeReconfigureManager.register_edge(cedg)
-                self.edgeReconfigureManager.reconfigure()
+                self.edgeReconfigureManager.initialise()
+                self.edgeReconfigureManager.reconfigure() 
             
             nav_ok, inc = self.monitored_navigation(inf, a)
             
             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)

From 97ed6b1a039df9bfdcc329fe1d0389c94d56cce5 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Wed, 10 Feb 2021 14:27:40 +0000
Subject: [PATCH 15/17] Cant add duplicate params when using srv
 `add_param_to_edge_config`

---
 topological_navigation/src/topological_navigation/manager2.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/topological_navigation/src/topological_navigation/manager2.py b/topological_navigation/src/topological_navigation/manager2.py
index f6a14be3..8668f3ef 100644
--- a/topological_navigation/src/topological_navigation/manager2.py
+++ b/topological_navigation/src/topological_navigation/manager2.py
@@ -852,6 +852,7 @@ def add_param_to_edge_config(self, edge_id, namespace, name, value, value_is_str
         node_name = edge_id.split('_')[0]
         num_available, index = self.get_instances_of_node(node_name)
         
+        new_param = {"namespace":namespace, "name":name, "value":value}
         if num_available == 1:
             the_node = copy.deepcopy(self.tmap2["nodes"][index])
             msg = ""
@@ -859,7 +860,8 @@ def add_param_to_edge_config(self, edge_id, namespace, name, value, value_is_str
                 if edge["edge_id"] == edge_id:
                     if "config" not in edge:
                         edge["config"] = []
-                    edge["config"].append({"namespace":namespace, "name":name, "value":value})
+                    if new_param not in edge["config"]:
+                        edge["config"].append(new_param)
                     msg = "edge action is {} and edge config is {}".format(edge["action"], edge["config"])
             
             self.tmap2["nodes"][index] = the_node

From 9836406efa4d8af2bedbc5240c849bf837127db0 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Wed, 10 Feb 2021 16:13:39 +0000
Subject: [PATCH 16/17] The edge reconfigure manager is in its own file.

---
 topological_navigation/scripts/navigation.py  | 104 +----------------
 .../EdgeReconfigureManager.py                 | 108 ++++++++++++++++++
 2 files changed, 110 insertions(+), 102 deletions(-)
 create mode 100644 topological_navigation/src/topological_navigation/EdgeReconfigureManager.py

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index 6f85f8bb..c141b4cd 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -15,8 +15,6 @@
 
 from geometry_msgs.msg import Pose
 
-from strands_navigation_msgs.srv import ReconfAtEdges
-
 from actionlib_msgs.msg import *
 from move_base_msgs.msg import *
 from std_msgs.msg import String
@@ -29,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
@@ -61,106 +61,6 @@
 }
 
 
-class EdgeReconfigureManager(object):
-    
-    
-    def __init__(self, use_tmap2):
-        
-        rospy.logwarn("USING EDGE RECONFIGURE ...")
-        
-        if not use_tmap2:
-            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 "-------"
-#############################################################################################################                
-                
-                
-#############################################################################################################
 """
  Class for Topological Navigation
 
diff --git a/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py b/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py
new file mode 100644
index 00000000..e56d2d92
--- /dev/null
+++ b/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+"""
+Created on Wed Feb 10 15:58:34 2021
+
+@author: Adam Binch (abinch@sagarobotics.com)
+"""
+#############################################################################################################
+import rospy, dynamic_reconfigure.client
+from strands_navigation_msgs.srv import ReconfAtEdges
+
+
+class EdgeReconfigureManager(object):
+    
+    
+    def __init__(self, use_tmap2):
+        
+        rospy.logwarn("Edge Reconfigure Manager: USING EDGE RECONFIGURE ...")
+        
+        if not use_tmap2:
+            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 "-------"
+############################################################################################################# 
\ No newline at end of file

From 4ac3bef70f1dc9cbf66c46cdc354bbfa2b42cc67 Mon Sep 17 00:00:00 2001
From: adambinch <abinch@sagarobotics.com>
Date: Fri, 12 Feb 2021 00:39:09 +0000
Subject: [PATCH 17/17] minor improvement to the edge reconfigure manager

---
 topological_navigation/scripts/navigation.py          |  2 +-
 .../topological_navigation/EdgeReconfigureManager.py  | 11 +++++++----
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/topological_navigation/scripts/navigation.py b/topological_navigation/scripts/navigation.py
index c141b4cd..f1af29e6 100755
--- a/topological_navigation/scripts/navigation.py
+++ b/topological_navigation/scripts/navigation.py
@@ -166,7 +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.edgeReconfigureManager = EdgeReconfigureManager(self.use_tmap2)
+            self.edgeReconfigureManager = EdgeReconfigureManager()
 
         rospy.loginfo("All Done ...")
         rospy.spin()
diff --git a/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py b/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py
index e56d2d92..78ba0066 100644
--- a/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py
+++ b/topological_navigation/src/topological_navigation/EdgeReconfigureManager.py
@@ -12,13 +12,16 @@
 class EdgeReconfigureManager(object):
     
     
-    def __init__(self, use_tmap2):
+    def __init__(self):
         
         rospy.logwarn("Edge Reconfigure Manager: USING EDGE RECONFIGURE ...")
         
-        if not use_tmap2:
-            self.current_edge_group = "none"
-            self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {})
+        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):