Merge pull request #126 from UOA-FSAE/cone_map_restructure_updated

Cone map restructure updated

interfaces.proto

@@ -35,21 +35,21 @@ message CANStamped {
 }
 
 message Cone {
-    uint32 id = 0; // ID value of cone (optional, defaults to 0 if not given)
 
     float32 confidence = 1; // The confidence that this is the cone, a percentage value from 0 to 100 (defaults to 0.0)
 
-    uint8 colour = 2; // The color of the cone: 0 for Blue, 1 for Orange, 2 for Yellow, and 3 for Other
+    uint8 type = 6; // The type of the cone: 0 for Blue, 1 for Yellow, 2 for Orange, 4 for Tall Orange, and 3 for Other
 
-    geometry_msgs.PoseWithCovariance pose = 3; // Position of cone (x, y, z) and rotation
+    geometry_msgs.Point position = 7;  // Position of cone (x, y, z)
 
     float32 radius = 4; // Size of cone, radius (should default to 0.0)
     float32 height = 5; // Size of cone, height (should default to 0.0)
 
 }
 // ? can this be renamed to Cones?
 message ConeMap { 
-    repeated Cone cones = 1; // List of cones detected and where they are     
+    repeated geometry_msgs.Point left_cones = 2; // Ordered list of cones on the left boundary 
+    repeated geometry_msgs.Point right_cones = 3; // Ordered list of cones on the right boundary
 }
 message ConeMapStamped {
     std_msgs.Header header = 1;

src/action/pure_pursuit/pure_pursuit/pure_pursuit.py

@@ -32,13 +32,14 @@ def __init__(self):
         # subscribe to best trajectory
         self.best_trajectory_sub = self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5)
         self.cone_map_sub = self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5)
+        self.create_subscription(Pose, "car_position", self.get_car_position, 5)
         self.cmd_vel_pub = self.create_publisher(AckermannDrive, "/drive", 5)
         self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5)
         self.track_point_pub = self.create_publisher(Pose, "moa/track_point", 5)
 
     def main_hearback(self, msg: ConeMap):
         # Update car's current location and update transformation matrix
-        self.car_pose = msg.cones[0].pose.pose
+        self.car_pose = self.car_position_pose
         self.position_vector, self.rotation_matrix_l2g, self.rotation_matrix_g2l = self.convert_to_transformation_matrix(self.car_pose.position.x, self.car_pose.position.y, self.car_pose.orientation.w)
 
         # Before proceed, check whether we have a trajectory input
@@ -206,6 +207,8 @@ def publish_ackermann(self):
         msg2 = AckermannDrive(**args2)
         self.cmd_vel_pub.publish(msg1)
         self.cmd_vis_pub.publish(msg2)
+
+    def get_car_position(self, msg:Pose): self.car_position_pose = msg
 
 def main(args=None):
     rclpy.init(args=args)

src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py

@@ -7,14 +7,17 @@
 from rclpy.executors import SingleThreadedExecutor
 from std_msgs.msg import Float32, Float64
 
-from geometry_msgs.msg import PoseArray
+from geometry_msgs.msg import PoseArray, Pose
 from moa_msgs.msg import ConeMap
+
 from std_msgs.msg import Header
 from builtin_interfaces.msg import Time
 import numpy as np
 
 class trajectory_following(Node):
     def __init__(self):
+
+        self._car_position: Pose
         super().__init__("Trajectory_Following")
         self.get_logger().info("Trajectory Following Node Started")
 
@@ -42,6 +45,7 @@ def __init__(self):
         self.create_subscription(PoseArray, "moa/selected_trajectory", self.get_desired_pose, qos_profile)
         self.create_subscription(Float32, "moa/selected_steering_angle", self.get_steering_angle, qos_profile)  
         self.create_subscription(ConeMap, "cone_map", self.callback, qos_profile)
+        self.create_subscription(ConeMap, "car_position", self.car_pos_cb)
 
         # publishers (including simulation)
         self.moa_steering_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 10)
@@ -52,7 +56,9 @@ def get_steering_angle(self, msg:Float32): self._steering_angle = msg.data  # ra
 
     def get_desired_pose(self, msg:PoseArray): self._desired_pose = msg.poses[-1]
 
-    # def get_car_position(self, msg:ConeMap): self._car_position = msg.cones[0].pose.pose.position
+    def get_car_position(self, pose:Pose): self._car_position = pose
+
+
 
     def callback(self, msg:ConeMap):
         if hasattr(self, "_steering_angle"):

src/moa/moa_msgs/msg/Cone.msg

@@ -0,0 +1,5 @@
+float32 confidence 
+uint8 type 
+geometry_msgs/Point position 
+float32 radius 
+float32 height 

src/moa/moa_msgs/msg/ConeMap.msg

@@ -0,0 +1,2 @@
+geometry_msgs/Point[] left_cones 
+geometry_msgs/Point[] right_cones 

src/perception/aruco_detection/aruco_detection/aruco_detection.py → src/perception/aquisition/aruco_detection/aruco_detection/aruco_detection.py

@@ -9,8 +9,7 @@
 import cv2.aruco as aruco
 import numpy as np
 
-from moa_msgs.msg import ConeMap
-from moa_msgs.msg import Cone
+from moa_msgs.msg import Cones, Cone
 
 
 class ArucoDetectionNode(Node):
@@ -21,7 +20,7 @@ def __init__(self):
         self.create_subscription(CameraInfo,'zed/zed_node/rgb/camera_info',self.camera_callback,10)
         self.create_subscription(PoseStamped,'zed/zed_node/pose',self.pose_callback,10)
         self.create_subscription(Image,'/zed/zed_node/rgb/image_rect_color',self.image_callback,10)
-        self.publisher = self.create_publisher(ConeMap, 'cone_detection', 10)
+        self.publisher = self.create_publisher(Cones, 'cone_detection', 10)
         self.localization_publisher = self.create_publisher(Pose, 'car_position', 5)
 
         self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
@@ -55,38 +54,33 @@ def image_callback(self, msg):
                         idc = id[0]
                         _,rvec,tvec = cv2.solvePnP(self.marker_points,corners[indx],self.camera_matrix,self.dist_coeffs)
 
-                        single_cone.id = indx
                         single_cone.confidence = 100.0
                         # assuming id 1 is blue cone and id 2 is yellow cone
-                        single_cone.colour = 0 if idc==1 else 2 if idc==2 else 3
+                        single_cone.type = 0 if idc==1 else 1 if idc==2 else 4
 
                         # x-axis is forward/backward
-                        single_cone.pose.pose.position.x = float(tvec[0][0])
+                        single_cone.position.x = float(tvec[0][0])
                         # y-axis is left/right
-                        single_cone.pose.pose.position.y = float(tvec[2][0])
+                        single_cone.position.y = float(tvec[2][0])
                         # z-axis is up/down
-                        single_cone.pose.pose.position.z = float(tvec[1][0])
+                        single_cone.position.z = float(tvec[1][0])
                         single_cone.radius = 1.0
                         single_cone.height = 1.0
                         # Only sending blue cone and yellow cone
-                        if single_cone.colour == 0 or single_cone.colour == 2:
+                        if single_cone.type == 0 or single_cone.type == 1:
                             self.aruco_msg.cones.append(single_cone)
                     #self.get_logger().info("POTENTIAL MARKERS DETECTED!!")
 
                 self.publisher.publish(self.aruco_msg)
                 self.edit_msg = False
 
     def pose_callback(self,msg):
-        self.aruco_msg = ConeMap()
-        localization_cone = Cone()
+        self.aruco_msg = Cones()
         localization_pose = Pose()
-        localization_cone.id = 99999;
         euler = self.convert_quaternion_to_euler(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
         localization_pose.position.x = -msg.pose.position.y
         localization_pose.position.y = msg.pose.position.x
         localization_pose.orientation.w = euler[1]
-        localization_cone.pose.pose = localization_pose
-        self.aruco_msg.cones.append(localization_cone)
         self.edit_msg = True
         self.localization_publisher.publish(localization_pose)
 

src/perception/sythesis/cone_mapping/cone_mapping/cone_map_collection.py

@@ -0,0 +1,50 @@
+from moa_msgs.msg import Cone
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Pose
+
+import math
+
+class ConeMapList(list):
+    def __init__(self):
+        super().__init__(self)
+
+    def insert_cone(self, car_pose, cone_coord):
+        car_coord = car_pose.position.x, car_pose.position.y
+        # Insert if first cone detected
+        if len(self) == 0:
+            self.append(cone_coord)
+            return
+
+        #
+        if len(self) == 1:
+            coord0 = self[0].x, self[0].y
+            if self.distance(car_coord, cone_coord) < self.distance(car_coord, coord0):
+                self.insert(0, cone_coord)
+                return
+            else:    
+                self.append(cone_coord)
+                return
+
+        for i in range(len(self) - 1):
+            coordi = self[i].position.x, self[i].position.y
+            coordi1 = self[i+1].position.x, self[i+1].position.y
+            if self.distance(coordi, cone_coord) < self.distance(coordi, coordi1):
+                self.insert(i, cone_coord)
+                return
+
+        self.append(cone_coord)  
+
+    def distance(self, coord1, coord2):
+        return  math.sqrt((coord1[0].x - coord2[0].x)**2 + (coord1[1].y - coord2[1].y)**2)
+
+
+
+class ConeWithId(Cone):
+    def __init__(self, id):
+        super().__init__(self)
+
+
+
+
+cone = ConeWithId(cone, id)

src/perception/cone_mapping/cone_mapping/cone_mapping/mapper_DBSCAN.py → src/perception/sythesis/cone_mapping/cone_mapping/mapper_DBSCAN.py

@@ -3,8 +3,8 @@
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
 
-from std_msgs.msg import String
 from moa_msgs.msg import ConeMap
+from moa_msgs.msg import Cones
 from moa_msgs.msg import Cone
 from geometry_msgs.msg import Point
 from geometry_msgs.msg import Quaternion
@@ -23,8 +23,9 @@ class Cone_Mapper(Node):
     def __init__(self):
         super().__init__('cone_mapper')
         qos_profile = QoSProfile(reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10)
-        self.subscription = self.create_subscription(ConeMap, 'cone_detection', self.listener_callback, qos_profile)
-        self.publisher = self.create_publisher(ConeMap, 'cone_map', 10)
+        self.subscription = self.create_subscription(Cones, 'cone_detection', self.cones_callback, qos_profile)
+        self.subscription = self.create_subscription(Pose, 'car_position', self.car_position_callback, 10)
+        self.publisher = self.create_publisher(ConeMap, 'cone_map', 5)
 
         # Clustering related tuning parameter
         self.default_standard_deviation = 0.5 # Also minimal standard deviation
@@ -35,7 +36,7 @@ def __init__(self):
         # Clustering method initialization
         self.most_updated_cone_map = ConeMap()
         self.number_of_measurements_for_cones = []
-        self.car_position = Pose()
+        self.car_positions = []
         self.cone_id = 1
 
         # Cone deletion tune
@@ -50,16 +51,22 @@ def __init__(self):
 
         self.get_logger().info("Cone Map Initialization Completed")
 
-    def listener_callback(self, msg):
-        msg_in_local_coordinate = msg
-        msg_in_global_coordinate = self.transform_raw_input_to_global_coordinate(msg_in_local_coordinate)
-        self.most_updated_cone_map = self.clustering_update(msg_in_global_coordinate)
+    def car_position_callback(self, msg: Pose):
+        self.car_positions.append(msg)
+
+    def cones_callback(self, msg: Cones):
+        pose_in_local_coordinate = msg
+        while len(self.car_positions) == 0:
+            pass
+        car_position = self.car_positions.pop(0)
+        pose_in_global_coordinate = self.transform_raw_input_to_global_coordinate(pose_in_local_coordinate, car_position)
+        self.most_updated_cone_map = self.clustering_update(pose_in_global_coordinate)
         self.publisher.publish(self.most_updated_cone_map)
         #self.get_logger().info("Cone Map Published")
 
 # Conversion to Global Coordinate
 ## Main
-    def transform_raw_input_to_global_coordinate(self, msg : ConeMap):
+    def transform_raw_input_to_global_coordinate(self, cones: Cones, car_position: Pose):
         """Extract measurement state from the Cone Map message subscription
 
         Args:
@@ -72,7 +79,7 @@ def transform_raw_input_to_global_coordinate(self, msg : ConeMap):
             None
         """
         # Convert Cone Map message into position (x and y), orientation (theta) and list of cones
-        x, y, theta, list_of_cones_in_local_coordinates = self.convert_message_to_data(msg)
+        x, y, theta, list_of_cones_in_local_coordinates = self.convert_message_to_data(cones, car_position)
         # Use list of cones and states (x, y and theta) to get the position vector and rotation matrix
         position_vector, rotation_matrix_local_to_global = self.get_coordinate_conversion_matrices(x, y, theta)
         # Conversion from local reference frame to global reference frame
@@ -100,7 +107,7 @@ def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -
         Raises:
             None
         '''
-        position_vector = np.array([[x], [y], [0]]);
+        position_vector = np.array([[x], [y], [0]])
         rotation_matrix_local_to_global = np.array(
             [[math.cos(theta), -math.sin(theta), 0], [math.sin(theta), math.cos(theta), 0], [0, 0, 1]])  # Inverse DCM
 
@@ -109,7 +116,7 @@ def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -
     def get_list_of_cones_in_global_coordinate(self, position_vector : np.array, rotation_matrix : np.array, list_of_cones : np.array) -> np.array:
         list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones)
         list_of_cones_output = list_of_cones_unrotated + position_vector
-        return list_of_cones_output;
+        return list_of_cones_output
 
 # Clustering Method
 ## Main
@@ -184,7 +191,7 @@ def measurement_is_not_new_cone(self, distance, recorded_cone: Cone):
         return distance <= self.new_cone_if_n_sigma_exceed_this * criterion_standard_deviation
 
 ## Utility Functions for Clustering
-    def convert_message_to_data(self, data: ConeMap) -> (float, float, float, np.array):
+    def convert_message_to_data(self, cones: Cones, car_position: Pose) -> (float, float, float, np.array):
         """Convert cone map message into useable data
 
         Args:
@@ -201,10 +208,10 @@ def convert_message_to_data(self, data: ConeMap) -> (float, float, float, np.arr
 
         """
         # Convert from Cone Map to data that is processed in the node
-        list_of_cones = data.cones[1::1];
+        list_of_cones = cones.cones
 
         # Extract the first cone in cone map as the cart localization data
-        cart_info = data.cones[0];
+        cart_info = car_position
 
         # Convert cart info to readable datas for cart
         x, y, theta, covariance, color, _ = self.extract_data_from_cone(cart_info)
@@ -225,7 +232,7 @@ def convert_message_to_data(self, data: ConeMap) -> (float, float, float, np.arr
 
         return x, y, theta, list_of_cones
 
-    def extract_data_from_cone(self, cone_input : Cone):
+    def extract_data_from_cone(self, cone_input: Cone):
         # For later process: We need to use quaternion orientation
         x = cone_input.pose.pose.position.x
         y = cone_input.pose.pose.position.y
@@ -234,6 +241,14 @@ def extract_data_from_cone(self, cone_input : Cone):
         color = cone_input.colour
         cone_id = cone_input.id
         return x, y, theta, covariance, color, cone_id
+
+    def extract_data_from_car(self, car_input: Pose):
+        x = car_input.position.x
+        y = car_input.position.y
+        theta = car_input.orientation.w
+        return x, y, theta
+
+
 
 # Delete non-existing cones
     def cone_deletion_callback(self):
@@ -298,12 +313,12 @@ def produce_cone_map_message(self, x : float, y : float, theta : float, list_of_
         list_of_cones_color = list_of_cones[2]
         length = len(list_of_cones_x)
         for index in range(length):
-            cone_input = self.pack_cone_message(list_of_cones_x[index], list_of_cones_y[index], 0.0, index + 1, self.default_covariance, int(list_of_cones_color[index]));
-            output_map.cones.append(cone_input)
+            cone_out = self.pack_cone_message(list_of_cones_x[index], list_of_cones_y[index], 0.0, index + 1, self.default_covariance, int(list_of_cones_color[index]));
+            output_map.cones.append(cone_out)
         return output_map
 
     def pack_cone_message(self, x : float, y : float, theta : float, cone_id : int, covariance_vector, color : int) -> Cone:
-        output_cone = Cone()
+        output_cone = Pose()
         position = Point()
         orientation = Quaternion()
         pose_with_covariance = PoseWithCovariance()
@@ -315,10 +330,9 @@ def pack_cone_message(self, x : float, y : float, theta : float, cone_id : int,
         pose.orientation = orientation
         pose_with_covariance.pose = pose
         pose_with_covariance.covariance = covariance_vector
-        output_cone.pose = pose_with_covariance
-        output_cone.id = cone_id
-        output_cone.colour = color
-        return output_cone
+        output_cone_pose = pose_with_covariance.pose
+        handedness = 1 if color == 1 else 0  
+        return (output_cone_pose, handedness)
 
     def distance_between_two_cones(self, cone_1, cone_2):
         x1, y1, _, _, _, _ = self.extract_data_from_cone(cone_1)