Cone detection test

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

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

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
 

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 recording2.svo2
+#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

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
 

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})
@@ -135,4 +135,9 @@ install(TARGETS
   DESTINATION lib/zed_launch
 )
 
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_package()

src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp

@@ -15,8 +15,8 @@
 #include <NvInfer.h>
 
 using namespace nvinfer1;
-#define NMS_THRESH 0.4
 #define CONF_THRESH 0.8
+#define NMS_THRESH 0.4
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -84,7 +84,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_model.engine";
+    std::string engine_name = model_name;
     Yolo detector;
     if (detector.init(engine_name)) {
         std::cerr << "Detector init failed!" << std::endl;

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<moa_msgs::msg::Detections>::SharedPtr subscription_;
 };

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:=<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
+    ])

src/perception/aquisition/zed_launch/package.xml

@@ -22,6 +22,7 @@
 	<exec_depend>geometry_msgs</exec_depend>
 	<exec_depend>sensor_msgs</exec_depend>
 	<exec_depend>cv_bridge</exec_depend>
+	<exec_depend>ros2launch</exec_depend>
 
 	<export>
 		<build_type>ament_cmake</build_type>

src/perception/aquisition/zed_launch/velocity/velocity.cpp

@@ -0,0 +1,33 @@
+#include <iostream>
+#include <cmath>
+
+#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);
+
+    }
+}
+
+//  ### THIS DOES NOT OUTPUT ACCURATE VELOCITY DATA ###

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,13 +27,16 @@ class ZedLaunchNode : public rclcpp::Node
     // Create the publishers
     cone_detection_publisher = this->create_publisher<moa_msgs::msg::Detections>("cone_detection", 10);
     car_position_publisher = this->create_publisher<geometry_msgs::msg::Pose>("car_position", 10);
+    car_velocity_publisher = this->create_publisher<geometry_msgs::msg::Vector3>("car_velocity", 10);
     image_publisher = this->create_publisher<sensor_msgs::msg::Image>("image", 10);
 
     // Start the threads
     std::thread t1(&ZedLaunchNode::cone_detection_loop, this);
 
     std::thread t2(&ZedLaunchNode::car_position, this);
 
+    std::thread t3(&ZedLaunchNode::car_velocity, this);
+
     t1.join();
   }
 
@@ -41,13 +45,16 @@ class ZedLaunchNode : public rclcpp::Node
     sl::Camera& zed;
     sl::Pose cam_w_pose;
     bool camera_running = true;
-    bool visualisation = false;
+    bool visualisation = true;
+    std::string model_name = "cone_detection_model.engine";
 
     // Publishers
     rclcpp::Publisher<moa_msgs::msg::Detections>::SharedPtr cone_detection_publisher;
     rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr car_position_publisher;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher;
+    rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr car_velocity_publisher;
 
     void cone_detection_loop();
     void car_position();
+    void car_velocity();
 };