diff --git a/config/dynamic_scan_tracking.yaml b/config/dynamic_scan_tracking.yaml index c1db3da..991622e 100644 --- a/config/dynamic_scan_tracking.yaml +++ b/config/dynamic_scan_tracking.yaml @@ -1,21 +1,11 @@ # drone detection from range image drone_detection_enabled: false -# position straight trajectory +# initial position initial_position_x: 10.52325 initial_position_y: -1.3305 initial_position_z: -0.3341 -# position circular trajectory -# initial_position_x: 7.8519 -# initial_position_y: 1.46506 -# initial_position_z: 0.56868 - -# position calibration -# initial_position_x: 13.4228 -# initial_position_y: 0.86042 -# initial_position_z: 0.57592 - search_radius: 0.2 gamma: 0.000000005 ## normalization constant used for point clouds weighted sum diff --git a/scripts/livox_to_img/__pycache__/livox_to_img.cpython-38.pyc b/scripts/livox_to_img/__pycache__/livox_to_img.cpython-38.pyc index 09741cc..417d7f5 100644 Binary files a/scripts/livox_to_img/__pycache__/livox_to_img.cpython-38.pyc and b/scripts/livox_to_img/__pycache__/livox_to_img.cpython-38.pyc differ diff --git a/scripts/livox_to_img/livox_to_img.py b/scripts/livox_to_img/livox_to_img.py index e4437ef..30e08e2 100755 --- a/scripts/livox_to_img/livox_to_img.py +++ b/scripts/livox_to_img/livox_to_img.py @@ -26,6 +26,7 @@ ############################################################################## import rospy +import os import cv2 import numpy as np @@ -68,7 +69,7 @@ def __init__(self): self.point_cloud_buffer = [None] * self.buffer_size self.current_index = 0 self.counter = 0 - self.model = yolov5.load('/home/iacopo/dynamic_scan_tracking_ws/src/model/model.pt') + self.model = yolov5.load(os.path.dirname(os.path.dirname(os.path.dirname(os.path.realpath(__file__)))) + "/model/model.pt") self.model.conf = 0.8 self.model.iou = 0.8 self.model.agnostic = False diff --git a/src/dynamic_scan_tracking.cpp b/src/dynamic_scan_tracking.cpp index 1fd5e0a..720e376 100644 --- a/src/dynamic_scan_tracking.cpp +++ b/src/dynamic_scan_tracking.cpp @@ -48,7 +48,7 @@ class DynamicScanTrackingNode // Define Publishers ros::Publisher pub_object_pose; // Final pose from intersection AST and ADT ros::Publisher pub_object_velocity; - // ros::Publisher pub_target_pcl; + ros::Publisher pub_target_pcl; // Define vector used to transform livox custom msg into PointCloud2 std::vector livox_data; @@ -146,7 +146,7 @@ class DynamicScanTrackingNode // Publishers for poses calculated for different integration times pub_object_pose = nh.advertise("/dynamic_scan_tracking/object_pose", 1000); pub_object_velocity = nh.advertise("/dynamic_scan_tracking/object_velocity", 1000); - // pub_target_pcl = nh.advertise("/target_pcl", 1000); // used to visualize current livox point cloud scan + pub_target_pcl = nh.advertise("/target_pcl", 1000); // used to visualize current livox point cloud scan // Initialize position and drone detection to false (changed later according to parameters) position_initialized = false; @@ -254,14 +254,14 @@ class DynamicScanTrackingNode ROS_INFO_STREAM("Initial position = " << obj_position); } - // void publishPointCloud(pcl::PointCloud::Ptr& pcl_ptr, ros::Publisher& publisher) - // { - // sensor_msgs::PointCloud2 pcl_ros_msg; - // pcl::toROSMsg(*pcl_ptr.get(), pcl_ros_msg); - // pcl_ros_msg.header.frame_id = "livox_frame"; - // pcl_ros_msg.header.stamp = ros::Time::now(); - // publisher.publish(pcl_ros_msg); - // } + void publishPointCloud(pcl::PointCloud::Ptr& pcl_ptr, ros::Publisher& publisher) + { + sensor_msgs::PointCloud2 pcl_ros_msg; + pcl::toROSMsg(*pcl_ptr.get(), pcl_ros_msg); + pcl_ros_msg.header.frame_id = "livox_frame"; + pcl_ros_msg.header.stamp = ros::Time::now(); + publisher.publish(pcl_ros_msg); + } void droneDetectionCallback(const geometry_msgs::PoseStamped initial_position) { @@ -290,11 +290,12 @@ class DynamicScanTrackingNode // Used in compute pose to define each point's weight unsigned long msg_timebase_ns = livox_data[0]->timebase; - // publishPointCloud(obj_cloud_ast, pub_target_pcl); + publishPointCloud(obj_cloud_ast, pub_target_pcl); // uncomment to visualize tracked target point cloud trackObjectAdaptiveIntegrationTime(point_cloud_ast, point_cloud_current_scan, obj_cloud_ast, deque_ast, future_position_ast, kf_ast, search_radius, msg_timebase_ns, MAX_INTEGRATION_TIME_AST, optimal_integration_time_ast, pose_vector_ast, current_pose_ast, previous_pose_ast); + trackObjectAdaptiveIntegrationTime(point_cloud_adt, point_cloud_current_scan, obj_cloud_adt, deque_adt, future_position_adt, kf_adt, search_radius, msg_timebase_ns, MAX_INTEGRATION_TIME_ADT, optimal_integration_time_adt, pose_vector_adt, current_pose_adt, previous_pose_adt); @@ -366,7 +367,7 @@ class DynamicScanTrackingNode int total_num_scans = (optimal_integration_time < point_cloud_deque.size()) ? optimal_integration_time : point_cloud_deque.size(); // Add the points from the last max_scans point clouds to the current point cloud - for (int i = 0; i <= total_num_scans; i++) + for (int i = 0; i <= total_num_scans; ++i) { *current_cloud += point_cloud_deque[i]; } @@ -384,6 +385,7 @@ class DynamicScanTrackingNode KalmanFilter& kalman_filter, float search_radius) { + // Initialize KDTree for searching object in point cloud pcl::KdTreeFLANN::Ptr kd_tree (new pcl::KdTreeFLANN()); kd_tree->setInputCloud(point_cloud); @@ -419,6 +421,7 @@ class DynamicScanTrackingNode object_cloud->points.push_back(pt); } } + } void updatePosition(pcl::PointCloud::Ptr& object_cloud, KalmanFilter& kalman_filter, unsigned long msg_timebase_ns)