From 1c0c7666a31b2456c3588df2b0c6d84fd24a2d47 Mon Sep 17 00:00:00 2001 From: JamesH Date: Wed, 26 Apr 2023 10:20:04 +0100 Subject: [PATCH 1/7] inclusion of goal msg so as to echo ~/goal --- topological_navigation_msgs/CMakeLists.txt | 1 + topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg 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 From e0d7f5ce379664ca9b0ada28c851bdbb2756a0f0 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Wed, 24 May 2023 21:40:16 +0200 Subject: [PATCH 2/7] topological map visualiser and policy mode visualisers updated to ros2 --- topological_navigation/package.xml | 3 +- topological_navigation/setup.py | 5 +- .../topological_navigation/policy_marker.py | 110 +++++++++++ .../scripts/visualise_map_ros2.py | 29 +++ .../topological_navigation/topomap_marker.py | 182 ++++++++++++++++++ 5 files changed, 327 insertions(+), 2 deletions(-) create mode 100644 topological_navigation/topological_navigation/policy_marker.py create mode 100755 topological_navigation/topological_navigation/scripts/visualise_map_ros2.py create mode 100644 topological_navigation/topological_navigation/topomap_marker.py 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..79fa3162 100644 --- a/topological_navigation/setup.py +++ b/topological_navigation/setup.py @@ -39,7 +39,10 @@ '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', + '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/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/topomap_marker.py b/topological_navigation/topological_navigation/topomap_marker.py new file mode 100644 index 00000000..b9de03ee --- /dev/null +++ b/topological_navigation/topological_navigation/topomap_marker.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy + +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') + self.map = None + self.map_markers = MarkerArray() + self.topmap_pub = self.create_publisher(MarkerArray, '~/vis', 2) + self.topmap_pub.publish(self.map_markers) + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.map_sub = self.create_subscription(TopologicalMap, '/topological_map', self.map_callback, qos) + self.get_logger().info('map visualiser init complete') + + def map_callback(self, msg): + print(dir(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) + + + 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 = 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 = 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 = 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 = 0.2 + marker.scale.y = 0.2 + marker.scale.z = 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 = 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() From 5d696a4e85eb85b67670d7e10726a9ee8527c200 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Thu, 22 Jun 2023 18:49:26 +0200 Subject: [PATCH 3/7] topological transform publisher --- .../topological_transform_publisher.py | 93 ++++++++----------- 1 file changed, 41 insertions(+), 52 deletions(-) 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() From 26868a3bb7ebd91858f6f7176600258971e2c1fa Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Wed, 16 Aug 2023 08:38:49 +0100 Subject: [PATCH 4/7] topovis scaling included for larger map visualsing from a distance --- .../topological_navigation/topomap_marker.py | 34 ++++++++++++++----- 1 file changed, 25 insertions(+), 9 deletions(-) diff --git a/topological_navigation/topological_navigation/topomap_marker.py b/topological_navigation/topological_navigation/topomap_marker.py index b9de03ee..0d4630d9 100644 --- a/topological_navigation/topological_navigation/topomap_marker.py +++ b/topological_navigation/topological_navigation/topomap_marker.py @@ -4,6 +4,7 @@ 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 @@ -15,16 +16,30 @@ class TopologicalVis(Node): def __init__(self): super().__init__('topological_map_markers') - self.map = None + 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) - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + + # 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 = int(msg.data) + self.map_callback(self.map) + def map_callback(self, msg): - print(dir(msg)) self.get_logger().info('map callback triggered') self.get_logger().info(f'name: {msg.name}') self.get_logger().info(f'map: {msg.map}') @@ -62,6 +77,7 @@ def map_callback(self, msg): 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): @@ -74,7 +90,7 @@ def get_legend_marker(self, action, col_id): marker.pose.position.y= 0.0 marker.pose.position.z= 0.2 marker.pose.orientation.w= 1.0 - marker.scale.z = 0.1 + marker.scale.z = self.scale * 0.1 marker.color.a = 0.5 marker.color.r = float(col[0]) marker.color.g = float(col[1]) @@ -88,7 +104,7 @@ def get_name_marker(self, node): marker.type = marker.TEXT_VIEW_FACING marker.text= node.name marker.pose = node.pose - marker.scale.z = 0.12 + marker.scale.z = self.scale * 0.12 marker.color.a = 0.9 marker.color.r = 0.3 marker.color.g = 0.3 @@ -108,7 +124,7 @@ def get_edge_marker(self, node, edge, col_id): if to_node: V2= to_node.pose.position marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 + marker.scale.x = self.scale * 0.1 marker.color.a = 0.5 marker.color.r = float(col[0]) marker.color.g = float(col[1]) @@ -125,9 +141,9 @@ def get_node_marker(self, node): marker = Marker() marker.header.frame_id = "map" marker.type = Marker.SPHERE - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 + 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 From 82949b804574bb30f7077455464222a1311af4e8 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Fri, 18 Aug 2023 09:54:23 +0100 Subject: [PATCH 5/7] updates to scalling for floats --- .../topological_navigation/topomap_marker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/topological_navigation/topological_navigation/topomap_marker.py b/topological_navigation/topological_navigation/topomap_marker.py index 0d4630d9..25d62253 100644 --- a/topological_navigation/topological_navigation/topomap_marker.py +++ b/topological_navigation/topological_navigation/topomap_marker.py @@ -36,7 +36,7 @@ def __init__(self): def rescale_callback(self, msg): self.get_logger().info('new scale recieved') self.get_logger().info(f'scale: {msg.data}') - self.scale = int(msg.data) + self.scale = float(msg.data) self.map_callback(self.map) def map_callback(self, msg): @@ -159,7 +159,7 @@ def get_zone_marker(self, node): marker.header.frame_id = "map" marker.type = marker.LINE_STRIP marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 + marker.scale.x = self.scale * 0.1 marker.color.a = 0.8 marker.color.r = 0.7 marker.color.g = 0.1 From aab6de31b19c0f84c24bf90f0ad564d46f78b0f5 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Mon, 21 Aug 2023 23:02:21 +0000 Subject: [PATCH 6/7] incorrect tab removed --- topological_navigation/topological_navigation/testing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 51a0241cc54f28e2fdadf3f483da2ed56b0dd475 Mon Sep 17 00:00:00 2001 From: James R Heselden Date: Tue, 3 Oct 2023 16:24:24 +0100 Subject: [PATCH 7/7] inclusion of topomap_marker2.py with functional rescaling --- topological_navigation/setup.py | 1 + .../topological_navigation/topomap_marker2.py | 214 ++++++++++++++++++ 2 files changed, 215 insertions(+) create mode 100644 topological_navigation/topological_navigation/topomap_marker2.py diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py index 79fa3162..36f090bf 100644 --- a/topological_navigation/setup.py +++ b/topological_navigation/setup.py @@ -42,6 +42,7 @@ '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/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()