From a85db5085cdb29f1a227f310480b8cf403efd3b5 Mon Sep 17 00:00:00 2001 From: fsae Date: Sat, 3 Jan 1970 04:34:59 +1200 Subject: [PATCH 01/14] added svo2 to gitignore - someone did not commit! --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index ada32d3f..f92d86f2 100644 --- a/.gitignore +++ b/.gitignore @@ -14,4 +14,4 @@ kaggle.json wandb/ *.engine *.onnx -*.svo2 \ No newline at end of file +*.svo2 \ No newline at end of file From 9a961bda06f39e173728ce853f4cad3187ed423f Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Thu, 26 Sep 2024 20:16:12 +1200 Subject: [PATCH 02/14] cone mapping modified for pang --- .../cone_detection/cone_detection.cpp | 4 +- .../zed_launch/zed_launch/zed_launch.hpp | 2 +- .../cone_mapping/kalman_filter.py | 45 ++++++++++++++----- 3 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp index 778baa73..5fa55332 100644 --- a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp @@ -15,7 +15,7 @@ #include using namespace nvinfer1; -#define NMS_THRESH 0.4 +#define NMS_THRESH 0.7 #define CONF_THRESH 0.8 #include @@ -84,7 +84,9 @@ void ZedLaunchNode::cone_detection_loop() auto camera_info = zed.getCameraInformation(pc_resolution).camera_configuration; // Creating the inference engine class + // std::string engine_name = "cone_detection_yolo10l.engine"; std::string engine_name = "cone_detection_model.engine"; + // std::string engine_name = "cone_detection_yolo9c.engine"; Yolo detector; if (detector.init(engine_name)) { std::cerr << "Detector init failed!" << std::endl; diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp index 334d20ba..8b0b0614 100644 --- a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp @@ -41,7 +41,7 @@ class ZedLaunchNode : public rclcpp::Node sl::Camera& zed; sl::Pose cam_w_pose; bool camera_running = true; - bool visualisation = false; + bool visualisation = true; // Publishers rclcpp::Publisher::SharedPtr cone_detection_publisher; diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py index 60b436a6..2aa93729 100644 --- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -7,6 +7,8 @@ import math import numpy as np +import time +import threading import kdtree @@ -55,6 +57,12 @@ def __init__(self): self.left_tree = None self.right_tree = None + self.mtx = threading.Lock() + + # Thread to remove points + # self.remove_point_thread = threading.Thread(target=self.remove_point_thread) + # self.remove_point_thread.start() + ################################################################################ (parameters to tune) # Initial error in the estimate @@ -111,8 +119,9 @@ def update_left_track(self, points: list) -> None: # If the left track is empty, pack all cone measurements and update the left track self.left_track = self.produce_track_message(points) # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) + times_modified = 0 self.left_tree = kdtree.create( - [Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)], dimensions=2) # For 2D points + [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) # For 2D points else: # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track for coord in points: @@ -121,19 +130,20 @@ def update_left_track(self, points: list) -> None: # kdtree.visualize(self.left_tree) # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates if distance <= self.match_radius: - index = point.data.data[0] - cone = self.left_track.cones[index] + cone = point.data.data[0] error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] + 1 new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) - point.data.data = (index, new_error_in_estimate) + point.data.data = (cone, new_error_in_estimate, times_modified) else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map cone = Point() cone.x = coord[0] cone.y = coord[1] self.left_track.cones.append(cone) - self.left_tree.add(Item(coord[0], coord[1], (len(self.left_track.cones) - 1, self.default_error_in_estimate))) + times_modified = 0 + self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) @@ -146,7 +156,8 @@ def update_right_track(self, points: list) -> None: if self.right_track == None: # If the right track is empty, pack all cone measurements and update the right track self.right_track = self.produce_track_message(points) - self.right_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)], dimensions=2) + times_modified = 0 + self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) else: # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track for coord in points: @@ -156,19 +167,20 @@ def update_right_track(self, points: list) -> None: # check if distance is nan # kdtree.visualize(self.right_tree) if distance <= self.match_radius: - index = point.data.data[0] - cone = self.right_track.cones[index] + cone = point.data.data[0] error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] + 1 new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) - point.data.data = (index, new_error_in_estimate) + point.data.data = (cone, new_error_in_estimate, times_modified) else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map cone = Point() cone.x = coord[0] cone.y = coord[1] self.right_track.cones.append(cone) - self.right_tree.add(Item(coord[0], coord[1], (len(self.right_track.cones) - 1, self.default_error_in_estimate))) + times_modified = 0 + self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: @@ -320,6 +332,19 @@ def produce_track_message(self, points: list) -> Track: output_track.cones.append(point) return output_track + def remove_point_thread(self): + while(self.left_tree == None): + time.sleep(1) + + while True: + for point in self.left_tree: + if point.data.data[2] < 100: + self.mtx.acquire + self.left_track.remove(point.data.data[0]) + self.left_tree.remove(point) + self.mtx.release + time.sleep(5) + def main(args=None): rclpy.init(args=args) From b7dddfe64b1da1d6d12771553cec5d399566ccab Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Fri, 27 Sep 2024 13:40:28 +1200 Subject: [PATCH 03/14] Added deleter for cone_map --- scripts/run_zed_launch.sh | 2 +- .../cone_detection/cone_detection.cpp | 6 +- .../zed_launch/zed_launch/zed_launch.hpp | 3 + .../cone_mapping/kalman_filter.py | 77 +++++++++++++------ 4 files changed, 58 insertions(+), 30 deletions(-) diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh index 7bdb5ce7..89a1e728 100644 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,3 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording2.svo2 \ No newline at end of file +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp index 5fa55332..ec433214 100644 --- a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp @@ -15,8 +15,6 @@ #include using namespace nvinfer1; -#define NMS_THRESH 0.7 -#define CONF_THRESH 0.8 #include @@ -84,9 +82,7 @@ void ZedLaunchNode::cone_detection_loop() auto camera_info = zed.getCameraInformation(pc_resolution).camera_configuration; // Creating the inference engine class - // std::string engine_name = "cone_detection_yolo10l.engine"; - std::string engine_name = "cone_detection_model.engine"; - // std::string engine_name = "cone_detection_yolo9c.engine"; + std::string engine_name = model_name; Yolo detector; if (detector.init(engine_name)) { std::cerr << "Detector init failed!" << std::endl; diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp index 8b0b0614..b382b77d 100644 --- a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp @@ -42,6 +42,9 @@ class ZedLaunchNode : public rclcpp::Node sl::Pose cam_w_pose; bool camera_running = true; bool visualisation = true; + std::string model_name = "cone_detection_model.engine"; + float CONF_THRESH = 0.8; + float NMS_THRESH = 0.5; // Publishers rclcpp::Publisher::SharedPtr cone_detection_publisher; diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py index 2aa93729..5f058180 100644 --- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -2,6 +2,7 @@ from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from std_msgs.msg import Float32MultiArray from moa_msgs.msg import Detections, Track from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance @@ -49,6 +50,9 @@ def __init__(self): # Create right track publisher self.right_track_publisher = self.create_publisher(Track, 'right_track', 10) + # times_modified publisher + self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10) + # Existing cone map self.left_track = None self.right_track = None @@ -57,11 +61,10 @@ def __init__(self): self.left_tree = None self.right_tree = None - self.mtx = threading.Lock() + self.times_modified_counter = 0 - # Thread to remove points - # self.remove_point_thread = threading.Thread(target=self.remove_point_thread) - # self.remove_point_thread.start() + # For two recordings because its glitchy at begginning + time.sleep(10) ################################################################################ (parameters to tune) @@ -74,6 +77,15 @@ def __init__(self): # Cone match radius self.match_radius = 1.0 + # Counter to remove points + self.remove_point_counter = 200 + + # value for the times_modified + self.times_modified_limit = 300000 + + # Modify rate + self.modify_rate = 1.08 + ################################################################################ (parameters to tune) def cones_callback(self, msg: Detections) -> None: @@ -88,7 +100,12 @@ def cones_callback(self, msg: Detections) -> None: # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic # print("Left track length: ", len(self.left_track.cones)) - # print("Right track length: ", len(self.right_track.cones)) + # print("Right track length: ", len(self.right_track.cones)) + self.times_modified_counter = self.times_modified_counter + 1 + if self.times_modified_counter > self.remove_point_counter: + self.times_modified_counter = 0 + self.remove_points() + self.left_track_publisher.publish(self.left_track) self.right_track_publisher.publish(self.right_track) @@ -119,7 +136,7 @@ def update_left_track(self, points: list) -> None: # If the left track is empty, pack all cone measurements and update the left track self.left_track = self.produce_track_message(points) # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) - times_modified = 0 + times_modified = 1 self.left_tree = kdtree.create( [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) # For 2D points else: @@ -132,17 +149,16 @@ def update_left_track(self, points: list) -> None: if distance <= self.match_radius: cone = point.data.data[0] error_in_estimate = point.data.data[1] - times_modified = point.data.data[2] + 1 + times_modified = point.data.data[2] * self.modify_rate new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) point.data.data = (cone, new_error_in_estimate, times_modified) - else: - # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map cone = Point() cone.x = coord[0] cone.y = coord[1] self.left_track.cones.append(cone) - times_modified = 0 + times_modified = 1 self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) @@ -156,7 +172,7 @@ def update_right_track(self, points: list) -> None: if self.right_track == None: # If the right track is empty, pack all cone measurements and update the right track self.right_track = self.produce_track_message(points) - times_modified = 0 + times_modified = 1 self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) else: # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track @@ -169,7 +185,7 @@ def update_right_track(self, points: list) -> None: if distance <= self.match_radius: cone = point.data.data[0] error_in_estimate = point.data.data[1] - times_modified = point.data.data[2] + 1 + times_modified = point.data.data[2] * self.modify_rate new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) point.data.data = (cone, new_error_in_estimate, times_modified) @@ -179,7 +195,7 @@ def update_right_track(self, points: list) -> None: cone.x = coord[0] cone.y = coord[1] self.right_track.cones.append(cone) - times_modified = 0 + times_modified = 1 self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) @@ -332,18 +348,31 @@ def produce_track_message(self, points: list) -> Track: output_track.cones.append(point) return output_track - def remove_point_thread(self): - while(self.left_tree == None): - time.sleep(1) + def remove_points(self): + if self.left_track is None or self.right_track is None: + return + + times_modified_list = Float32MultiArray() - while True: - for point in self.left_tree: - if point.data.data[2] < 100: - self.mtx.acquire - self.left_track.remove(point.data.data[0]) - self.left_tree.remove(point) - self.mtx.release - time.sleep(5) + left_tree = self.left_tree + right_tree = self.right_tree + for point in kdtree.level_order(left_tree): + # print(point) + times_modified_list.data.append(point.data.data[2]) + if point.data.data[2] < self.times_modified_limit: + self.left_track.cones.remove(point.data.data[0]) + self.left_tree.remove(point.data) + for point in kdtree.level_order(right_tree): + times_modified_list.data.append(point.data.data[2]) + # print(point) + if point.data.data[2] < self.times_modified_limit: + self.right_track.cones.remove(point.data.data[0]) + self.right_tree.remove(point.data) + + self.times_modified_publisher.publish(times_modified_list) + + # kdtree.visualize(self.left_tree) + # kdtree.visualize(self.right_tree) def main(args=None): From 184369f4b624ccc3b98b1cb8b5b4dc4374bf90d5 Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Sat, 28 Sep 2024 17:23:00 +1200 Subject: [PATCH 04/14] cone deleter sucks --- .../cone_detection/cone_subscriber.cpp | 6 +- .../cone_mapping/kalman_filter copy.py | 389 ++++++++++++++++++ 2 files changed, 394 insertions(+), 1 deletion(-) create mode 100644 src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp index 1bdedcea..78a8258d 100644 --- a/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp @@ -35,7 +35,11 @@ class ConeSubscriber : public rclcpp::Node private: void cone_detection_callback(const moa_msgs::msg::Detections & msg) const { - std::cout << "detections: " << msg.car_pose.position.x << " " << msg.car_pose.position.y << " " << msg.car_pose.position.z << " " << std::endl; + for (auto i = 0u; i < msg.blue.size(); i++) + { + float distance = sqrt(pow(msg.blue[i].x, 2) + pow(msg.blue[i].y, 2)); + std::cout << "blue: " << i << " " << msg.blue[i].x << " " << msg.blue[i].y << " " << distance << std::endl; + } } rclcpp::Subscription::SharedPtr subscription_; }; diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py new file mode 100644 index 00000000..8476729d --- /dev/null +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py @@ -0,0 +1,389 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from std_msgs.msg import Float32MultiArray +from moa_msgs.msg import Detections, Track +from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance + +import math +import numpy as np +import time +import threading + +import kdtree + +class Item(object): + def __init__(self, x, y, data): + self.coords = (x, y) + self.data = data + + def __len__(self): + return len(self.coords) + + def __getitem__(self, i): + return self.coords[i] + + def __repr__(self): + return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.data) + + +class Cone_Mapper(Node): + def __init__(self): + super().__init__('cone_mapper') + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # Create cone detection subscriber + self.cones_subscription = self.create_subscription( + Detections, + 'cone_detection', + self.cones_callback, + qos_profile) + + # Create left track publisher + self.left_track_publisher = self.create_publisher(Track, 'left_track', 10) + + # Create right track publisher + self.right_track_publisher = self.create_publisher(Track, 'right_track', 10) + + # times_modified publisher + self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10) + + # Existing cone map + self.left_track = Track() + self.right_track = Track() + + self.left_track.cones = [] + self.right_track.cones = [] + + # KDTrees for searching + self.left_tree = None + self.right_tree = None + + self.times_modified_counter = 0 + + # recording sleep time + time.sleep(12) + +################################################################################ (parameters to tune) + + # Initial error in the estimate + self.default_error_in_estimate = 4.0 + + # Error in measurement + self.error_in_measurement = 2.5 + + # Cone match radius + self.match_radius = 1.0 + + # Counter to remove points + self.remove_point_counter = 200 + + # value for the times_modified + self.times_modified_limit = 300000 + + # Modify rate + self.modify_rate = 1.1 + +################################################################################ (parameters to tune) + + def cones_callback(self, msg: Detections) -> None: + """This function updates the existing cone map with newly detected cones and + publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic. + + Args: + msg (Detections): car's position and input cones from the /cone_detection topic + """ + # Update the existing cone map with new measurements + self.update_existing_cone_map(msg) + + # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic + # print("Left track length: ", len(self.left_track.cones)) + # print("Right track length: ", len(self.right_track.cones)) + self.times_modified_counter = self.times_modified_counter + 1 + if self.times_modified_counter > self.remove_point_counter: + self.times_modified_counter = 0 + self.remove_points() + + self.left_track_publisher.publish(self.left_track) + self.right_track_publisher.publish(self.right_track) + + + def update_existing_cone_map(self, msg: Detections) -> None: + """This function updates the existing cone map using new cone measurementsf + + Args: + msg (Detections): Contains the car's position and lists of detected cones + """ + # Get all cones' global positions + car_position = msg.car_pose + global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position) + global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position) + + # Update the left track and the right track + self.update_left_track(global_blue_cone_positions) + self.update_right_track(global_yellow_cone_positions) + + + def update_left_track(self, points: list) -> None: + """This function updates the left track of the cone map + + Args: + points (list): A list of blue cones' global (x, y) coordinates + """ + if self.left_track.cones == []: + # If the left track is empty, pack all cone measurements and update the left track + self.left_track = self.produce_track_message(points) + # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) + times_modified = 1 + self.left_tree = kdtree.create( + [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) # For 2D points + else: + # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.left_tree.search_nn(coord) + # print(self.left_tree.search_nn(coord)) + # kdtree.visualize(self.left_tree) + # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data = (cone, new_error_in_estimate, times_modified) + else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.left_track.cones.append(cone) + times_modified = 1 + self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + + + + def update_right_track(self, points: list) -> None: + """This function updates the right track of the cone map + + Args: + points (list): A list of yellow cones' (x, y) coordinates + """ + if self.right_track.cones == []: + # If the right track is empty, pack all cone measurements and update the right track + self.right_track = self.produce_track_message(points) + times_modified = 1 + self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) + else: + # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track + for coord in points: + point, distance = self.right_tree.search_nn(coord) + # print(self.right_tree.search_nn(coord)) + # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates + # check if distance is nan + # kdtree.visualize(self.right_tree) + if distance <= self.match_radius: + cone = point.data.data[0] + error_in_estimate = point.data.data[1] + times_modified = point.data.data[2] * self.modify_rate + new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) + point.data.coords = (new_estimate_x, new_estimate_y) + point.data.data = (cone, new_error_in_estimate, times_modified) + else: + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + cone = Point() + cone.x = coord[0] + cone.y = coord[1] + self.right_track.cones.append(cone) + times_modified = 1 + self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + + + def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: + """This function update the x and y coordinate of of the input cone using kalman filtering + + Args: + cone (Point): input cone's location + coord (tuple): new measurement of (x, y) of the input cone + error_in_estimate (float): error in estimate of the cone's actual position + + Returns: + new_estimate_x (float): cone's new estimated x coordinate + new_estimate_y (float): cone's new estimated y coordinate + new_error_in_estimate (float): cone's new error in estimate + """ + # Get the previous error_in_estimate, estimated x and estimated y coordinate + prev_error_in_estimate = error_in_estimate + prev_estimate_x = cone.x + prev_estimate_y = cone.y + + # Calculate the kalman gain + kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement) + + # Calculate the new error_in_estimate, estimated x and estimated y coordinate + new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x) + new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y) + new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate) + + # update x and y coordinate + cone.x = new_estimate_x + cone.y = new_estimate_y + + return new_estimate_x, new_estimate_y, new_error_in_estimate + + + def cones_local_to_global(self, cones: list, car_position: Pose) -> list: + """This function converts all cones from the local frame to the global frame + + Args: + cones ([Point]): contains all the cones' positions in local frame + car_position (Pose): car's current position + + Returns: + global_cone_positions (list): a list contains each cone's global (x, y) + """ + # If no cones are detected, return an empty np.array + if len(cones) == 0: + return [] + + # Extract car's x coordinate, y coordinate and rotation angle + x, y, theta = self.extract_data_from_car(car_position) + + # Convert cones positions into a np.array contains each cone's local x coordinate, y coordinate + list_of_cones = self.convert_cones_to_data(cones) + + # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix + position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta) + + # Convert cone's coordinates from local frame to global frame + global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones) + + # Convert global_cone_columns to a list of cones' (x,y) position + global_cone_positions = [tuple(global_cone_columns[:,i]) for i in range(len(cones))] + + return global_cone_positions + + + def extract_data_from_car(self, car_position: Pose) -> tuple: + """This function extracts car's x coordinate, y coordinate and rotation angle from car_position + + Args: + car_position (Pose): car's current position + + Returns: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + """ + x = car_position.position.x + y = car_position.position.y + theta = car_position.orientation.w + return x, y, theta + + + def convert_cones_to_data(self, cones: list) -> np.array: + """Convert a list of cones' positions into a 2*n np.array contains each cone's local x coordinate and y coordinate + + Args: + cones ([Point]): A list of cones, each cone is represented as a Point + + Returns: + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + """ + # Extract x and y coordinate from all cones and store them in a 2*n np.arra + list_of_local_cones_x = [cone.x for cone in cones] + list_of_local_cones_y = [cone.y for cone in cones] + list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y]) + return list_of_cones + + + def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple: + """This function gets car's gloabl position vector and local coordinate rotation matrix + + Args: + x (float): car's x coordinate + y (float): car's y coordinate + theta (float): car's rotation angle + + Returns: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + """ + position_vector = np.array([[x],[y]]) + rotation_matrix = np.array([[math.cos(theta), -math.sin(theta)],[math.sin(theta), math.cos(theta)]]) + + return position_vector, rotation_matrix + + + def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array: + """This function converts all cones' (x, y) from the local frame to the global frame + + Args: + position_vector (np.array): car's global position vector + rotation_matrix (np.array): rotation matrix for cone's local coordinates + list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate + + Returns: + list_of_cones_output (np.array): 2*n np.array contains each cone's global x coordinate and y coordinate + """ + 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 + + + def produce_track_message(self, points: list) -> Track: + """This function packs all cones' (x, y) coordinates into a cone map message + + Args: + points (list): A list of cones' (x, y) coordinates + + Returns: + output_track (Track): Track message + """ + output_track = Track() + for p in points: + point = Point() + point.x = p[0] + point.y = p[1] + output_track.cones.append(point) + return output_track + + def remove_points(self): + if self.left_track is None or self.right_track is None: + return + + times_modified_list = Float32MultiArray() + + left_tree = self.left_tree + right_tree = self.right_tree + for point in kdtree.level_order(self.left_tree): + # print(point) + times_modified_list.data.append(point.data.data[2]) + if point.data.data[2] < self.times_modified_limit: + self.left_track.cones.remove(point.data.data[0]) + self.left_tree.remove(point.data) + for point in kdtree.level_order(self.right_tree): + times_modified_list.data.append(point.data.data[2]) + if point.data.data[2] < self.times_modified_limit: + self.right_track.cones.remove(point.data.data[0]) + self.right_tree.remove(point.data) + + self.times_modified_publisher.publish(times_modified_list) + + # kdtree.visualize(self.left_tree) + # kdtree.visualize(self.right_tree) + + +def main(args=None): + rclpy.init(args=args) + cone_mapper = Cone_Mapper() + rclpy.spin(cone_mapper) + cone_mapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 4689e777899e2e2e10e81cdc96ed86bf23d7a97e Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Sun, 29 Sep 2024 16:39:37 +1300 Subject: [PATCH 05/14] finished --- scripts/run_zed_launch.sh | 2 +- .../cone_mapping/cone_mapping/kalman_filter.py | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh index 89a1e728..7bdb5ce7 100644 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,3 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 \ No newline at end of file +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording2.svo2 \ No newline at end of file diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py index 5f058180..69d69146 100644 --- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -58,13 +58,13 @@ def __init__(self): self.right_track = None # KDTrees for searching - self.left_tree = None - self.right_tree = None + self.left_tree = kdtree.create(None, dimensions=2) + self.right_tree = kdtree.create(None, dimensions=2) self.times_modified_counter = 0 # For two recordings because its glitchy at begginning - time.sleep(10) + time.sleep(5) ################################################################################ (parameters to tune) @@ -81,7 +81,7 @@ def __init__(self): self.remove_point_counter = 200 # value for the times_modified - self.times_modified_limit = 300000 + self.times_modified_limit = 500000 # Modify rate self.modify_rate = 1.08 @@ -132,7 +132,7 @@ def update_left_track(self, points: list) -> None: Args: points (list): A list of blue cones' global (x, y) coordinates """ - if self.left_track == None: + if self.left_tree == kdtree.create(None, dimensions=2): # If the left track is empty, pack all cone measurements and update the left track self.left_track = self.produce_track_message(points) # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) @@ -169,7 +169,7 @@ def update_right_track(self, points: list) -> None: Args: points (list): A list of yellow cones' (x, y) coordinates """ - if self.right_track == None: + if self.right_tree == kdtree.create(None, dimensions=2): # If the right track is empty, pack all cone measurements and update the right track self.right_track = self.produce_track_message(points) times_modified = 1 From 66c722cfc53c04ce6143ad00272b3e3487aa6aa5 Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Sun, 29 Sep 2024 17:52:52 +1300 Subject: [PATCH 06/14] Velocity broken --- .../aquisition/zed_launch/CMakeLists.txt | 2 +- .../zed_launch/velocity/velocity.cpp | 31 +++++++++++++++++++ .../zed_launch/zed_launch/zed_launch.hpp | 6 ++++ 3 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 src/perception/aquisition/zed_launch/velocity/velocity.cpp diff --git a/src/perception/aquisition/zed_launch/CMakeLists.txt b/src/perception/aquisition/zed_launch/CMakeLists.txt index 34c3682f..e441e2c9 100644 --- a/src/perception/aquisition/zed_launch/CMakeLists.txt +++ b/src/perception/aquisition/zed_launch/CMakeLists.txt @@ -73,7 +73,7 @@ link_directories(/usr/local/cuda/lib64) include_directories(/usr/include/x86_64-linux-gnu/) link_directories(/usr/lib/x86_64-linux-gnu/) -FILE(GLOB_RECURSE SRC_FILES cone_detection/cone_detection.cpp cone_detection/yolo.cpp zed_launch/zed_launch.cpp localisation/localisation.cpp) +FILE(GLOB_RECURSE SRC_FILES cone_detection/cone_detection.cpp cone_detection/yolo.cpp zed_launch/zed_launch.cpp localisation/localisation.cpp velocity/velocity.cpp) FILE(GLOB_RECURSE HDR_FILES cone_detection/*.h* zed_launch/*.h*) cuda_add_executable(zed_launch_node ${HDR_FILES} ${SRC_FILES}) diff --git a/src/perception/aquisition/zed_launch/velocity/velocity.cpp b/src/perception/aquisition/zed_launch/velocity/velocity.cpp new file mode 100644 index 00000000..8afcd649 --- /dev/null +++ b/src/perception/aquisition/zed_launch/velocity/velocity.cpp @@ -0,0 +1,31 @@ +#include +#include + +#include "sl/Camera.hpp" +#include "../zed_launch/zed_launch.hpp" + +#include "geometry_msgs/msg/vector3.hpp" + +void ZedLaunchNode::car_velocity() { + + geometry_msgs::msg::Vector3 car_velocity; + sl::SensorsData sensor_data; + sl::SensorsData::IMUData imu_data; + + car_velocity.x = 0; + car_velocity.y = 0; + car_velocity.z = 0; + + while (camera_running) { + std::this_thread::sleep_for(10ms); + + zed.getSensorsData(sensor_data, sl::TIME_REFERENCE::IMAGE); + imu_data = sensor_data.imu; + + car_velocity.x += imu_data.linear_acceleration.x * 0.01; + car_velocity.y += imu_data.linear_acceleration.y * 0.01; + + car_velocity_publisher->publish(car_velocity); + + } +} \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp index b382b77d..9d28029a 100644 --- a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp @@ -11,6 +11,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "moa_msgs/msg/detections.hpp" #include "sensor_msgs/msg/image.hpp" +#include "geometry_msgs/msg/vector3.hpp" using namespace std::chrono_literals; @@ -26,6 +27,7 @@ class ZedLaunchNode : public rclcpp::Node // Create the publishers cone_detection_publisher = this->create_publisher("cone_detection", 10); car_position_publisher = this->create_publisher("car_position", 10); + car_velocity_publisher = this->create_publisher("car_velocity", 10); image_publisher = this->create_publisher("image", 10); // Start the threads @@ -33,6 +35,8 @@ class ZedLaunchNode : public rclcpp::Node std::thread t2(&ZedLaunchNode::car_position, this); + std::thread t3(&ZedLaunchNode::car_velocity, this); + t1.join(); } @@ -50,7 +54,9 @@ class ZedLaunchNode : public rclcpp::Node rclcpp::Publisher::SharedPtr cone_detection_publisher; rclcpp::Publisher::SharedPtr car_position_publisher; rclcpp::Publisher::SharedPtr image_publisher; + rclcpp::Publisher::SharedPtr car_velocity_publisher; void cone_detection_loop(); void car_position(); + void car_velocity(); }; \ No newline at end of file From c17993cfe789460ca39fb4c8f555ff5f6532f2f1 Mon Sep 17 00:00:00 2001 From: fsae computer <> Date: Wed, 2 Oct 2024 20:21:15 +1300 Subject: [PATCH 07/14] cone_mapping_modified --- .../cone_mapping/kalman_filter.py | 121 ++++++++---------- 1 file changed, 56 insertions(+), 65 deletions(-) diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py index 69d69146..52f0afad 100644 --- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -54,17 +54,16 @@ def __init__(self): self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10) # Existing cone map - self.left_track = None - self.right_track = None + self.left_track = Track() + self.left_track.cones = [] + self.right_track = Track() + self.right_track.cones = [] # KDTrees for searching self.left_tree = kdtree.create(None, dimensions=2) self.right_tree = kdtree.create(None, dimensions=2) - self.times_modified_counter = 0 - - # For two recordings because its glitchy at begginning - time.sleep(5) + self.times_updated_counter = 0 ################################################################################ (parameters to tune) @@ -98,20 +97,17 @@ def cones_callback(self, msg: Detections) -> None: # Update the existing cone map with new measurements self.update_existing_cone_map(msg) - # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic - # print("Left track length: ", len(self.left_track.cones)) - # print("Right track length: ", len(self.right_track.cones)) - self.times_modified_counter = self.times_modified_counter + 1 - if self.times_modified_counter > self.remove_point_counter: - self.times_modified_counter = 0 - self.remove_points() + self.times_updated_counter += 1 + if self.times_updated_counter > self.remove_point_counter: + self.times_updated_counter = 0 + self.remove_cones() self.left_track_publisher.publish(self.left_track) self.right_track_publisher.publish(self.right_track) def update_existing_cone_map(self, msg: Detections) -> None: - """This function updates the existing cone map using new cone measurementsf + """This function updates the existing cone map using new cone measurements Args: msg (Detections): Contains the car's position and lists of detected cones @@ -121,9 +117,11 @@ def update_existing_cone_map(self, msg: Detections) -> None: global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position) global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position) - # Update the left track and the right track - self.update_left_track(global_blue_cone_positions) - self.update_right_track(global_yellow_cone_positions) + # Update the left track and the right track if cones got detected + if len(global_blue_cone_positions) != 0: + self.update_left_track(global_blue_cone_positions) + if len(global_yellow_cone_positions) != 0: + self.update_right_track(global_yellow_cone_positions) def update_left_track(self, points: list) -> None: @@ -132,19 +130,14 @@ def update_left_track(self, points: list) -> None: Args: points (list): A list of blue cones' global (x, y) coordinates """ - if self.left_tree == kdtree.create(None, dimensions=2): + if len(self.left_track.cones) == 0: # If the left track is empty, pack all cone measurements and update the left track - self.left_track = self.produce_track_message(points) - # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)]) - times_modified = 1 - self.left_tree = kdtree.create( - [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) # For 2D points + self.add_cones_to_left_track(points) + self.left_tree = kdtree.create([Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!! else: # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track for coord in points: point, distance = self.left_tree.search_nn(coord) - # print(self.left_tree.search_nn(coord)) - # kdtree.visualize(self.left_tree) # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates if distance <= self.match_radius: cone = point.data.data[0] @@ -152,14 +145,15 @@ def update_left_track(self, points: list) -> None: times_modified = point.data.data[2] * self.modify_rate new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) - point.data.data = (cone, new_error_in_estimate, times_modified) - else: # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + point.data.data[1] = new_error_in_estimate + point.data.data[2] = times_modified + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map + else: cone = Point() cone.x = coord[0] cone.y = coord[1] self.left_track.cones.append(cone) - times_modified = 1 - self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + self.left_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1])) @@ -167,36 +161,32 @@ def update_right_track(self, points: list) -> None: """This function updates the right track of the cone map Args: - points (list): A list of yellow cones' (x, y) coordinates + points (list): A list of blue cones' global (x, y) coordinates """ - if self.right_tree == kdtree.create(None, dimensions=2): + if len(self.right_track.cones) == 0: # If the right track is empty, pack all cone measurements and update the right track - self.right_track = self.produce_track_message(points) - times_modified = 1 - self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2) + self.add_cones_to_right_track(points) + self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!! else: # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track for coord in points: point, distance = self.right_tree.search_nn(coord) - # print(self.right_tree.search_nn(coord)) # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates - # check if distance is nan - # kdtree.visualize(self.right_tree) if distance <= self.match_radius: cone = point.data.data[0] error_in_estimate = point.data.data[1] times_modified = point.data.data[2] * self.modify_rate new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate) point.data.coords = (new_estimate_x, new_estimate_y) - point.data.data = (cone, new_error_in_estimate, times_modified) + point.data.data[1] = new_error_in_estimate + point.data.data[2] = times_modified + # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map else: - # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map cone = Point() cone.x = coord[0] cone.y = coord[1] self.right_track.cones.append(cone) - times_modified = 1 - self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified))) + self.right_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1])) def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple: @@ -331,48 +321,49 @@ def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, return list_of_cones_output - def produce_track_message(self, points: list) -> Track: - """This function packs all cones' (x, y) coordinates into a cone map message + def add_cones_to_left_track(self, points: list) -> None: + """This function packs all cones' (x, y) coordinates and add them to the left track Args: points (list): A list of cones' (x, y) coordinates - - Returns: - output_track (Track): Track message """ - output_track = Track() for p in points: point = Point() point.x = p[0] point.y = p[1] - output_track.cones.append(point) - return output_track - - def remove_points(self): - if self.left_track is None or self.right_track is None: + self.left_track.cones.append(point) + + def add_cones_to_right_track(self, points: list) -> None: + """This function packs all cones' (x, y) coordinates and add them to the right track + + Args: + points (list): A list of cones' (x, y) coordinates + """ + for p in points: + point = Point() + point.x = p[0] + point.y = p[1] + self.right_track.cones.append(point) + + def remove_cones(self): + if len(self.left_track.cones) == 0 or len(self.right_track.cones) == 0: return times_modified_list = Float32MultiArray() - left_tree = self.left_tree - right_tree = self.right_tree - for point in kdtree.level_order(left_tree): - # print(point) - times_modified_list.data.append(point.data.data[2]) + for point in kdtree.level_order(self.left_tree): if point.data.data[2] < self.times_modified_limit: + times_modified_list.data.append(point.data.data[2]) self.left_track.cones.remove(point.data.data[0]) - self.left_tree.remove(point.data) - for point in kdtree.level_order(right_tree): - times_modified_list.data.append(point.data.data[2]) - # print(point) + self.left_tree = self.left_tree.remove(point.data) + + for point in kdtree.level_order(self.right_tree): if point.data.data[2] < self.times_modified_limit: + times_modified_list.data.append(point.data.data[2]) self.right_track.cones.remove(point.data.data[0]) - self.right_tree.remove(point.data) + self.right_tree = self.right_tree.remove(point.data) self.times_modified_publisher.publish(times_modified_list) - - # kdtree.visualize(self.left_tree) - # kdtree.visualize(self.right_tree) def main(args=None): From e2921cd7fbb2de04333ae491ebd3ead647e39274 Mon Sep 17 00:00:00 2001 From: JetsonOrin Date: Thu, 3 Oct 2024 16:42:06 +1300 Subject: [PATCH 08/14] Modified kalman filter params --- scripts/run_zed_launch.sh | 2 +- .../sythesis/cone_mapping/cone_mapping/kalman_filter.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh index 7bdb5ce7..e0a93cc2 100644 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,3 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording2.svo2 \ No newline at end of file +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node \ No newline at end of file diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py index 52f0afad..07c2dd28 100644 --- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py +++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py @@ -80,7 +80,7 @@ def __init__(self): self.remove_point_counter = 200 # value for the times_modified - self.times_modified_limit = 500000 + self.times_modified_limit = 100 # Modify rate self.modify_rate = 1.08 From d1535195451c5bf23e5b6f8cd0e888e05f974c77 Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Sun, 17 Nov 2024 15:49:14 +1300 Subject: [PATCH 09/14] Created a launch file for the zed_launch --- .../aquisition/zed_launch/CMakeLists.txt | 5 +++ .../zed_launch/launch/zed_launch.launch.py | 38 +++++++++++++++++++ .../aquisition/zed_launch/package.xml | 1 + 3 files changed, 44 insertions(+) create mode 100644 src/perception/aquisition/zed_launch/launch/zed_launch.launch.py diff --git a/src/perception/aquisition/zed_launch/CMakeLists.txt b/src/perception/aquisition/zed_launch/CMakeLists.txt index e441e2c9..8052b12e 100644 --- a/src/perception/aquisition/zed_launch/CMakeLists.txt +++ b/src/perception/aquisition/zed_launch/CMakeLists.txt @@ -135,4 +135,9 @@ install(TARGETS DESTINATION lib/zed_launch ) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_package() diff --git a/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py new file mode 100644 index 00000000..f2629de8 --- /dev/null +++ b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py @@ -0,0 +1,38 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Declare the launch argument for recording name + recording_name_arg = DeclareLaunchArgument( + 'recording_name', + default_value='default_recording', + description=""" + This launch file is used to start the ZED camera node with or + without recording functionality. + Usage: + - Without recording: ros2 launch zed_launch.launch.py + - With recording: ros2 launch zed_launch.launch.py recording_name:= + + The 'recording_name' argument is optional. If provided, the camera node will + initiate a recording session using the specified file name. + """ + ) + + # Access the recording name configured by the user + recording_name = LaunchConfiguration('recording_name') + + # Define the node, including the recording name as a command-line argument + camera_node = Node( + package='zed_launch', + executable='zed_launch_node', + name='zed_camera', + output='screen', + arguments=[recording_name] + ) + + return LaunchDescription([ + recording_name_arg, + camera_node + ]) \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/package.xml b/src/perception/aquisition/zed_launch/package.xml index 2b27b0e7..58be6bd6 100644 --- a/src/perception/aquisition/zed_launch/package.xml +++ b/src/perception/aquisition/zed_launch/package.xml @@ -22,6 +22,7 @@ geometry_msgs sensor_msgs cv_bridge + ros2launch ament_cmake From 2fb5f0de812c623e674bac7dfb405bdb631186a7 Mon Sep 17 00:00:00 2001 From: AdrianRosioru Date: Sun, 1 Dec 2024 12:08:55 +1300 Subject: [PATCH 10/14] Made changes as requested --- scripts/run_zed_launch.sh | 2 +- .../aquisition/zed_launch/cone_detection/cone_detection.cpp | 2 ++ src/perception/aquisition/zed_launch/velocity/velocity.cpp | 4 +++- .../aquisition/zed_launch/zed_launch/zed_launch.hpp | 2 -- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh index e0a93cc2..89a1e728 100644 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,3 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node \ No newline at end of file +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp index ec433214..0ae218a4 100644 --- a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp +++ b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp @@ -15,6 +15,8 @@ #include using namespace nvinfer1; +#define CONF_THRESH 0.8 +#define NMS_THRESH 0.4 #include diff --git a/src/perception/aquisition/zed_launch/velocity/velocity.cpp b/src/perception/aquisition/zed_launch/velocity/velocity.cpp index 8afcd649..aa196c38 100644 --- a/src/perception/aquisition/zed_launch/velocity/velocity.cpp +++ b/src/perception/aquisition/zed_launch/velocity/velocity.cpp @@ -28,4 +28,6 @@ void ZedLaunchNode::car_velocity() { car_velocity_publisher->publish(car_velocity); } -} \ No newline at end of file +} + +// ### THIS DOES NOT OUTPUT ACCURATE VELOCITY DATA ### \ No newline at end of file diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp index 9d28029a..e563d7ff 100644 --- a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp +++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp @@ -47,8 +47,6 @@ class ZedLaunchNode : public rclcpp::Node bool camera_running = true; bool visualisation = true; std::string model_name = "cone_detection_model.engine"; - float CONF_THRESH = 0.8; - float NMS_THRESH = 0.5; // Publishers rclcpp::Publisher::SharedPtr cone_detection_publisher; From 108d5f734235dcea189f825adbc6ac701911f20b Mon Sep 17 00:00:00 2001 From: JetsonOrin Date: Sun, 16 Mar 2025 18:49:48 +1300 Subject: [PATCH 11/14] Pang's path planning code --- .../rosbag2_2025_03_06-20_57_16/metadata.yaml | 21 +++ .../rosbag2_2025_03_06-20_57_16_0.db3 | Bin 0 -> 24576 bytes .../rosbag2_2025_03_06-21_00_01/metadata.yaml | 21 +++ .../rosbag2_2025_03_06-21_00_01_0.db3 | Bin 0 -> 24576 bytes scripts/run_path_planning.sh | 2 +- scripts/run_zed_launch.sh | 2 +- scripts/z_recording_simulation.sh | 0 .../path_planning/fasttube_planner.py | 166 ++++++++++++++++++ src/planning/path_planning/setup.py | 1 + 9 files changed, 211 insertions(+), 2 deletions(-) create mode 100644 bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml create mode 100644 bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 create mode 100644 bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml create mode 100644 bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 mode change 100644 => 100755 scripts/run_zed_launch.sh mode change 100644 => 100755 scripts/z_recording_simulation.sh create mode 100644 src/planning/path_planning/path_planning/fasttube_planner.py diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml new file mode 100644 index 00000000..b13c0d04 --- /dev/null +++ b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_2025_03_06-20_57_16_0.db3 + files: + - path: rosbag2_2025_03_06-20_57_16_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 b/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 new file mode 100644 index 0000000000000000000000000000000000000000..8d37ede6e5f7bc6ba5f0198482595947f8cbb001 GIT binary patch literal 24576 zcmeI&Z;#S29KiAI_;b+&G4TQ60ZE?;$!Sb{=o8^w%;^N>X6{H#NWB%W31hfTbNCG3 zgKx=q!lEuW!1B{DYmI* z@@iJ!6!xw9p=#ZFdhw=VdTWiqYW`|AAAD;3HdPV=2q1s}0tg_000IagP;P;XTD|qS zW3TkWS2u|cRGi3QIo1A+smAh)itDFir|&u9Z1~E#6onyTG!*$T(cN{4{`qNd{iu4} z>U8Y&Dv_V(Dj$}YwQN?eKX9GhBwg*Bbv);_GZyd1XYcysiFoTwPLgn`uS`(8&f&-t z!;h|e@^8`nxm}m=Wg^AE9Sua@)>}WT9JktS`{#>oRNHyuG_MvSO4|;_vyg!jo^xq} z*afm$?l;9M(sHhU$V7*WX?CA2J{8VpDpLORYZy+SW~rJFDD2)5vVu z*KrbsdrNKZ=63UsWnLr%5I_I{1Q0*~0R#|0009ILD3idwO2ukle+$f4MVtG-`O%+* z00IagfB*srAb20zz2fJpcdz literal 0 HcmV?d00001 diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml new file mode 100644 index 00000000..eae7d682 --- /dev/null +++ b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml @@ -0,0 +1,21 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 0 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + message_count: 0 + topics_with_message_count: + [] + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_2025_03_06-21_00_01_0.db3 + files: + - path: rosbag2_2025_03_06-21_00_01_0.db3 + starting_time: + nanoseconds_since_epoch: 9223372036854775807 + duration: + nanoseconds: 0 + message_count: 0 \ No newline at end of file diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 b/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 new file mode 100644 index 0000000000000000000000000000000000000000..8d37ede6e5f7bc6ba5f0198482595947f8cbb001 GIT binary patch literal 24576 zcmeI&Z;#S29KiAI_;b+&G4TQ60ZE?;$!Sb{=o8^w%;^N>X6{H#NWB%W31hfTbNCG3 zgKx=q!lEuW!1B{DYmI* z@@iJ!6!xw9p=#ZFdhw=VdTWiqYW`|AAAD;3HdPV=2q1s}0tg_000IagP;P;XTD|qS zW3TkWS2u|cRGi3QIo1A+smAh)itDFir|&u9Z1~E#6onyTG!*$T(cN{4{`qNd{iu4} z>U8Y&Dv_V(Dj$}YwQN?eKX9GhBwg*Bbv);_GZyd1XYcysiFoTwPLgn`uS`(8&f&-t z!;h|e@^8`nxm}m=Wg^AE9Sua@)>}WT9JktS`{#>oRNHyuG_MvSO4|;_vyg!jo^xq} z*afm$?l;9M(sHhU$V7*WX?CA2J{8VpDpLORYZy+SW~rJFDD2)5vVu z*KrbsdrNKZ=63UsWnLr%5I_I{1Q0*~0R#|0009ILD3idwO2ukle+$f4MVtG-`O%+* z00IagfB*srAb20zz2fJpcdz literal 0 HcmV?d00001 diff --git a/scripts/run_path_planning.sh b/scripts/run_path_planning.sh index 130ff830..27b16204 100755 --- a/scripts/run_path_planning.sh +++ b/scripts/run_path_planning.sh @@ -1,6 +1,6 @@ #!/bin/bash source install/setup.bash colcon build --packages-select path_planning -ros2 run path_planning centerline_planner +ros2 run path_planning fasttube #ros2 run path_planning trajectory_generation diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh old mode 100644 new mode 100755 index 89a1e728..e0a93cc2 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,3 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 \ No newline at end of file +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node \ No newline at end of file diff --git a/scripts/z_recording_simulation.sh b/scripts/z_recording_simulation.sh old mode 100644 new mode 100755 diff --git a/src/planning/path_planning/path_planning/fasttube_planner.py b/src/planning/path_planning/path_planning/fasttube_planner.py new file mode 100644 index 00000000..53852b98 --- /dev/null +++ b/src/planning/path_planning/path_planning/fasttube_planner.py @@ -0,0 +1,166 @@ +import rclpy +from rclpy.node import Node + +from moa_msgs.msg import Track +from geometry_msgs.msg import Pose, PoseArray + +import numpy as np +import matplotlib.pyplot as plt +from fsd_path_planning import PathPlanner, MissionTypes, ConeTypes +import math + + +class centerline_planner(Node): + def __init__(self): + super().__init__("centerline_planner") + + # parameters + self._plot = True + self.look_forward = 4 + self.left_cones = [] + self.right_cones = [] + self.path_planner = PathPlanner(MissionTypes.trackdrive) + + # subscribers + self.subscription_left_cone_map = self.create_subscription(Track, 'left_track', self.left_cone_map_callback, 10) + self.subscription_right_cone_map = self.create_subscription(Track, 'right_track', self.right_cone_map_callback, 10) + self.create_subscription(Pose, "car_position", self.set_car_position, 10) # car pose + + # publishers + self.centerline_publisher = self.create_publisher(PoseArray, "moa/selected_trajectory", 10) + + def set_car_position(self, msg:Pose) -> None: + self.car_pose = msg + self.loop() + + def left_cone_map_callback(self, msg:Track) -> None: + self.left_cones = msg.cones + + def right_cone_map_callback(self, msg:Track) -> None: + self.right_cones = msg.cones + + def loop(self) -> None: + self.get_logger().info(f"car pose recieved = {hasattr(self,'car_pose')}") + + if not hasattr(self,'car_pose'): + return + + lb, rb = self.get_boundaries() # get boundaries (list of [x,y] points) + + if not len(lb) > 0 or not len(rb) > 0: + return + + lblocal, _, _= self.get_next_points(lb,self.look_forward) # get local points (list of [x,y] points) close to car + rblocal, car_position, car_orientation = self.get_next_points(rb,self.look_forward) + + if not len(lblocal) > 0 or not len(rblocal) > 0: + return + + global_cones = self.get_global_cones(lblocal, rblocal) + car_direction = self.get_car_direction(car_orientation) + + path = self.path_planner.calculate_path_in_global_frame(global_cones, car_position, car_direction) # compute centerline + + centerline = path[:, 1:3] + + if not len(centerline) > 0: + return + + # publish + msg = self.get_posearray_msg(centerline) + self.centerline_publisher.publish(msg) + + self.plot(lb,rb,centerline,self._plot) # plot + + + def get_global_cones(self, lb, rb): + cones_by_type = [np.zeros((0, 2)) for _ in range(5)] + cones_by_type[ConeTypes.LEFT] = lb + cones_by_type[ConeTypes.RIGHT] = rb + return cones_by_type + + + def get_car_direction(self, car_orientation): + return np.array([-math.sin(car_orientation), math.cos(car_orientation)]) + + + # MAIN FUNCTIONS + def get_boundaries(self): + """Retrieves the left and right boundary from msg and returns as a [x,y] list""" + left_cones = np.array([[P.x, P.y] for P in self.left_cones]) + right_cones = np.array([[P.x, P.y] for P in self.right_cones]) + + return left_cones, right_cones + + def get_next_points(self,line,look_forward): + """Retrieves the points closest to the car + *ASSUMES THE points ARE SORTED/ORDERED + """ + car_point = np.array([self.car_pose.position.x,self.car_pose.position.y]) + car_orientation = self.car_pose.orientation.w + points = np.array(line) + + min_indx = np.argmin(np.linalg.norm(car_point-points, axis=1)) + # min_indx += self.is_behind_car(points[min_indx], car_point, car_orientation) + points = self.get_local_points(min_indx,points,look_forward) + + return points, car_point, car_orientation + + + def get_posearray_msg(self, line): + pose_array = PoseArray() + + for P in line: + pose = Pose() + pose.position.x = P[0] # assing x,y values to position of pose + pose.position.y = P[1] + pose_array.poses.append(pose) + + return pose_array + + + def plot(self,lb,rb,centerline,to_plot=False): + """Plots the boundary and centerline points""" + if to_plot: + # x, y points + lb = np.array(lb) + rb = np.array(rb) + centerline = np.array(centerline) + + lbx, lby = lb[:,0], lb[:,1] + rbx, rby = rb[:,0], rb[:,1] + centx, centy = centerline[:,0], centerline[:,1] + car_x, car_y = [self.car_pose.position.x, self.car_pose.position.y] + + # plot + plt.ion() + plt.clf() + + plt.plot(lbx,lby,'*b',label='left') + plt.plot(rbx,rby,'*y',label='right') + plt.plot(centx,centy,'-r',label='centerline') + plt.plot(car_x,car_y,'*k',label='car position') + + plt.pause(0.1) + plt.legend() + plt.show() + + + def get_local_points(self,min_indx,points,look_forward): + remaining_points = len(points) - (min_indx+1) # number of points forwards the car has detected + if remaining_points < look_forward: look_forward = remaining_points+1 + points = points[min_indx:min_indx+look_forward] # local points based on closest distance to car + + return points + + +def main(args=None): + rclpy.init(args=args) + node = centerline_planner() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/planning/path_planning/setup.py b/src/planning/path_planning/setup.py index 52cedbb5..f0527522 100644 --- a/src/planning/path_planning/setup.py +++ b/src/planning/path_planning/setup.py @@ -27,6 +27,7 @@ 'centerline_planner = path_planning.centerline_planner:main', 'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main', 'center_line = path_planning.simple_centerline_planner:main', + 'fasttube = path_planning.fasttube_planner:main' ], }, ) From 975ffd037d5d16c2034a510a0d645931dd93ca25 Mon Sep 17 00:00:00 2001 From: winola Date: Mon, 24 Mar 2025 21:44:00 +1300 Subject: [PATCH 12/14] zed launch shell script --- scripts/run_zed_launch.sh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh index e0a93cc2..22a65913 100755 --- a/scripts/run_zed_launch.sh +++ b/scripts/run_zed_launch.sh @@ -1,3 +1,5 @@ #!/bin/bash source install/setup.bash -colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node \ No newline at end of file +#colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node +## with sim +colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2 \ No newline at end of file From ec44e7e828f86987947fea4389d06f2e2c2d56c9 Mon Sep 17 00:00:00 2001 From: winola Date: Wed, 26 Mar 2025 20:56:08 +1300 Subject: [PATCH 13/14] modified for crown --- .../stanley_controller/stanley_controller/stanley_controller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/action/stanley_controller/stanley_controller/stanley_controller.py b/src/action/stanley_controller/stanley_controller/stanley_controller.py index d0b274f9..fbc071f3 100644 --- a/src/action/stanley_controller/stanley_controller/stanley_controller.py +++ b/src/action/stanley_controller/stanley_controller/stanley_controller.py @@ -4,7 +4,6 @@ import math from geometry_msgs.msg import PoseArray from geometry_msgs.msg import Pose -from moa_msgs.msg import ConeMap from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped from std_msgs.msg import Header From e1bc6579599a63e88c22a0d0d5079134813cf301 Mon Sep 17 00:00:00 2001 From: winola Date: Mon, 7 Apr 2025 20:14:05 +1200 Subject: [PATCH 14/14] CanTalk update --- src/hardware_drivers/CanTalk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hardware_drivers/CanTalk b/src/hardware_drivers/CanTalk index f887c380..0d8cb207 160000 --- a/src/hardware_drivers/CanTalk +++ b/src/hardware_drivers/CanTalk @@ -1 +1 @@ -Subproject commit f887c38038fd4ba4a87c7075ca9f883bd60670bb +Subproject commit 0d8cb20773093716615f7e18d7ba739931cc9ae1