Merge pull request #162 from Iranaphor/GoalMsg_Testing

Development work toward a working ROS2 version

topological_navigation/package.xml

@@ -31,7 +31,8 @@
   <depend>visualization_msgs</depend>
   <depend>topological_navigation_msgs</depend>
 
-  <depend>tf2</depend>
+  <depend>python-transforms3d-pip</depend>
+  <depend>tf_transformations</depend>
 
   <export>
     <build_type>ament_python</build_type>

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'
         ],
     },
 

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()

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: [email protected]
+# @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 ([email protected])
-"""
-#########################################################################################################
-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()
-#########################################################################################################
+if __name__ == '__main__':
+    main()

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()
+
+

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