diff --git a/topological_navigation/package.xml b/topological_navigation/package.xml
index 60ae93eb..ff4fd8e6 100644
--- a/topological_navigation/package.xml
+++ b/topological_navigation/package.xml
@@ -31,7 +31,8 @@
visualization_msgs
topological_navigation_msgs
- tf2
+ python-transforms3d-pip
+ tf_transformations
ament_python
diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py
index 258c0138..36f090bf 100644
--- a/topological_navigation/setup.py
+++ b/topological_navigation/setup.py
@@ -39,7 +39,11 @@
'topological_transform_publisher.py = topological_navigation.scripts.topological_transform_publisher:main',
'travel_time_estimator.py = topological_navigation.scripts.travel_time_estimator:main',
'visualise_map2.py = topological_navigation.scripts.visualise_map2:main',
- 'visualise_map.py = topological_navigation.scripts.visualise_map:main'
+ 'visualise_map.py = topological_navigation.scripts.visualise_map:main',
+ 'visualise_map_ros2.py = topological_navigation.scripts.visualise_map_ros2:main',
+ 'topomap_marker.py = topological_navigation.topomap_marker:main',
+ 'topomap_marker2.py = topological_navigation.topomap_marker2:main',
+ 'policy_marker.py = topological_navigation.policy_marker:main'
],
},
diff --git a/topological_navigation/topological_navigation/policy_marker.py b/topological_navigation/topological_navigation/policy_marker.py
new file mode 100644
index 00000000..b8b82bf0
--- /dev/null
+++ b/topological_navigation/topological_navigation/policy_marker.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+import math
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+
+from tf_transformations import quaternion_from_euler
+
+from interactive_markers.interactive_marker_server import *
+from visualization_msgs.msg import *
+
+from topological_navigation_msgs.msg import TopologicalMap, NavRoute
+
+
+class PoliciesVis(Node):
+
+ def __init__(self):
+ super().__init__('topological_policy_markers')
+ self.map = None
+ self.policy = MarkerArray()
+ self.route_nodes = NavRoute()
+ self.marker_pub = self.create_publisher(MarkerArray, '~/vis', 10)
+ self.marker_pub.publish(self.policy)
+ qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+ self.map_sub = self.create_subscription(TopologicalMap, '/topological_map', self.map_callback, qos)
+ self.policy_sub = self.create_subscription(NavRoute, 'mdp_plan_exec/current_policy_mode', self.policies_callback, 10)
+ self.get_logger().info('policy visualiser init complete')
+
+ def map_callback(self, msg):
+ self.get_logger().info('map callback triggered')
+ self.map = msg
+
+ def policies_callback(self, msg):
+ self.get_logger().info('policies callback triggered')
+ self.get_logger().info(str(msg))
+ if not self.map: return
+
+ self.added_sources = []
+ self.route_nodes = msg
+ print(self.route_nodes)
+
+ self.map_edges.markers=[]
+
+ counter=0
+ total = len(self.route_nodes.source)
+
+ for i in range(0, total):
+ # Find source and edge
+ source = self.route_nodes.source[i]
+ edge_id = self.route_nodes.edge_id[i]
+
+ # Get positions for start and end
+ ori = tmap_utils.get_node(self.map, source)
+ targ = tmap_utils.get_edge_from_id(self.map, ori.name, edge_id).node
+ if not targ: continue
+
+ # Edge
+ target = self.get_node(self.map, targ)
+ self.added_sources.append(source)
+ colour = [0.1,0.1,0.9] if targ in self.added_sources else [0.9,0.1,0.1]
+ self.map_edges.markers.append(self.create_edge(ori.pose.position, target.pose.position, colour))
+
+ # Publish
+ for i, marker in enumerate(self.policy.markers):
+ marker.id = i
+ self.policies_pub.publish(self.policy)
+
+ def create_edge(self, point1, point2, color):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.ARROW
+
+ pose = Pose()
+
+ pose.position.x = point1.x
+ pose.position.y = point1.y
+ pose.position.z = point1.z
+ angle = math.atan2((point2.y-point1.y),(point2.x-point1.x))
+
+ qat = quaternion_from_euler(0, 0, angle)
+ pose.orientation.w = qat[3]
+ pose.orientation.x = qat[0]
+ pose.orientation.y = qat[1]
+ pose.orientation.z = qat[2]
+
+ r = math.hypot((point2.y-point1.y),(point2.x-point1.x))#/3.0
+ marker.scale.x = r
+ marker.scale.y = 0.15
+ marker.scale.z = 0.15
+ marker.color.a = 0.95
+ marker.color.r = colour[0]
+ marker.color.g = colour[1]
+ marker.color.b = colour[2]
+ marker.pose = pose
+ self.policy.markers.append(marker)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ PV = PoliciesVis()
+ rclpy.spin(PV)
+
+ PV.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py b/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py
index 5d5163cc..8407b0fe 100755
--- a/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py
+++ b/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py
@@ -1,68 +1,57 @@
-#!/usr/bin/env python
-"""
-Created on Fri Feb 26 10:37:50 2021
+# -*- coding: utf-8 -*-
+#!/usr/bin/env python3
+# ----------------------------------
+# @author: Adam Binch
+# @email: abinch@sagarobotics.com
+# @date: Fri Feb 26 10:37:50 2021
+# ----------------------------------
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+import tf2_ros
-@author: Adam Binch (abinch@sagarobotics.com)
-"""
-#########################################################################################################
-import rospy, json
from std_msgs.msg import String
from geometry_msgs.msg import Vector3, Quaternion, TransformStamped
-from rospy_message_converter import message_converter
-import tf2_ros
-
-
-class TopologicalTransformPublisher(object):
- """This class defines a tf publisher that publishes the transformation
- between parent and child frames of a topological_map_2.
- """
- def __init__(self):
- self.rec_map=False
- self.broadcaster = tf2_ros.StaticTransformBroadcaster()
-
- rospy.Subscriber('topological_map_2', String, self.map_callback)
-
- rospy.loginfo("Transform Publisher waiting for the Topological Map...")
- while not self.rec_map :
- rospy.sleep(rospy.Duration.from_sec(0.1))
+class TopologicalTransformPublisher(Node):
+ def __init__(self):
+ super().__init__("topological_transform_publisher")
+ self.tf_broadcaster = tf2_ros.static_transform_broadcaster.StaticTransformBroadcaster(self)
+ qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+ self.create_subscription(String, '/topological_map_2', self.map_callback, qos)
+ self.get_logger().info("Transform Publisher waiting for the Topological Map...")
def map_callback(self, msg):
-
- self.tmap = json.loads(msg.data)
- self.rec_map=True
-
- rospy.loginfo("Transform Publisher received the Topological Map")
-
- self.publish_transform()
-
-
- def publish_transform(self):
-
- transformation = self.tmap["transformation"]
-
- trans = message_converter.convert_dictionary_to_ros_message("geometry_msgs/Vector3", transformation["translation"])
- rot = message_converter.convert_dictionary_to_ros_message("geometry_msgs/Quaternion", transformation["rotation"])
-
+ tmap = json.loads(msg.data)
+ self.get_logger().info("Transform Publisher received the Topological Map")
+ transformation = tmap["transformation"]
msg = TransformStamped()
- msg.header.stamp = rospy.Time.now()
+ msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = transformation["parent"]
msg.child_frame_id = transformation["child"]
- msg.transform.translation = trans
- msg.transform.rotation = rot
-
- self.broadcaster.sendTransform(msg)
+ msg.transform.translation.x = transformation["translation"]["x"]
+ msg.transform.translation.y = transformation["translation"]["y"]
+ msg.transform.translation.z = transformation["translation"]["z"]
+ msg.transform.rotation.x = transformation["rotation"]["x"]
+ msg.transform.rotation.y = transformation["rotation"]["y"]
+ msg.transform.rotation.z = transformation["rotation"]["z"]
+ msg.transform.rotation.w = transformation["rotation"]["w"]
+ self.tf_broadcaster.sendTransform(msg)
-#########################################################################################################
+def main(args=None):
+ rclpy.init(args=args)
-#########################################################################################################
-if __name__ == '__main__':
+ TTP = TopologicalTransformPublisher()
+ rclpy.spin(TTP)
- rospy.init_node("topological_transform_publisher")
+ TTP.destroy_node()
+ rclpy.shutdown()
- TopologicalTransformPublisher()
- rospy.spin()
-#########################################################################################################
\ No newline at end of file
+if __name__ == '__main__':
+ main()
diff --git a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py
new file mode 100755
index 00000000..cb9270e4
--- /dev/null
+++ b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py
@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+
+from topological_navigation.policy_marker import PoliciesVis
+from topological_navigation.topomap_marker import TopologicalVis
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ TV = TopologicalVis()
+ PV = PoliciesVis()
+
+ executor = MultiThreadedExecutor()
+ executor.add_node(TV)
+ executor.add_node(PV)
+ executor.spin()
+
+ TV.destroy_node()
+ PV.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
+
diff --git a/topological_navigation/topological_navigation/testing.py b/topological_navigation/topological_navigation/testing.py
index b09df39d..13790baf 100644
--- a/topological_navigation/topological_navigation/testing.py
+++ b/topological_navigation/topological_navigation/testing.py
@@ -25,7 +25,7 @@ def create_cross_map(width, height, nodeSeparation):
# horizontal nodes
prevNodeName = ''
for x in range(-(width/2),(width/2)+1):
- if x != 0:
+ if x != 0:
nodeName = 'h_%s'%x
node = TopologicalNode(name=nodeName, pose=Pose(position=Point(x * nodeSeparation, yOrigin, 0)))
nodes[nodeName] = node
diff --git a/topological_navigation/topological_navigation/topomap_marker.py b/topological_navigation/topological_navigation/topomap_marker.py
new file mode 100644
index 00000000..25d62253
--- /dev/null
+++ b/topological_navigation/topological_navigation/topomap_marker.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+
+from std_msgs.msg import String
+from visualization_msgs.msg import *
+from geometry_msgs.msg import Point
+
+from topological_navigation_msgs.msg import TopologicalMap
+import topological_navigation.tmap_utils as tmap_utils
+
+
+class TopologicalVis(Node):
+
+ def __init__(self):
+ super().__init__('topological_map_markers')
+ qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+ # Marker Publisher
+ self.map_markers = MarkerArray()
+ self.topmap_pub = self.create_publisher(MarkerArray, '~/vis', 2)
+ self.topmap_pub.publish(self.map_markers)
+
+ # Rescaller Subscriber
+ self.scale = 1
+ self.rescale_sub = self.create_subscription(String, '~/rescale', self.rescale_callback, qos)
+
+ # Map Subscriber
+ self.map = None
+ self.map_sub = self.create_subscription(TopologicalMap, '/topological_map', self.map_callback, qos)
+ self.get_logger().info('map visualiser init complete')
+
+
+ def rescale_callback(self, msg):
+ self.get_logger().info('new scale recieved')
+ self.get_logger().info(f'scale: {msg.data}')
+ self.scale = float(msg.data)
+ self.map_callback(self.map)
+
+ def map_callback(self, msg):
+ self.get_logger().info('map callback triggered')
+ self.get_logger().info(f'name: {msg.name}')
+ self.get_logger().info(f'map: {msg.map}')
+ self.get_logger().info(f'pointset: {msg.pointset}')
+ self.get_logger().info(f'last_updated: {msg.last_updated}')
+ self.get_logger().info(f'total nodes: {len(msg.nodes)}')
+
+ # Message
+ self.map = msg
+
+ # Array
+ self.map_markers = MarkerArray()
+ self.map_markers.markers=[]
+
+ # Legend
+ actions = list(set(sum([[edge.action for edge in node.edges] for node in self.map.nodes],[])))
+ for i, action in enumerate(actions):
+ self.map_markers.markers.append(self.get_legend_marker(action, i))
+
+ # Map
+ for node in self.map.nodes:
+
+ # Nodes
+ self.map_markers.markers.append(self.get_node_marker(node)) # Node
+ self.map_markers.markers.append(self.get_name_marker(node)) # Name
+ self.map_markers.markers.append(self.get_zone_marker(node)) # Zone
+
+ # Edges
+ for edge in node.edges:
+ marker = self.get_edge_marker(node, edge, actions.index(edge.action))
+ if marker:
+ self.map_markers.markers.append(marker)
+
+ # Publish
+ for i, marker in enumerate(self.map_markers.markers):
+ marker.id = i
+ self.topmap_pub.publish(self.map_markers)
+ self.get_logger().info('new map visual published')
+
+
+ def get_legend_marker(self, action, col_id):
+ col=self.get_colour(col_id)
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.TEXT_VIEW_FACING
+ marker.text=action
+ marker.pose.position.x= 1.0+(0.12*col_id)
+ marker.pose.position.y= 0.0
+ marker.pose.position.z= 0.2
+ marker.pose.orientation.w= 1.0
+ marker.scale.z = self.scale * 0.1
+ marker.color.a = 0.5
+ marker.color.r = float(col[0])
+ marker.color.g = float(col[1])
+ marker.color.b = float(col[2])
+ marker.ns='/legend'
+ return marker
+
+ def get_name_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.TEXT_VIEW_FACING
+ marker.text= node.name
+ marker.pose = node.pose
+ marker.scale.z = self.scale * 0.12
+ marker.color.a = 0.9
+ marker.color.r = 0.3
+ marker.color.g = 0.3
+ marker.color.b = 0.3
+ marker.ns='/names'
+ return marker
+
+ def get_edge_marker(self, node, edge, col_id):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.LINE_LIST
+ V1=Point()
+ V2=Point()
+ V1= node.pose.position
+ to_node=tmap_utils.get_node(self.map, edge.node)
+ col=self.get_colour(col_id)
+ if to_node:
+ V2= to_node.pose.position
+ marker.pose.orientation.w= 1.0
+ marker.scale.x = self.scale * 0.1
+ marker.color.a = 0.5
+ marker.color.r = float(col[0])
+ marker.color.g = float(col[1])
+ marker.color.b = float(col[2])
+ marker.points.append(V1)
+ marker.points.append(V2)
+ marker.ns='/edges'
+ return marker
+ else:
+ rospy.logwarn("No node %s found" %edge.node)
+ return None
+
+ def get_node_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = Marker.SPHERE
+ marker.scale.x = self.scale * 0.2
+ marker.scale.y = self.scale * 0.2
+ marker.scale.z = self.scale * 0.2
+ marker.color.a = 0.4
+ marker.color.r = 0.2
+ marker.color.g = 0.2
+ marker.color.b = 0.7
+ marker.pose = node.pose
+ marker.pose.position.z = marker.pose.position.z+0.1
+ marker.ns='/nodes'
+ return marker
+
+
+ def get_zone_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.LINE_STRIP
+ marker.pose.orientation.w= 1.0
+ marker.scale.x = self.scale * 0.1
+ marker.color.a = 0.8
+ marker.color.r = 0.7
+ marker.color.g = 0.1
+ marker.color.b = 0.2
+
+ for j in node.verts :
+ Vert = Point()
+ Vert.z = 0.05
+ Vert.x = node.pose.position.x + j.x
+ Vert.y = node.pose.position.y + j.y
+ marker.points.append(Vert)
+
+ Vert = Point()
+ Vert.z = 0.05
+ Vert.x = node.pose.position.x + node.verts[0].x
+ Vert.y = node.pose.position.y + node.verts[0].y
+ marker.points.append(Vert)
+ marker.ns='/zones'
+ return marker
+
+ def get_colour(self, number):
+ pallete=[[1,1,1],[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0],[1,0,1],[0,1,1]]
+ return pallete[number] if number < len(pallete) else pallete[0]
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ TV = TopologicalVis()
+ rclpy.spin(TV)
+
+ TV.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/topological_navigation/topological_navigation/topomap_marker2.py b/topological_navigation/topological_navigation/topomap_marker2.py
new file mode 100644
index 00000000..e5f16ba0
--- /dev/null
+++ b/topological_navigation/topological_navigation/topomap_marker2.py
@@ -0,0 +1,214 @@
+#!/usr/bin/env python3
+
+import yaml
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, DurabilityPolicy
+
+from std_msgs.msg import String
+from visualization_msgs.msg import *
+from geometry_msgs.msg import Point
+
+#from topological_navigation_msgs.msg import TopologicalMap
+import topological_navigation.tmap_utils as tmap_utils
+
+
+class TopologicalVis(Node):
+
+ def __init__(self):
+ super().__init__('topological_map_markers')
+ qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+ # Marker Publisher
+ self.map_markers = MarkerArray()
+ self.topmap_pub = self.create_publisher(MarkerArray, '~/vis', 2)
+ self.topmap_pub.publish(self.map_markers)
+
+ # Rescaller Subscriber
+ self.scale = 1
+ self.rescale_sub = self.create_subscription(String, '~/rescale', self.rescale_callback, qos)
+
+ # Map Subscriber
+ self.map = None
+ self.map_sub = self.create_subscription(String, '/topological_map_2', self.map_callback, qos)
+ self.get_logger().info('map visualiser init complete')
+
+
+ def rescale_callback(self, msg):
+ self.get_logger().info('new scale recieved')
+ self.get_logger().info(f'scale: {msg.data}')
+ self.scale = float(msg.data)
+ self.map_callback(self.map)
+
+ def map_callback(self, msg):
+ # Save Message for Republishing with scale
+ self.map = msg
+
+ # Load Yaml
+ self.tmap = yaml.safe_load(msg.data)
+
+ # Display Meta Information
+ self.get_logger().info(' ')
+ self.get_logger().info('-----')
+ self.get_logger().info(' ')
+ self.get_logger().info('map callback triggered')
+ self.get_logger().info(f'name: {self.tmap["name"]}')
+ self.get_logger().info(f'map: {self.tmap["metric_map"]}')
+ self.get_logger().info(f'pointset: {self.tmap["pointset"]}')
+ self.get_logger().info(f'last_updated: {self.tmap["meta"]["last_updated"]}')
+ self.get_logger().info(f'total nodes: {len(self.tmap["nodes"])}')
+
+ # Array
+ self.map_markers = MarkerArray()
+ self.map_markers.markers=[]
+
+ # Legend
+ actions = list(set(sum([[edge['action'] for edge in node['node']['edges']] for node in self.tmap['nodes']],[])))
+ for i, action in enumerate(actions):
+ self.map_markers.markers.append(self.get_legend_marker(action, i))
+
+ # Map
+ for node in self.tmap['nodes']:
+
+ # Nodes
+ self.map_markers.markers.append(self.get_node_marker(node)) # Node
+ self.map_markers.markers.append(self.get_name_marker(node)) # Name
+ self.map_markers.markers.append(self.get_zone_marker(node)) # Zone
+
+ # Edges
+ for edge in node['node']['edges']:
+ marker = self.get_edge_marker(node, edge, actions.index(edge['action']))
+ if marker:
+ self.map_markers.markers.append(marker)
+
+ # Publish
+ for i, marker in enumerate(self.map_markers.markers):
+ marker.id = i
+ self.topmap_pub.publish(self.map_markers)
+ self.get_logger().info('new map visual published')
+
+
+ def get_legend_marker(self, action, col_id):
+ col=self.get_colour(col_id)
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.TEXT_VIEW_FACING
+ marker.text=action
+ marker.pose.position.x= 1.0+(0.12*col_id)
+ marker.pose.position.y= 0.0
+ marker.pose.position.z= 0.2
+ marker.pose.orientation.w= 1.0
+ marker.scale.z = self.scale * 0.1
+ marker.color.a = 0.5
+ marker.color.r = float(col[0])
+ marker.color.g = float(col[1])
+ marker.color.b = float(col[2])
+ marker.ns='/legend'
+ return marker
+
+ def get_name_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.TEXT_VIEW_FACING
+ marker.text= node['node']['name']
+ marker.pose.position.x = node['node']['pose']['position']['x']
+ marker.pose.position.y = node['node']['pose']['position']['y']
+ marker.pose.position.z = node['node']['pose']['position']['z']
+ marker.scale.z = self.scale * 0.12
+ marker.color.a = 0.9
+ marker.color.r = 0.3
+ marker.color.g = 0.3
+ marker.color.b = 0.3
+ marker.ns='/names'
+ return marker
+
+ def get_edge_marker(self, node, edge, col_id):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.LINE_LIST
+ V1 = Point()
+ V1.x = node['node']['pose']['position']['x']
+ V1.y = node['node']['pose']['position']['y']
+ V1.z = node['node']['pose']['position']['z']
+ to_node = tmap_utils.get_node_from_tmap2(self.tmap, edge['node'])
+ col = self.get_colour(col_id)
+ if to_node:
+ V2 = Point()
+ V2.x = to_node['node']['pose']['position']['x']
+ V2.y = to_node['node']['pose']['position']['y']
+ V2.z = to_node['node']['pose']['position']['z']
+ marker.pose.orientation.w = 1.0
+ marker.scale.x = self.scale * 0.1
+ marker.color.a = 0.5
+ marker.color.r = float(col[0])
+ marker.color.g = float(col[1])
+ marker.color.b = float(col[2])
+ marker.points.append(V1)
+ marker.points.append(V2)
+ marker.ns='/edges'
+ return marker
+ else:
+ self.get_logger().warn("No node %s found" %edge['node'])
+ return None
+
+ def get_node_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = Marker.SPHERE
+ marker.scale.x = self.scale * 0.2
+ marker.scale.y = self.scale * 0.2
+ marker.scale.z = self.scale * 0.2
+ marker.color.a = 0.4
+ marker.color.r = 0.2
+ marker.color.g = 0.2
+ marker.color.b = 0.7
+ marker.pose.position.x = node['node']['pose']['position']['x']
+ marker.pose.position.y = node['node']['pose']['position']['y']
+ marker.pose.position.z = node['node']['pose']['position']['z'] + 0.2
+ marker.ns='/nodes'
+ return marker
+
+
+ def get_zone_marker(self, node):
+ marker = Marker()
+ marker.header.frame_id = "map"
+ marker.type = marker.LINE_STRIP
+ marker.pose.orientation.w = 1.0
+ marker.scale.x = self.scale * 0.1
+ marker.color.a = 0.8
+ marker.color.r = 0.7
+ marker.color.g = 0.1
+ marker.color.b = 0.2
+
+ for j in node['node']['verts']:
+ vert = Point()
+ vert.z = 0.05
+ vert.x = node['node']['pose']['position']['x'] + j['x']
+ vert.y = node['node']['pose']['position']['y'] + j['y']
+ marker.points.append(vert)
+
+ vert = Point()
+ vert.z = 0.05
+ vert.x = node['node']['pose']['position']['x'] + node['node']['verts'][0]['x']
+ vert.y = node['node']['pose']['position']['y'] + node['node']['verts'][0]['y']
+ marker.points.append(vert)
+ marker.ns='/zones'
+ return marker
+
+ def get_colour(self, number):
+ pallete=[[1,1,1],[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0],[1,0,1],[0,1,1]]
+ return pallete[number] if number < len(pallete) else pallete[0]
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ TV = TopologicalVis()
+ rclpy.spin(TV)
+
+ TV.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/topological_navigation_msgs/CMakeLists.txt b/topological_navigation_msgs/CMakeLists.txt
index 7e118ccb..a2f91039 100644
--- a/topological_navigation_msgs/CMakeLists.txt
+++ b/topological_navigation_msgs/CMakeLists.txt
@@ -36,6 +36,7 @@ set(msg_deps
# Declare the custom message files
set(msg_files
"msg/DistributionStamped.msg"
+ "msg/ExecutePolicyModeGoal.msg"
"msg/PoseObservation.msg"
"msg/LikelihoodObservation.msg"
"msg/ParticlesState.msg"
diff --git a/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg b/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg
new file mode 100644
index 00000000..19b5c7a1
--- /dev/null
+++ b/topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg
@@ -0,0 +1,2 @@
+#goal definition
+topological_navigation_msgs/NavRoute route