diff --git a/mil_common/axros b/mil_common/axros index a86842c93..55395aa65 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit a86842c93370a62c0dad3fba0e6ef250137edf21 +Subproject commit 55395aa65b612ea5d57bc675a15ea0446f6be51a diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp index 626bf073e..7d8f9cd78 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include #include #include #include diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp index f210b7dd8..f1d25a9eb 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/object_associator.cpp @@ -2,13 +2,12 @@ #include #include #include +#include +#include #include #include -#include -#include - namespace pcodar { void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clusters_t clusters, bool moving_back) @@ -16,9 +15,10 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust // Tracks which clusters have been seen std::unordered_set seen; - // Establish Area of Interest (AOI) that new points are not allowed to be published while Navigator is moving backwards - Eigen::Vector3f min_aoi(0.0f, -5.0f, -0.5f); // Min bounds (x, y, z) - Eigen::Vector3f max_aoi(10.0f, 5.0f, 0.5f); // Max bounds (x, y, z) + // Establish Area of Interest (AOI) that new points are not allowed to be published while Navigator is moving + // backwards + Eigen::Vector3f min_aoi(0.0f, -5.0f, -0.5f); // Min bounds (x, y, z) + Eigen::Vector3f max_aoi(10.0f, 5.0f, 0.5f); // Max bounds (x, y, z) // Iterate through each new cluster, finding which persistent cluster(s) it matches for (cluster_t const& cluster : clusters) @@ -48,7 +48,7 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust } // If Navigator is moving back, new objects outside of the AOI are invalid - if(moving_back) + if (moving_back) { // Compute the centroid of the cluster Eigen::Vector4f centroid; @@ -76,9 +76,9 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust // Check if the centroid is outside the AOI bool outside_aoi = (centroid[0] < min_aoi[0] || centroid[0] > max_aoi[0] || // X bounds centroid[1] < min_aoi[1] || centroid[1] > max_aoi[1] || // Y bounds - centroid[2] < min_aoi[2] || centroid[2] > max_aoi[2]); // Z bounds + centroid[2] < min_aoi[2] || centroid[2] > max_aoi[2]); // Z bounds - if(!outside_aoi) + if (!outside_aoi) { std::cout << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << "\n"; continue; @@ -102,11 +102,11 @@ void Associator::associate(ObjectMap& prev_objects, point_cloud const& pc, clust } } } - catch (tf::TransformException &ex) + catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - ros::Duration(1.0).sleep(); - continue; // Skip this iteration if the transform fails + ROS_ERROR("%s", ex.what()); + ros::Duration(1.0).sleep(); + continue; // Skip this iteration if the transform fails } } else diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp index 28738eb67..bc45a1319 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp @@ -143,7 +143,7 @@ void Node::ConfigCallback(Config const& config, uint32_t level) void Node::initialize() { NodeBase::initialize(); - // Subscribe to odom + // Subscribe to odom fl_sub = nh_.subscribe("/wamv/thrusters/FL_thrust_cmd", 1, &Node::thrust_fl_cb, this); fr_sub = nh_.subscribe("/wamv/thrusters/FR_thrust_cmd", 1, &Node::thrust_fr_cb, this);