diff --git a/cone_detection_lidar/include/cone_detection_lidar/pointcloud_to_grid_core.hpp b/cone_detection_lidar/include/cone_detection_lidar/pointcloud_to_grid_core.hpp index 2770a9b..efd003f 100644 --- a/cone_detection_lidar/include/cone_detection_lidar/pointcloud_to_grid_core.hpp +++ b/cone_detection_lidar/include/cone_detection_lidar/pointcloud_to_grid_core.hpp @@ -10,7 +10,17 @@ class PointXY PointXY() : x(0), y(0) {} // Default constructor // initialize with x and y PointXY(double x_val, double y_val) : x(x_val), y(y_val) {} +}; +// X, Y and orientation +class PointXYori : public PointXY +{ +public: + double ori; + PointXYori() : ori(0) {} // Default constructor + // initialize with x and y + PointXYori(double x_val, double y_val, double ori_val) : PointXY(x_val, y_val), ori(ori_val) {} }; + class PointXYZI { public: diff --git a/cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp b/cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp index 4aaa4b7..bee5e7d 100644 --- a/cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp +++ b/cone_detection_lidar/include/cone_detection_lidar/trajectory.hpp @@ -7,6 +7,19 @@ #include #include +class SearchConfig +{ +public: + double x_start; + double y_start; + double search_range_deg; + double search_resolution_deg; + double search_start_mid_deg; + double search_length; + // default constructor + SearchConfig() : x_start(0), y_start(0), search_range_deg(0), search_resolution_deg(0), search_start_mid_deg(0), search_length(0) {} +}; + // Extending the GridMap class class GridMapTrajectory : public GridMap { @@ -77,21 +90,20 @@ class GridMapTrajectory : public GridMap return true; } - PointXY angualar_search(double x_start, double y_start, double search_range_deg, double search_resolution_deg, double search_start_mid_deg, double search_length, std::vector &hpoints, double &max_true_angle) + std::vector angualar_search(SearchConfig s, std::vector &hpoints, double &max_true_angle) { - - double search_range = search_range_deg * M_PI / 180; - double search_resolution = search_resolution_deg * M_PI / 180; - double search_start_mid = search_start_mid_deg * M_PI / 180; + double search_range = s.search_range_deg * M_PI / 180; + double search_resolution = s.search_resolution_deg * M_PI / 180; + double search_start_mid = s.search_start_mid_deg * M_PI / 180; int loop_increment = int(search_range / search_resolution); std::vector search_results(loop_increment); for (int loop = 0; loop < loop_increment; loop++) { double search_ang = -0.5 * search_range + double(loop) * search_resolution; - search_results[loop] = drawline(hpoints, x_start, y_start, search_start_mid + search_ang, search_length); + search_results[loop] = drawline(hpoints, s.x_start, s.y_start, search_start_mid + search_ang, s.search_length); } // find the longest continous true (free) segment in search_results - int max_true = 0; + int max_true1 = 0; int max_true_start = 0; int max_true_end = 0; int true_count = 0; @@ -108,25 +120,26 @@ class GridMapTrajectory : public GridMap { true_end = loop - 1; } - if (true_count > max_true) + if (true_count > max_true1) { - max_true = true_count; + max_true1 = true_count; max_true_end = true_end; } true_count = 0; } } // if everything is true (false else never evaluated) - if (true_count > max_true) + if (true_count > max_true1) { - max_true = true_count; + max_true1 = true_count; max_true_end = loop_increment - 1; } - max_true_start = max_true_end - max_true + 1; + max_true_start = max_true_end - max_true1 + 1; - // this for loop is only fro visualization + // this for loop is only for visualization // true and false segments (green and red) + /* for (int i = 0; i < loop_increment; i++) { if (search_results[i]) @@ -138,15 +151,19 @@ class GridMapTrajectory : public GridMap hpoints[(cell_num_y / 2 + i - loop_increment / 2) * cell_num_x] = -128; } } + */ // RCLCPP_INFO_STREAM(this->get_logger(), "max_true: " << max_true << " max_true_start: " << max_true_start << " max_true_end: " << max_true_end); int max_true_center = (max_true_start + max_true_end) / 2; max_true_angle = -0.5 * search_range + search_start_mid + double(max_true_center) * search_resolution; - double x_end = x_start + search_length * cos(max_true_angle); - double y_end = y_start + search_length * sin(max_true_angle); - PointXY p_end = PointXY(x_end, y_end); - return p_end; + double x_end = s.x_start + s.search_length * cos(max_true_angle); + double y_end = s.y_start + s.search_length * sin(max_true_angle); + PointXYori p_end = PointXYori(x_end, y_end, max_true_angle); + std::vector p_end_vector; + p_end_vector.push_back(p_end); + // TODO: return the second longest segment center as well + return p_end_vector; } }; diff --git a/cone_detection_lidar/src/grid_trajectory.cpp b/cone_detection_lidar/src/grid_trajectory.cpp index 9723e11..0c25bba 100644 --- a/cone_detection_lidar/src/grid_trajectory.cpp +++ b/cone_detection_lidar/src/grid_trajectory.cpp @@ -229,28 +229,48 @@ class PointCloudToGrid : public rclcpp::Node } } } - if (search_iter > 6){ + if (search_iter > 6) + { search_iter = 6; } - std::vector traj(search_iter + 1); // TODO: remove and use only tree - double max_true_angle = 0.0; + std::vector traj_end_points(1); + double traj_search_angle = 0.0; Tree tree; - traj[0] = PointXY(0.0, 0.0); + traj_end_points[0] = PointXYori(0.0, 0.0, 0.0); tree.modifyRoot(PointXY(0.0, 0.0)); auto node_actual = tree.getRoot(); double traj_search_start_mid = search_start_mid_deg; - for(int i = 1; i <= search_iter; i++) + SearchConfig config; + for (int i = 1; i <= search_iter; i++) { - traj[i] = grid_map.angualar_search(traj[i-1].x, traj[i-1].y, search_range_deg, search_resolution_deg, traj_search_start_mid, search_length, hpoints, max_true_angle); - node_actual = tree.addNode(node_actual, traj[i]); - // TODO: add alternative paths to tree - traj_search_start_mid = max_true_angle * 180 / M_PI; + for (size_t i = 0; i < traj_end_points.size(); ++i) + { + config.x_start = traj_end_points[i].x; + config.y_start = traj_end_points[i].y; + config.search_range_deg = search_range_deg; + config.search_resolution_deg = search_resolution_deg; + config.search_start_mid_deg = traj_search_start_mid; + config.search_length = search_length; + std::vector actual_end_points = grid_map.angualar_search(config, hpoints, traj_search_angle); + node_actual = tree.addNode(node_actual, traj_end_points[i]); + traj_search_start_mid = traj_search_angle * 180 / M_PI; + // TODO: test this logic with multiple actual end points + for(size_t j = 0; j < actual_end_points.size(); ++j) + { + if (j == 0){ + traj_end_points[i] = actual_end_points[j]; // replace the current end point with the first actual end point + } + else{ + traj_end_points.insert(traj_end_points.begin() + i, actual_end_points[j]); // insert if there are more than one actual end points + } + } + } } visualization_msgs::msg::MarkerArray mark_array; visualization_msgs::msg::Marker debug1_marker, line1_marker; - init_debug_marker(debug1_marker, traj[0].x, traj[0].y, 1); + init_debug_marker(debug1_marker, traj_end_points[0].x, traj_end_points[0].y, 1); init_line_marker(line1_marker, 2); debug1_marker.header.frame_id = input_msg->header.frame_id; debug1_marker.header.stamp = this->now();