From 7bab36e700420f7e41c5538c13684df9cc8b8e3d Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Thu, 8 Nov 2018 15:54:09 -0800 Subject: [PATCH 01/39] Risk Analysis structure finished with accompanying rviz publishers --- src/sb_pointcloud_processing/CMakeLists.txt | 13 ++ .../include/RiskAnalysis.h | 81 +++++++++ .../include/RiskAnalysisNode.h | 74 ++++++++ .../src/RiskAnalysis.cpp | 169 ++++++++++++++++++ .../src/RiskAnalysisNode.cpp | 114 ++++++++++++ .../src/risk_analysis.cpp | 20 +++ src/sb_utils/include/RvizUtils.h | 20 +++ src/sb_utils/src/RvizUtils.cpp | 16 ++ 8 files changed, 507 insertions(+) create mode 100644 src/sb_pointcloud_processing/include/RiskAnalysis.h create mode 100644 src/sb_pointcloud_processing/include/RiskAnalysisNode.h create mode 100644 src/sb_pointcloud_processing/src/RiskAnalysis.cpp create mode 100644 src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp create mode 100644 src/sb_pointcloud_processing/src/risk_analysis.cpp diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt index c8faa74d1..29ef6582c 100644 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ b/src/sb_pointcloud_processing/CMakeLists.txt @@ -81,6 +81,13 @@ target_link_libraries(sb_pointcloud_processing ${PCL_LIBRARIES} ) +add_executable(risk_analysis + src/risk_analysis.cpp + src/RiskAnalysisNode.cpp + src/RiskAnalysis.cpp + include/RiskAnalysis.h + include/RiskAnalysisNode.h + ) add_executable(test_pcl_generator_node src/test_pcl_generator_node.cpp test/TestUtils.h @@ -97,6 +104,12 @@ add_dependencies(line_extractor_node ${mapping_igvc_EXPORTED_TARGETS} ) +target_link_libraries(risk_analysis + ${catkin_LIBRARIES} + ${PCL_COMMON_LIBRARIES} + ${PCL_IO_LIBRARIES} + ) + install( TARGETS sb_pointcloud_processing diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h new file mode 100644 index 000000000..1b2386d7f --- /dev/null +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -0,0 +1,81 @@ +/* + * Created By: Robyn Castro + * Created On: October 14, 2018 + * Description: Processes surrounding environment data to create a regional assessment of risk + */ + +#ifndef RISKANALYSIS_H +#define RISKANALYSIS_H + +// Messages +#include +#include + +#include "mapping_msgs_urc/RiskArea.h" +#include "mapping_msgs_urc/RiskAreaArray.h" + +// Point Cloud +#include +#include +#include +#include + +// Standard Libraries +#include + +typedef struct { + geometry_msgs::Polygon region_area; + std::vector points; +} RegionOfPoints; + +class RiskAnalysis { +public: + /** + * Constructor + */ + RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, + int num_horizontal_cell_div, int region_min_points); + + /* + * Required empty constructor + */ + RiskAnalysis(); + + /** + * Analyses a pointcloud + */ + mapping_msgs_urc::RiskAreaArray assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); + + + +private: + + std::vector> initialisePointRegions(pcl::PCLPointCloud2 point_cloud); + + void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector> ®ions); + + mapping_msgs_urc::RiskAreaArray analysePointRegions(std::vector> regions); + + float calculateStandardDeviation(std::vector values); + + geometry_msgs::Polygon getRegionAreaFromIndices(int row, int column); + + int determineRow(float x); + int determineColumn(float y); + + float region_width; + float region_height; + + int num_vertical_cell_div; + int num_horizontal_cell_div; + + int region_min_points; + + float cell_width; + float cell_height; + int total_cells; + +}; + + +#endif //SNOWFLAKE_RISKANALYSIS_H diff --git a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h new file mode 100644 index 000000000..47609f0d9 --- /dev/null +++ b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h @@ -0,0 +1,74 @@ +// +// Created by robyn on 20/10/18. +// + +#ifndef RISKANALYSISNODE_H +#define RISKANALYSISNODE_H + +// ROS +#include + +// Utilities +#include +#include + +// Messages +#include +#include +#include "mapping_msgs_urc/RiskArea.h" +#include "mapping_msgs_urc/RiskAreaArray.h" + +// Point Cloud +#include +#include +#include +#include + +// Risk Analysis +#include "RiskAnalysis.h" + +class RiskAnalysisNode { +public: + + /** + * Constructor + */ + RiskAnalysisNode(int argc, char **argv, std::string node_name); + +private: + + /* + * The callback function is called whenever the node receives a + * PointCloud message. It converts sensor_msgs PointCloud2 pointer + * to PCL PointCloud pointer and then extracts risk areas from the PointCloud + * + * @param point_cloud the received PointCloud message + */ + void pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_cloud); + + void publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas); + + visualization_msgs::Marker::_color_type convertRiskToColor(float risk); + + RiskAnalysis risk_analysis; + + float area_of_interest_width; + float area_of_interest_height; + + int num_vertical_cell_div; + int num_horizontal_cell_div; + + int region_min_points; + + int seq_count; + + mapping_msgs_urc::RiskAreaArray pcl_risk_areas; + + std::vector gradient; + + ros::Subscriber pcl_subscriber; + ros::Publisher risk_publisher; + ros::Publisher risk_marker_publisher; +}; + +#endif //RISKANALYSISNODE_H diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp new file mode 100644 index 000000000..e0f126d54 --- /dev/null +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -0,0 +1,169 @@ +/* + * Created By: Robyn Castro + * Created On: October 14, 2018 + * Description: Processes surrounding environment data to create a regional assessment of risk + */ + + +#include "RiskAnalysis.h" + +RiskAnalysis::RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, int num_horizontal_cell_div, int region_min_points) : + region_width(region_width), + region_height(region_height), + num_vertical_cell_div(num_vertical_cell_div), + num_horizontal_cell_div(num_horizontal_cell_div) { + + cell_width = region_width / num_horizontal_cell_div; + cell_height = region_height / num_vertical_cell_div; + total_cells = num_horizontal_cell_div * num_vertical_cell_div; +} + +RiskAnalysis::RiskAnalysis() { + // Empty Constructor +} + +mapping_msgs_urc::RiskAreaArray RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) +{ + mapping_msgs_urc::RiskAreaArray risk_areas; + + // Initialise regions with an area and associated points + std::vector> regions = initialisePointRegions(point_cloud); + + + // Convert to PCLPointCloud + pcl::PointCloud::Ptr pcl(new pcl::PointCloud()); + pcl::fromPCLPointCloud2(point_cloud, *pcl); + + fillPointRegions(pcl, regions); + + risk_areas = analysePointRegions(regions); + + return risk_areas; +} + +std::vector> RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) +{ + std::vector> regions; + + for (int i = 0; i < num_vertical_cell_div; i++) { + std::vector new_region_row; + for (int j = 0; j < num_horizontal_cell_div; j++) { + RegionOfPoints new_region; + + geometry_msgs::Polygon region_area; + region_area = getRegionAreaFromIndices(i, j); + new_region.region_area = region_area; + + new_region_row.push_back(new_region); + } + regions.push_back(new_region_row); + } + + return regions; +} + +void RiskAnalysis::fillPointRegions(pcl::PointCloud::Ptr pcl, + std::vector> ®ions) { + for (size_t i = 0; i < pcl->size(); i++) { + // Convert PCLPointXYZ to geometry_msgs::Point + geometry_msgs::Point cur_point; + cur_point.x = pcl->points[i].x; + cur_point.y = pcl->points[i].y; + cur_point.z = pcl->points[i].z; + + // Determine which cell the point belongs to + int col = determineRow(pcl->points[i].x); + int row = determineColumn(pcl->points[i].y); + + bool validColumn = col >= 0 && col < regions[0].size(); + bool validRow = row >= 0 && row < regions.size(); + + if (validColumn && validRow) { + regions[row][col].points.push_back(cur_point); + } + } +} + +mapping_msgs_urc::RiskAreaArray RiskAnalysis::analysePointRegions(std::vector> regions) { + + mapping_msgs_urc::RiskAreaArray risk_areas; + + for (int i = 0; i < regions.size(); i++) { + for (int j = 0; j < regions[0].size(); j++) { + if (regions[i][j].points.size() > region_min_points) { + std::vector z_values; + for (int k = 0; k < regions[i][j].points.size(); k++) { + z_values.push_back(regions[i][j].points[k].z); + } + + // Determine risk of the area + std_msgs::Float64 risk; + risk.data = calculateStandardDeviation(z_values); + + // Add risk area onto the array + mapping_msgs_urc::RiskArea risk_area; + risk_area.score = risk; + risk_area.area = regions[i][j].region_area; + risk_areas.areas.push_back(risk_area); + } + } + } + + return risk_areas; +} + +float RiskAnalysis::calculateStandardDeviation(std::vector values) { + float sum = std::accumulate(values.begin(), values.end(), 0.0); + float mean = sum / values.size(); + + std::vector diff(values.size()); + std::transform(values.begin(), values.end(), diff.begin(), + std::bind2nd(std::minus(), mean)); + double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + double stdev = std::sqrt(sq_sum / values.size()); + + return stdev; +} + +geometry_msgs::Polygon RiskAnalysis::getRegionAreaFromIndices(int row, int column) { + geometry_msgs::Point32 top_left_point; + top_left_point.x = region_height - row*cell_height; + top_left_point.y = region_width / 2 - column*cell_width; + top_left_point.z = 0; + + geometry_msgs::Point32 top_right_point; + top_right_point.x = region_height - row*cell_height; + top_right_point.y = region_width / 2 - (column + 1)*cell_width; + top_right_point.z = 0; + + geometry_msgs::Point32 bottom_left_point; + bottom_left_point.x = region_width - (row + 1)*cell_height; + bottom_left_point.y = region_width / 2 - column*cell_width; + bottom_left_point.z = 0; + + geometry_msgs::Point32 bottom_right_point; + bottom_right_point.x = region_width - (row + 1)*cell_height; + bottom_right_point.y = region_width / 2 - (column + 1)*cell_width; + bottom_right_point.z = 0; + + geometry_msgs::Polygon region_area; + region_area.points.push_back(top_left_point); + region_area.points.push_back(top_right_point); + region_area.points.push_back(bottom_right_point); + region_area.points.push_back(bottom_left_point); + + return region_area; + +} + +int RiskAnalysis::determineRow(float x) +{ + int row = x / cell_height; + return row; +} + +int RiskAnalysis::determineColumn(float y) +{ + int col = (y + region_width / 2) / cell_width; + return col; +} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp new file mode 100644 index 000000000..e604c512a --- /dev/null +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -0,0 +1,114 @@ +/* + * Created By: Robyn Castro + * Created On: October 14, 2018 + * Description: Takes in a point cloud and creates a regional assessment. + * + */ + +#include "RiskAnalysisNode.h" + +RiskAnalysisNode::RiskAnalysisNode(int argc, char **argv, std::string node_name) { + // Setup NodeHandles + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + // Initialise parameters + float default_width = 10; + SB_getParam(private_nh, "area_of_interest_width", area_of_interest_width, default_width); + + float default_height = 5; + SB_getParam(private_nh, "area_of_interest_height", area_of_interest_height, default_height); + + int default_vertical_divisions = 10; + SB_getParam(private_nh, "num_vertical_cell_div", num_vertical_cell_div, default_vertical_divisions); + + int default_horizontal_divisions = 30; + SB_getParam(private_nh, "area_of_interest_width", num_horizontal_cell_div, default_horizontal_divisions); + + int default_min_points = 100; + SB_getParam(private_nh, "region_min_points", region_min_points, default_min_points); + + // In the beginning we have not published any markers + seq_count = 0; + + // Since risk goes from 0 - 10, we need 11 gradient values + size_t gradient_size = 11; + double gradient_step = 1.0 / gradient_size; + + gradient.reserve(gradient_size); + + for (int i = 0; i < gradient_size; i++) { + gradient[i] = snowbots::RvizUtils::createMarkerColor(i*gradient_step, gradient_step*(gradient_size - i), 0, 1.0f); + } + + // Initialise Subscribers + std::string topic_to_subscribe_to = "input_pointcloud"; // dummy topic name + int refresh_rate = 10; + pcl_subscriber = nh.subscribe( + topic_to_subscribe_to, refresh_rate, &RiskAnalysisNode::pclCallBack, this); + + // Initialise Publishers + std::string risk_areas_topic = + "output_risk_areas"; // dummy topic name + uint32_t queue_size = 1; + risk_publisher = private_nh.advertise( + risk_areas_topic, queue_size); + + std::string risk_area_markers_topic = + "output_risk_area_markers"; // dummy topic name + + risk_marker_publisher = private_nh.advertise( + risk_area_markers_topic, queue_size); + + // Initialise risk_analysis with parameters + risk_analysis = RiskAnalysis(area_of_interest_width, area_of_interest_height, num_vertical_cell_div, num_horizontal_cell_div, region_min_points); + +} + +void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_cloud) { + pcl::PCLPointCloud2 pcl_pc2; + + // convert sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 + pcl_conversions::toPCL(*point_cloud, pcl_pc2); + + // extract risk areas + pcl_risk_areas = risk_analysis.assessPointCloudRisk(pcl_pc2); + + publishMarkers(pcl_risk_areas); + + visualization_msgs::MarkerArray risk_area_markers; + std::string frame_id; + std::string ns = "debug"; + + for (int i = 0; i < pcl_risk_areas.areas.size(); i++) { + visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker( + pcl_risk_areas.areas[i].area, + convertRiskToColor(pcl_risk_areas.areas[i].score.data), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns + ); + + risk_area_markers.markers.push_back(risk_area_marker); + } + + // Visualise markers + risk_marker_publisher.publish(risk_area_markers); +} + +void RiskAnalysisNode::publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas) { + risk_areas.header.frame_id = + + risk_areas.header.seq = seq_count; + seq_count++; + + risk_areas.header.stamp = ros::Time::now(); + + risk_publisher.publish(risk_areas); +} + +visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { + // Round down to the nearest integer to get risk color + return gradient[(int) risk]; +} \ No newline at end of file diff --git a/src/sb_pointcloud_processing/src/risk_analysis.cpp b/src/sb_pointcloud_processing/src/risk_analysis.cpp new file mode 100644 index 000000000..7d5d766bc --- /dev/null +++ b/src/sb_pointcloud_processing/src/risk_analysis.cpp @@ -0,0 +1,20 @@ +/* + * Created By: Robyn Castro + * Created On: October 14, 2018 + * Description: Spawns Risk Analysis Node + */ + +#include "RiskAnalysisNode.h" + +#include + +int main(int argc, char** argv) { + // Setup your ROS node + std::string node_name = "risk_analysis"; + // Create an instance of your class + RiskAnalysisNode risk_analysis(argc, argv, node_name); + // Start up ROS, this will continue to run until the node is killed + ros::spin(); + // Once the node stops, return 0 + return 0; +} diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index ce2b42a0d..9aa5c6178 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -9,6 +9,8 @@ // Messages #include +#include + #include #include @@ -73,6 +75,24 @@ class RvizUtils { int type = visualization_msgs::Marker::POINTS, int id = 0); + /** + * Turn a polygon into a marker for rviz + * + * @param polygon the polygon to be converted + * @param color the color of the point + * @param frame_id the frame id + * @param ns the namespace + * + * @return an rviz marker + */ + static visualization_msgs::Marker + createPolygonMarker(geometry_msgs::Polygon polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1); /** * Creates a Marker Array (array of Markers) * diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 3d86beb9a..5c123fe67 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -70,6 +70,22 @@ Marker RvizUtils::createMarker(geometry_msgs::Point point, return marker; } +Marker RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, std::string frame_id, + std::string ns, int type, int id) { + Marker marker; + + setupMarker(marker, scale, frame_id, ns, type, id); + + // Set the color + marker.color = color; + + // Setup the line strip + marker.points = polygon.points; + + return marker; +} + MarkerArray RvizUtils::createMarkerArray(vector> points_array, Marker::_color_type color, From 6cbbb64bb52b06bb877df1fd4f2d11833b60ec49 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 17 Nov 2018 08:16:38 -0800 Subject: [PATCH 02/39] Added some unit tests for Risk Analysis --- src/sb_pointcloud_processing/CMakeLists.txt | 2 + .../include/RiskAnalysis.h | 18 +- .../launch/risk_analysis.launch | 17 ++ .../src/RiskAnalysis.cpp | 10 +- .../src/RiskAnalysisNode.cpp | 16 +- .../test/risk-analysis-test.cpp | 213 ++++++++++++++++++ 6 files changed, 254 insertions(+), 22 deletions(-) create mode 100644 src/sb_pointcloud_processing/launch/risk_analysis.launch create mode 100644 src/sb_pointcloud_processing/test/risk-analysis-test.cpp diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt index 29ef6582c..97263ad3c 100644 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ b/src/sb_pointcloud_processing/CMakeLists.txt @@ -148,6 +148,8 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(colourspace-converter-test test/colourspace-converter-test.cpp include/ColourspaceConverter.h src/ColourspaceConverter.cpp) target_link_libraries(colourspace-converter-test ${PCL_LIBRARIES}) + catkin_add_gtest(risk-analysis-test test/risk-analysis-test.cpp src/RiskAnalysis.cpp include/RiskAnalysis.h) + target_link_libraries(risk-analysis-test ${PCL_LIBRARIES}) # Adding rostest to the package find_package(rostest REQUIRED) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index 1b2386d7f..f4cd33f7c 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -36,7 +36,7 @@ class RiskAnalysis { RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, int num_horizontal_cell_div, int region_min_points); - /* + /** * Required empty constructor */ RiskAnalysis(); @@ -46,22 +46,22 @@ class RiskAnalysis { */ mapping_msgs_urc::RiskAreaArray assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); + std::vector> initialisePointRegions(pcl::PCLPointCloud2 point_cloud); + void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector> ®ions); -private: + mapping_msgs_urc::RiskAreaArray analysePointRegions(std::vector> regions); - std::vector> initialisePointRegions(pcl::PCLPointCloud2 point_cloud); + float calculateStandardDeviation(std::vector values); - void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector> ®ions); + geometry_msgs::Polygon getRegionAreaFromIndices(int row, int column); - mapping_msgs_urc::RiskAreaArray analysePointRegions(std::vector> regions); + int determineRow(float x); - float calculateStandardDeviation(std::vector values); + int determineColumn(float y); +private: - geometry_msgs::Polygon getRegionAreaFromIndices(int row, int column); - int determineRow(float x); - int determineColumn(float y); float region_width; float region_height; diff --git a/src/sb_pointcloud_processing/launch/risk_analysis.launch b/src/sb_pointcloud_processing/launch/risk_analysis.launch new file mode 100644 index 000000000..8f4dbe7ed --- /dev/null +++ b/src/sb_pointcloud_processing/launch/risk_analysis.launch @@ -0,0 +1,17 @@ + + + + + + area_of_interest_width: 5 + area_of_interest_height: 5 + num_vertical_cell_div: 50 + num_horizontal_cell_div: 50 + region_min_points: 10 + + + + diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index e0f126d54..9fcd0611a 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -128,22 +128,22 @@ float RiskAnalysis::calculateStandardDeviation(std::vector values) { geometry_msgs::Polygon RiskAnalysis::getRegionAreaFromIndices(int row, int column) { geometry_msgs::Point32 top_left_point; top_left_point.x = region_height - row*cell_height; - top_left_point.y = region_width / 2 - column*cell_width; + top_left_point.y = region_width / 2.0 - column*cell_width; top_left_point.z = 0; geometry_msgs::Point32 top_right_point; top_right_point.x = region_height - row*cell_height; - top_right_point.y = region_width / 2 - (column + 1)*cell_width; + top_right_point.y = region_width / 2.0 - (column + 1)*cell_width; top_right_point.z = 0; geometry_msgs::Point32 bottom_left_point; bottom_left_point.x = region_width - (row + 1)*cell_height; - bottom_left_point.y = region_width / 2 - column*cell_width; + bottom_left_point.y = region_width / 2.0 - column*cell_width; bottom_left_point.z = 0; geometry_msgs::Point32 bottom_right_point; - bottom_right_point.x = region_width - (row + 1)*cell_height; - bottom_right_point.y = region_width / 2 - (column + 1)*cell_width; + bottom_right_point.x = region_width - (row + 1.0)*cell_height; + bottom_right_point.y = region_width / 2.0 - (column + 1.0)*cell_width; bottom_right_point.z = 0; geometry_msgs::Polygon region_area; diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index e604c512a..f9039450d 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -72,19 +72,19 @@ void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_ // convert sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 pcl_conversions::toPCL(*point_cloud, pcl_pc2); - // extract risk areas - pcl_risk_areas = risk_analysis.assessPointCloudRisk(pcl_pc2); + // extract risk + pcl_risk = risk_analysis.assessPointCloudRisk(pcl_pc2); - publishMarkers(pcl_risk_areas); + publishMarkers(pcl_risk); visualization_msgs::MarkerArray risk_area_markers; - std::string frame_id; + std::string frame_id = "camera_color_optical_frame"; std::string ns = "debug"; - for (int i = 0; i < pcl_risk_areas.areas.size(); i++) { + for (int i = 0; i < pcl_risk.areas.size(); i++) { visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker( - pcl_risk_areas.areas[i].area, - convertRiskToColor(pcl_risk_areas.areas[i].score.data), + pcl_risk.areas[i].area, + convertRiskToColor(pcl_risk.areas[i].score.data), snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), frame_id, ns @@ -98,7 +98,7 @@ void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_ } void RiskAnalysisNode::publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas) { - risk_areas.header.frame_id = + risk_areas.header.frame_id = "camera_color_optical_frame"; risk_areas.header.seq = seq_count; seq_count++; diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp new file mode 100644 index 000000000..89c8ba8f6 --- /dev/null +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -0,0 +1,213 @@ +/* + * Created By: Robyn Castro + * Created On: October 31, 2018 + * Description: Tests calculation of risks + */ + +#include "./TestUtils.h" +#include "mapping_msgs_urc/RiskAreaArray.h" + +#include +#include + +/* + * Function Prototype for Risk Analysis + * + * RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, + * int num_horizontal_cell_div, int region_min_points); + * + */ + +pcl::PCLPointCloud2 empty_cloud = pcl::PCLPointCloud2(); +void testRegionDimensions(std::vector> point_region, float region_width, float region_height, + int num_vertical_cell_divs, int num_horizontal_cell_divs); + +TEST(RegionCreation, DivisionsEqualToDimensions) { + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; + int num_horizontal_cell_divs = 10; + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, + num_vertical_cell_divs, num_horizontal_cell_divs, + min_points_in_region); + + geometry_msgs::Polygon top_left_region = risk_analysis.getRegionAreaFromIndices(0,0); + EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); + EXPECT_FLOAT_EQ(region_width/2, top_left_region.points[0].y); + + geometry_msgs::Polygon top_right_region = risk_analysis.getRegionAreaFromIndices(0,9); + EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); + EXPECT_FLOAT_EQ(-region_width/2, top_right_region.points[1].y); + + geometry_msgs::Polygon bottom_right_region = risk_analysis.getRegionAreaFromIndices(9,9); + EXPECT_FLOAT_EQ(0, bottom_right_region.points[2].x); + EXPECT_FLOAT_EQ(-region_width/2, bottom_right_region.points[2].y); + + geometry_msgs::Polygon bottom_left_region = risk_analysis.getRegionAreaFromIndices(9,0); + EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); + EXPECT_FLOAT_EQ(region_width/2, bottom_left_region.points[3].y); + +} + +TEST(RegionInitialisation, DivisionsEqualToDimensions) { + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; + int num_horizontal_cell_divs = 10; + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, + num_vertical_cell_divs, num_horizontal_cell_divs, + min_points_in_region); + + std::vector> point_region; + + point_region = risk_analysis.initialisePointRegions(empty_cloud); + + EXPECT_EQ(num_vertical_cell_divs, point_region.size()); + EXPECT_EQ(num_horizontal_cell_divs, point_region[0].size()); + + testRegionDimensions(point_region, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); +} + +TEST(RegionAnalysis, DivisionsEqualToDimensions) { + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; + int num_horizontal_cell_divs = 10; + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, + num_vertical_cell_divs, num_horizontal_cell_divs, + min_points_in_region); + + mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); +} + +TEST(RegionCreation, NonIntegerCellDimensions) { + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 100; + int num_horizontal_cell_divs = 100; + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, + num_vertical_cell_divs, num_horizontal_cell_divs, + min_points_in_region); + + geometry_msgs::Polygon top_left_region = risk_analysis.getRegionAreaFromIndices(0,0); + EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); + EXPECT_FLOAT_EQ(region_width/2, top_left_region.points[0].y); + + geometry_msgs::Polygon top_right_region = risk_analysis.getRegionAreaFromIndices(0,99); + EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); + EXPECT_FLOAT_EQ(-region_width/2, top_right_region.points[1].y); + + geometry_msgs::Polygon bottom_right_region = risk_analysis.getRegionAreaFromIndices(99,99); + EXPECT_NEAR(0, bottom_right_region.points[2].x, 0.001); + EXPECT_FLOAT_EQ(-region_width/2, bottom_right_region.points[2].y); + + geometry_msgs::Polygon bottom_left_region = risk_analysis.getRegionAreaFromIndices(99,0); + EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); + EXPECT_FLOAT_EQ(region_width/2, bottom_left_region.points[3].y); + +} + +TEST(RegionAnalysis, NonIntegerCellDimensions) { + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 100; + int num_horizontal_cell_divs = 100; + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, + num_vertical_cell_divs, num_horizontal_cell_divs, + min_points_in_region); + + mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); +} + + + +void testRegionDimensions(std::vector> point_region, float region_width, float region_height, + int num_vertical_cell_divs, int num_horizontal_cell_divs) { + float cell_width = region_width / num_horizontal_cell_divs; + float cell_height = region_height / num_vertical_cell_divs; + + for (int i = 0; i < point_region.size(); i++) { + for (int j = 0; j < point_region[0].size(); j++) { + geometry_msgs::Polygon cur_cell = point_region[i][j].region_area; + + // Top Left Point vs Bottom Left Point + EXPECT_FLOAT_EQ(cell_height, cur_cell.points[0].x - cur_cell.points[3].x); + + // Top Right Point vs Bottom Right Point + EXPECT_FLOAT_EQ(cell_height, cur_cell.points[1].x - cur_cell.points[2].x); + + // Top Left Point vs Top Right Point + EXPECT_FLOAT_EQ(cell_width, cur_cell.points[0].y - cur_cell.points[1].y); + + // Bottom Left Point vs Bottom Right Point + EXPECT_FLOAT_EQ(cell_width, cur_cell.points[3].y - cur_cell.points[2].y); + + } + } +} + +TEST(StandardDeviation, OneDataPoint) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {1}; + + EXPECT_FLOAT_EQ(0.0, risk_analysis.calculateStandardDeviation(data_set)); +} + +TEST(StandardDeviation, TwoEqualDataPoints) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {1, 1}; + + EXPECT_FLOAT_EQ(0.0, risk_analysis.calculateStandardDeviation(data_set)); +} + +TEST(StandardDeviation, TwoUnequalEqualDataPoints) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {1, 2}; + + EXPECT_FLOAT_EQ(0.5, risk_analysis.calculateStandardDeviation(data_set)); +} + +TEST(StandardDeviation, NonIntegerDataPoints) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {5.4, 3.4, 8, 100, 20, 5.5}; + + EXPECT_NEAR(34.54487018, risk_analysis.calculateStandardDeviation(data_set), 0.01); +} + +TEST(StandardDeviation, NegativeNonIntegerDataPoints) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {-5.4, 3.4, -8, 100, -20, 5.5}; + + EXPECT_NEAR(39.96858836, risk_analysis.calculateStandardDeviation(data_set), 0.01); +} + +TEST(StandardDeviation, RandomDataPoints) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {1, 2, -7, 5.4, 3.4, 8, 100, 20, -5}; + + EXPECT_NEAR(31.21424176, risk_analysis.calculateStandardDeviation(data_set), 0.01); +} + +TEST(StandardDeviation, RandomDataPointsLargeNumbers) { + RiskAnalysis risk_analysis = RiskAnalysis(); + + std::vector data_set = {1, 2, -7, 5.4, 3.4, 8, 100, 20, -5000, 254000, 5, -200080}; + + EXPECT_NEAR(93261.48782, risk_analysis.calculateStandardDeviation(data_set), 0.01); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From cbc6530c227e7cc678c53f0580e307cccb8bf075 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 17 Nov 2018 08:36:03 -0800 Subject: [PATCH 03/39] Added rviz polygon markers --- src/sb_utils/include/RvizUtils.h | 20 ++++++++++++++++++++ src/sb_utils/src/RvizUtils.cpp | 16 ++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index ce2b42a0d..9aa5c6178 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -9,6 +9,8 @@ // Messages #include +#include + #include #include @@ -73,6 +75,24 @@ class RvizUtils { int type = visualization_msgs::Marker::POINTS, int id = 0); + /** + * Turn a polygon into a marker for rviz + * + * @param polygon the polygon to be converted + * @param color the color of the point + * @param frame_id the frame id + * @param ns the namespace + * + * @return an rviz marker + */ + static visualization_msgs::Marker + createPolygonMarker(geometry_msgs::Polygon polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1); /** * Creates a Marker Array (array of Markers) * diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 3d86beb9a..5c123fe67 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -70,6 +70,22 @@ Marker RvizUtils::createMarker(geometry_msgs::Point point, return marker; } +Marker RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, std::string frame_id, + std::string ns, int type, int id) { + Marker marker; + + setupMarker(marker, scale, frame_id, ns, type, id); + + // Set the color + marker.color = color; + + // Setup the line strip + marker.points = polygon.points; + + return marker; +} + MarkerArray RvizUtils::createMarkerArray(vector> points_array, Marker::_color_type color, From 78483206fda2895700d5abd3fa8427394173702e Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 17 Nov 2018 08:41:26 -0800 Subject: [PATCH 04/39] Fixed a function javadoc commentfor createPolygonMarker --- src/sb_utils/include/RvizUtils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index 9aa5c6178..3330c93d6 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -79,7 +79,7 @@ class RvizUtils { * Turn a polygon into a marker for rviz * * @param polygon the polygon to be converted - * @param color the color of the point + * @param color the color of the polygon * @param frame_id the frame id * @param ns the namespace * From 9021bc48864306a06483eb613d2b70be23ea0d29 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 17 Nov 2018 09:05:56 -0800 Subject: [PATCH 05/39] Resolved type mismatch in RvizUtils --- src/sb_utils/src/RvizUtils.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 5c123fe67..87931f0de 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -81,7 +81,12 @@ Marker RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, visualizat marker.color = color; // Setup the line strip - marker.points = polygon.points; + marker.points.reserve(polygon.points.size()); + for (int i = 0; i < polygon.points.size(); i++) { + marker.points[i].x = polygon.points[i].x; + marker.points[i].y = polygon.points[i].y; + marker.points[i].z = polygon.points[i].z; + } return marker; } From 7ea488f57c09847cfc100109e64bffbe75b17728 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 1 Dec 2018 11:23:55 -0800 Subject: [PATCH 06/39] Ran fix formatting script --- src/sb_utils/CMakeLists.txt | 17 +- src/sb_utils/include/RvizUtils.h | 34 +++- src/sb_utils/src/RvizUtils.cpp | 71 +++++--- src/sb_utils/test/rviz_utils_rostest.cpp | 209 +++++++++++++++++++++++ src/sb_utils/test/rviz_utils_test.test | 8 + 5 files changed, 306 insertions(+), 33 deletions(-) create mode 100644 src/sb_utils/test/rviz_utils_rostest.cpp create mode 100644 src/sb_utils/test/rviz_utils_test.test diff --git a/src/sb_utils/CMakeLists.txt b/src/sb_utils/CMakeLists.txt index b20019c0b..47ec811ef 100644 --- a/src/sb_utils/CMakeLists.txt +++ b/src/sb_utils/CMakeLists.txt @@ -66,7 +66,16 @@ target_link_libraries(sb_utils ${catkin_LIBRARIES}) ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_utils_lib.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +if (CATKIN_ENABLE_TESTING) + + # Adding gtests to the package + catkin_add_gtest(rviz_utils_rostest + test/rviz_utils_test.test + test/rviz_utils_rostest.cpp + src/RvizUtils.cpp + include/RvizUtils.h + ) + + target_link_libraries(rviz_utils_rostest ${catkin_LIBRARIES}) + +endif() diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index 3330c93d6..ecab343f4 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -7,6 +7,9 @@ #ifndef SB_UTILS_RVIZUTILS_H #define SB_UTILS_RVIZUTILS_H +// ROS +#include + // Messages #include #include @@ -75,6 +78,25 @@ class RvizUtils { int type = visualization_msgs::Marker::POINTS, int id = 0); + /** + * Turn a point into a marker for rviz + * + * @param point the point to be converted + * @param color the color of the point + * @param frame_id the frame id + * @param ns the namespace + * + * @return an rviz marker + */ + static visualization_msgs::Marker + createMarker(geometry_msgs::Point point, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::POINTS, + int id = 0); + /** * Turn a polygon into a marker for rviz * @@ -87,12 +109,12 @@ class RvizUtils { */ static visualization_msgs::Marker createPolygonMarker(geometry_msgs::Polygon polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1); + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1); /** * Creates a Marker Array (array of Markers) * diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 87931f0de..6d2afe514 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -6,15 +6,14 @@ */ #include -using namespace std; using namespace visualization_msgs; using namespace snowbots; -Marker RvizUtils::createMarker(vector points, +Marker RvizUtils::createMarker(std::vector points, Marker::_color_type color, Marker::_scale_type scale, - string frame_id, - string ns, + std::string frame_id, + std::string ns, int type, int id) { Marker marker; @@ -30,11 +29,11 @@ Marker RvizUtils::createMarker(vector points, return marker; } -Marker RvizUtils::createMarker(vector points, +Marker RvizUtils::createMarker(std::vector points, std::vector colors, Marker::_scale_type scale, - string frame_id, - string ns, + std::string frame_id, + std::string ns, int type, int id) { Marker marker; @@ -53,8 +52,8 @@ Marker RvizUtils::createMarker(vector points, Marker RvizUtils::createMarker(geometry_msgs::Point point, std::vector colors, Marker::_scale_type scale, - string frame_id, - string ns, + std::string frame_id, + std::string ns, int type, int id) { Marker marker; @@ -70,9 +69,34 @@ Marker RvizUtils::createMarker(geometry_msgs::Point point, return marker; } -Marker RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, std::string frame_id, - std::string ns, int type, int id) { +Marker RvizUtils::createMarker(geometry_msgs::Point point, + Marker::_color_type color, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { + Marker marker; + + setupMarker(marker, scale, frame_id, ns, type, id); + + // Set the color + marker.color = color; + + // Set the points + marker.points.push_back(point); + + return marker; +} + +Marker +RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -81,23 +105,24 @@ Marker RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, visualizat marker.color = color; // Setup the line strip - marker.points.reserve(polygon.points.size()); for (int i = 0; i < polygon.points.size(); i++) { - marker.points[i].x = polygon.points[i].x; - marker.points[i].y = polygon.points[i].y; - marker.points[i].z = polygon.points[i].z; + geometry_msgs::Point point; + point.x = polygon.points[i].x; + point.y = polygon.points[i].y; + point.z = polygon.points[i].z; + marker.points.push_back(point); } return marker; } -MarkerArray -RvizUtils::createMarkerArray(vector> points_array, - Marker::_color_type color, - Marker::_scale_type scale, - string frame_id, - string ns, - int type) { +MarkerArray RvizUtils::createMarkerArray( +std::vector> points_array, +Marker::_color_type color, +Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int type) { visualization_msgs::MarkerArray markerArray; for (unsigned int i = 0; i < points_array.size(); i++) { Marker marker = diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp new file mode 100644 index 000000000..1278f6a9d --- /dev/null +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -0,0 +1,209 @@ +#include "RvizUtils.h" +#include + +/** + * This is the helper class which will publish and subscribe messages which will + * test the node being instantiated + * It contains at the minimum: + * publisher - publishes the input to the node + * subscriber - publishes the output of the node + * callback function - the callback function which corresponds to the + * subscriber + * getter function - to provide a way for gtest to check for equality of + * the message recieved + */ +class RvizUtilsRosTest : public testing::Test { + protected: + ros::NodeHandle nh; + + // This class doesn't publish or subscribe currently, so no initialisation + // necessary. + ros::Publisher test_publisher; + ros::Subscriber test_subscriber; +}; + +template +void EXPECT_POINT_EQ(std::vector points1, std::vector points2); + +geometry_msgs::Point32 initialisePoint32(float x, float y, float z) { + geometry_msgs::Point32 point; + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +geometry_msgs::Point initialisePoint(float x, float y, float z) { + geometry_msgs::Point point; + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +TEST_F(RvizUtilsRosTest, boxPolygonMarker) { + std::string frame_id = "camera_color_optical_frame"; + std::string ns = "debug"; + + geometry_msgs::Polygon polygon; + polygon.points.push_back(initialisePoint32(0, 0, 0)); + polygon.points.push_back(initialisePoint32(1, 0, 0)); + polygon.points.push_back(initialisePoint32(0, 1, 0)); + polygon.points.push_back(initialisePoint32(1, 1, 0)); + + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createPolygonMarker( + polygon, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns); + + // Make sure message metadata is correct + EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); + EXPECT_EQ(ns, risk_area_marker.ns); + + EXPECT_POINT_EQ(polygon.points, risk_area_marker.points); +} + +TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { + std::string frame_id = "camera_color_optical_frame"; + std::string ns = "debug"; + + geometry_msgs::Polygon polygon; + polygon.points.push_back(initialisePoint32(0, 0, 0)); + polygon.points.push_back(initialisePoint32(1, 0, 0)); + polygon.points.push_back(initialisePoint32(0, 1, 0)); + + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createPolygonMarker( + polygon, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns); + + // Make sure message metadata is correct + EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); + EXPECT_EQ(ns, risk_area_marker.ns); + + EXPECT_POINT_EQ(polygon.points, risk_area_marker.points); +} + +TEST_F(RvizUtilsRosTest, singlePointMarker) { + std::string frame_id = "camera_color_optical_frame"; + std::string ns = "debug"; + + geometry_msgs::Point point = initialisePoint(0, 0, 1); + + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createMarker( + initialisePoint(0, 0, 1), + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns); + + // Make sure message metadata is correct + EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); + EXPECT_EQ(ns, risk_area_marker.ns); + + EXPECT_EQ(point.x, risk_area_marker.points[0].x); + EXPECT_EQ(point.y, risk_area_marker.points[0].y); + EXPECT_EQ(point.z, risk_area_marker.points[0].z); +} + +TEST_F(RvizUtilsRosTest, singlePointMarkerInArray) { + std::string frame_id = "camera_color_optical_frame"; + std::string ns = "debug"; + + std::vector points; + points.push_back(initialisePoint(0, 0, 1)); + + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createMarker( + points, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns); + + // Make sure message metadata is correct + EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); + EXPECT_EQ(ns, risk_area_marker.ns); + + EXPECT_POINT_EQ(points, risk_area_marker.points); +} + +TEST_F(RvizUtilsRosTest, multiplePointMarkersInArray) { + std::string frame_id = "camera_color_optical_frame"; + std::string ns = "debug"; + + std::vector points; + points.push_back(initialisePoint(0, 0, 1)); + points.push_back(initialisePoint(0, 0, 1)); + points.push_back(initialisePoint(0, -8, 1)); + points.push_back(initialisePoint(100, 0, 1)); + + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createMarker( + points, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + frame_id, + ns); + + // Make sure message metadata is correct + EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); + EXPECT_EQ(ns, risk_area_marker.ns); +} + +TEST_F(RvizUtilsRosTest, createMarkerScale) { + float x = 0; + float y = -1; + float z = 1; + + visualization_msgs::Marker::_scale_type scale; + scale = snowbots::RvizUtils::createrMarkerScale(x, y, z); + + EXPECT_FLOAT_EQ(x, scale.x); + EXPECT_FLOAT_EQ(y, scale.y); + EXPECT_FLOAT_EQ(z, scale.z); +} + +TEST_F(RvizUtilsRosTest, createMarkerColor) { + float r = 0; + float g = 0.5f; + float b = 1.0f; + float a = 1.0f; + + visualization_msgs::Marker::_color_type color; + color = snowbots::RvizUtils::createMarkerColor(r, g, b, a); + + EXPECT_FLOAT_EQ(r, color.r); + EXPECT_FLOAT_EQ(g, color.g); + EXPECT_FLOAT_EQ(b, color.b); + EXPECT_FLOAT_EQ(a, color.a); +} + +template +void EXPECT_POINT_EQ(std::vector points1, std::vector points2) { + // Check for equal size + EXPECT_EQ(points1.size(), points2.size()); + + if (points1.size() == points2.size()) { + for (int i = 0; i < points1.size(); i++) { + EXPECT_EQ(points1[i].x, points2[i].x); + EXPECT_EQ(points1[i].y, points2[i].y); + EXPECT_EQ(points1[i].z, points2[i].z); + } + } +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "rviz_utils_rostest"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/src/sb_utils/test/rviz_utils_test.test b/src/sb_utils/test/rviz_utils_test.test new file mode 100644 index 000000000..a0be3ef14 --- /dev/null +++ b/src/sb_utils/test/rviz_utils_test.test @@ -0,0 +1,8 @@ + + + + + \ No newline at end of file From a6e82c57793876e264d2b8471274f1249d197e27 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 1 Dec 2018 11:35:23 -0800 Subject: [PATCH 07/39] Added point equality testing inside multiplePointMarkersTest --- src/sb_utils/test/rviz_utils_rostest.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 1278f6a9d..704e401c3 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -158,6 +158,8 @@ TEST_F(RvizUtilsRosTest, multiplePointMarkersInArray) { // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); EXPECT_EQ(ns, risk_area_marker.ns); + + EXPECT_POINT_EQ(points, risk_area_marker.points); } TEST_F(RvizUtilsRosTest, createMarkerScale) { From 6083b46961dfc80090c760a2537bed3db50d9ee3 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 1 Dec 2018 11:37:34 -0800 Subject: [PATCH 08/39] Ran fix formatting script --- src/sb_utils/test/rviz_utils_rostest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 704e401c3..13dfb59fe 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -158,7 +158,7 @@ TEST_F(RvizUtilsRosTest, multiplePointMarkersInArray) { // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); EXPECT_EQ(ns, risk_area_marker.ns); - + EXPECT_POINT_EQ(points, risk_area_marker.points); } From ae0d4c04358ccac0f06ca851ed58d98dd04fbca9 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 1 Dec 2018 11:59:50 -0800 Subject: [PATCH 09/39] Revert "Revert "Pegged librealsense Version as a Temporary Fix (#359)" (#361)" This reverts commit 863ae21820dd296199fa0dc67694983a1cef6e52. --- setup_scripts/setup_realsense.sh | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/setup_scripts/setup_realsense.sh b/setup_scripts/setup_realsense.sh index 9fc43a042..7e65eeb49 100755 --- a/setup_scripts/setup_realsense.sh +++ b/setup_scripts/setup_realsense.sh @@ -11,9 +11,20 @@ sudo apt-get install -y software-properties-common sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u sudo rm -f /etc/apt/sources.list.d/realsense-public.list + sudo apt-get update -y -sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg +# TODO (#359): We can (and SHOULD) switch back to using the below line once +# the ros realsense packages are updated to work with librealsense2 2.16.2 +# see https://github.com/intel-ros/realsense/issues/502 for updates. +# (Right now we just peg the version to 2.16.1) +# sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg +version="2.16.1-0~realsense0.51" +sudo apt-get install librealsense2-dkms -y +sudo apt install librealsense2=${version} -y +sudo apt-get install librealsense2-utils=${version} -y +sudo apt-get install librealsense2-dev=${version} -y +sudo apt-get install librealsense2-dbg=${version} -y echo "================================================================" echo "Finished configuring realsense. " From 6ff67fd3ae43908e1b50fbf21d2eb924332ceb93 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 1 Dec 2018 12:26:22 -0800 Subject: [PATCH 10/39] Changed setup_realsense realsense version to a working version --- setup_scripts/setup_realsense.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup_scripts/setup_realsense.sh b/setup_scripts/setup_realsense.sh index 7e65eeb49..623a1e472 100755 --- a/setup_scripts/setup_realsense.sh +++ b/setup_scripts/setup_realsense.sh @@ -19,7 +19,7 @@ sudo apt-get update -y # see https://github.com/intel-ros/realsense/issues/502 for updates. # (Right now we just peg the version to 2.16.1) # sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg -version="2.16.1-0~realsense0.51" +version="2.16.1-0~realsense0.13" sudo apt-get install librealsense2-dkms -y sudo apt install librealsense2=${version} -y sudo apt-get install librealsense2-utils=${version} -y From 039c048d06178da29d85e44ace3d646f14a95265 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Wed, 12 Dec 2018 17:31:39 -0800 Subject: [PATCH 11/39] Updated realsense version --- setup_scripts/setup_realsense.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup_scripts/setup_realsense.sh b/setup_scripts/setup_realsense.sh index 623a1e472..20823b301 100755 --- a/setup_scripts/setup_realsense.sh +++ b/setup_scripts/setup_realsense.sh @@ -19,7 +19,7 @@ sudo apt-get update -y # see https://github.com/intel-ros/realsense/issues/502 for updates. # (Right now we just peg the version to 2.16.1) # sudo apt-get install -y librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg -version="2.16.1-0~realsense0.13" +version="2.16.3-0~realsense0.116" sudo apt-get install librealsense2-dkms -y sudo apt install librealsense2=${version} -y sudo apt-get install librealsense2-utils=${version} -y From 9e13c99ba7c1f33f24520ddb0847cbbb058ff05c Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 5 Jan 2019 07:08:51 -0800 Subject: [PATCH 12/39] Fixed install_tools script --- setup_scripts/install_tools.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup_scripts/install_tools.sh b/setup_scripts/install_tools.sh index 59f46e0d0..21cd135c3 100755 --- a/setup_scripts/install_tools.sh +++ b/setup_scripts/install_tools.sh @@ -54,7 +54,7 @@ done ################# # Install CLion # ################# -sudo snap install clion +sudo snap install clion --classic ################### # Install Arduino # From c07370dfe5b2e2e98c651bedc9d8c979c66b3935 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 12 Jan 2019 10:13:41 -0800 Subject: [PATCH 13/39] Fixed RvizUtils templated function --- src/decision_igvc/src/GpsManager.cpp | 3 +- .../include/RiskAnalysis.h | 56 +++--- .../include/RiskAnalysisNode.h | 21 +-- .../src/LineExtractorNode.cpp | 8 +- .../src/RiskAnalysis.cpp | 80 ++++---- .../src/RiskAnalysisNode.cpp | 94 ++++++---- .../test/risk-analysis-test.cpp | 173 +++++++++++------- src/sb_utils/include/RvizUtils.h | 47 +++-- src/sb_utils/src/RvizUtils.cpp | 162 +++++++--------- src/sb_utils/test/rviz_utils_rostest.cpp | 24 +-- 10 files changed, 368 insertions(+), 300 deletions(-) diff --git a/src/decision_igvc/src/GpsManager.cpp b/src/decision_igvc/src/GpsManager.cpp index fa909a8dd..2b4379864 100644 --- a/src/decision_igvc/src/GpsManager.cpp +++ b/src/decision_igvc/src/GpsManager.cpp @@ -104,6 +104,7 @@ geometry_msgs::TransformStamped global_to_local_transform) { tf2::doTransform(p, output, t_stamped); output.point.z = 0; // Necessary to remove vertical transform of waypoint + snowbots::RvizUtils::createMarkerScale(0, 0, 0); // Create marker std::vector colors; std_msgs::ColorRGBA color; @@ -113,7 +114,7 @@ geometry_msgs::TransformStamped global_to_local_transform) { rviz_marker_publisher.publish(snowbots::RvizUtils::createMarker( output.point, colors, - snowbots::RvizUtils::createrMarkerScale(0.5, 0.5, 0.5), + snowbots::RvizUtils::createMarkerScale(0.5, 0.5, 0.5), base_frame, "debug", visualization_msgs::Marker::POINTS)); diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index 53de66266..bdac67c1e 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -1,15 +1,16 @@ /* * Created By: Robyn Castro * Created On: October 14, 2018 - * Description: Processes surrounding environment data to create a regional assessment of risk + * Description: Processes surrounding environment data to create a regional + * assessment of risk */ #ifndef RISKANALYSIS_H #define RISKANALYSIS_H // Messages -#include #include +#include #include "mapping_msgs_urc/RiskArea.h" #include "mapping_msgs_urc/RiskAreaArray.h" @@ -32,40 +33,45 @@ typedef struct { } RegionOfPoints; class RiskAnalysis { -public: + public: /** * Constructor */ - RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, - int num_horizontal_cell_div, int region_min_points); - - /** - * Required empty constructor - */ - RiskAnalysis(); + RiskAnalysis(float region_width, + float region_height, + int num_vertical_cell_div, + int num_horizontal_cell_div, + int region_min_points); - /** - * Analyses a pointcloud - */ - mapping_msgs_urc::RiskAreaArray assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); - - std::vector> initialisePointRegions(pcl::PCLPointCloud2 point_cloud); + /** + * Required empty constructor + */ + RiskAnalysis(); - void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector> ®ions); + /** + * Analyses a pointcloud + */ + mapping_msgs_urc::RiskAreaArray + assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); - mapping_msgs_urc::RiskAreaArray analysePointRegions(std::vector> regions); + std::vector> + initialisePointRegions(pcl::PCLPointCloud2 point_cloud); - float calculateStandardDeviation(std::vector values); + void fillPointRegions(pcl::PointCloud::Ptr pcl, + std::vector>& regions); - sb_geom_msgs::Polygon2D getRegionAreaFromIndices(int row, int column); + mapping_msgs_urc::RiskAreaArray + analysePointRegions(std::vector> regions); - int determineRow(float x); + float calculateStandardDeviation(std::vector values); - int determineColumn(float y); -private: + sb_geom_msgs::Polygon2D getRegionAreaFromIndices(int row, int column); + int determineRow(float x); + int determineColumn(float y); + private: float region_width; float region_height; @@ -77,8 +83,6 @@ class RiskAnalysis { float cell_width; float cell_height; int total_cells; - }; - -#endif //SNOWFLAKE_RISKANALYSIS_H +#endif // SNOWFLAKE_RISKANALYSIS_H diff --git a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h index c06395c92..a45ecb409 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h @@ -9,38 +9,37 @@ #include // Utilities -#include #include +#include // Messages -#include -#include #include "mapping_msgs_urc/RiskArea.h" #include "mapping_msgs_urc/RiskAreaArray.h" +#include +#include // Point Cloud #include -#include #include +#include #include // Risk Analysis #include "RiskAnalysis.h" class RiskAnalysisNode { -public: - + public: /** * Constructor */ - RiskAnalysisNode(int argc, char **argv, std::string node_name); - -private: + RiskAnalysisNode(int argc, char** argv, std::string node_name); + private: /* * The callback function is called whenever the node receives a * PointCloud message. It converts sensor_msgs PointCloud2 pointer - * to PCL PointCloud pointer and then extracts risk areas from the PointCloud + * to PCL PointCloud pointer and then extracts risk areas from the + * PointCloud * * @param point_cloud the received PointCloud message */ @@ -71,4 +70,4 @@ class RiskAnalysisNode { ros::Publisher risk_marker_publisher; }; -#endif //RISKANALYSISNODE_H +#endif // RISKANALYSISNODE_H diff --git a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp index c1299d34f..84759f50d 100644 --- a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp +++ b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp @@ -121,8 +121,8 @@ void LineExtractorNode::visualizeClusters() { convertClustersToPointsWithColors(this->clusters, cluster_points, colors); visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createMarkerScale( - this->scale, this->scale, this->scale); + snowbots::RvizUtils::createMarkerScale( + this->scale, this->scale, this->scale); std::string ns = "debug"; @@ -173,8 +173,8 @@ std::vector line_obstacles) { visualization_msgs::Marker::_color_type color = snowbots::RvizUtils::createMarkerColor(0.0, 1.0, 1.0, 1.0); visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createMarkerScale( - this->scale, this->scale, this->scale); + snowbots::RvizUtils::createMarkerScale( + this->scale, this->scale, this->scale); std::string ns = "debug"; diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index 11cadb393..efef860dd 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -1,19 +1,22 @@ /* * Created By: Robyn Castro * Created On: October 14, 2018 - * Description: Processes surrounding environment data to create a regional assessment of risk + * Description: Processes surrounding environment data to create a regional + * assessment of risk */ - #include "RiskAnalysis.h" -RiskAnalysis::RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, int num_horizontal_cell_div, int region_min_points) : - region_width(region_width), +RiskAnalysis::RiskAnalysis(float region_width, + float region_height, + int num_vertical_cell_div, + int num_horizontal_cell_div, + int region_min_points) + : region_width(region_width), region_height(region_height), num_vertical_cell_div(num_vertical_cell_div), num_horizontal_cell_div(num_horizontal_cell_div) { - - cell_width = region_width / num_horizontal_cell_div; + cell_width = region_width / num_horizontal_cell_div; cell_height = region_height / num_vertical_cell_div; total_cells = num_horizontal_cell_div * num_vertical_cell_div; } @@ -22,16 +25,17 @@ RiskAnalysis::RiskAnalysis() { // Empty Constructor } -mapping_msgs_urc::RiskAreaArray RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) -{ +mapping_msgs_urc::RiskAreaArray +RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) { mapping_msgs_urc::RiskAreaArray risk_areas; // Initialise regions with an area and associated points - std::vector> regions = initialisePointRegions(point_cloud); - + std::vector> regions = + initialisePointRegions(point_cloud); // Convert to PCLPointCloud - pcl::PointCloud::Ptr pcl(new pcl::PointCloud()); + pcl::PointCloud::Ptr pcl( + new pcl::PointCloud()); pcl::fromPCLPointCloud2(point_cloud, *pcl); fillPointRegions(pcl, regions); @@ -41,8 +45,8 @@ mapping_msgs_urc::RiskAreaArray RiskAnalysis::assessPointCloudRisk(pcl::PCLPoint return risk_areas; } -std::vector> RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) -{ +std::vector> +RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { std::vector> regions; for (int i = 0; i < num_vertical_cell_div; i++) { @@ -51,7 +55,7 @@ std::vector> RiskAnalysis::initialisePointRegions(pc RegionOfPoints new_region; sb_geom_msgs::Polygon2D region_area; - region_area = getRegionAreaFromIndices(i, j); + region_area = getRegionAreaFromIndices(i, j); new_region.region_area = region_area; new_region_row.push_back(new_region); @@ -62,8 +66,9 @@ std::vector> RiskAnalysis::initialisePointRegions(pc return regions; } -void RiskAnalysis::fillPointRegions(pcl::PointCloud::Ptr pcl, - std::vector> ®ions) { +void RiskAnalysis::fillPointRegions( +pcl::PointCloud::Ptr pcl, +std::vector>& regions) { for (size_t i = 0; i < pcl->size(); i++) { // Convert PCLPointXYZ to geometry_msgs::Point geometry_msgs::Point cur_point; @@ -76,7 +81,7 @@ void RiskAnalysis::fillPointRegions(pcl::PointCloud::Ptr pcl, int row = determineColumn(pcl->points[i].y); bool validColumn = col >= 0 && col < regions[0].size(); - bool validRow = row >= 0 && row < regions.size(); + bool validRow = row >= 0 && row < regions.size(); if (validColumn && validRow) { regions[row][col].points.push_back(cur_point); @@ -84,8 +89,8 @@ void RiskAnalysis::fillPointRegions(pcl::PointCloud::Ptr pcl, } } -mapping_msgs_urc::RiskAreaArray RiskAnalysis::analysePointRegions(std::vector> regions) { - +mapping_msgs_urc::RiskAreaArray RiskAnalysis::analysePointRegions( +std::vector> regions) { mapping_msgs_urc::RiskAreaArray risk_areas; for (int i = 0; i < regions.size(); i++) { @@ -103,7 +108,7 @@ mapping_msgs_urc::RiskAreaArray RiskAnalysis::analysePointRegions(std::vector values) { - float sum = std::accumulate(values.begin(), values.end(), 0.0); + float sum = std::accumulate(values.begin(), values.end(), 0.0); float mean = sum / values.size(); std::vector diff(values.size()); - std::transform(values.begin(), values.end(), diff.begin(), + std::transform(values.begin(), + values.end(), + diff.begin(), std::bind2nd(std::minus(), mean)); - double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + double sq_sum = + std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); double stdev = std::sqrt(sq_sum / values.size()); return stdev; } -sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, int column) { +sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, + int column) { sb_geom_msgs::Point2D top_left_point; - top_left_point.x = region_height - row*cell_height; - top_left_point.y = region_width / 2.0 - column*cell_width; + top_left_point.x = region_height - row * cell_height; + top_left_point.y = region_width / 2.0 - column * cell_width; sb_geom_msgs::Point2D top_right_point; - top_right_point.x = region_height - row*cell_height; - top_right_point.y = region_width / 2.0 - (column + 1)*cell_width; + top_right_point.x = region_height - row * cell_height; + top_right_point.y = region_width / 2.0 - (column + 1) * cell_width; sb_geom_msgs::Point2D bottom_left_point; - bottom_left_point.x = region_width - (row + 1)*cell_height; - bottom_left_point.y = region_width / 2.0 - column*cell_width; + bottom_left_point.x = region_width - (row + 1) * cell_height; + bottom_left_point.y = region_width / 2.0 - column * cell_width; sb_geom_msgs::Point2D bottom_right_point; - bottom_right_point.x = region_width - (row + 1.0)*cell_height; - bottom_right_point.y = region_width / 2.0 - (column + 1.0)*cell_width; + bottom_right_point.x = region_width - (row + 1.0) * cell_height; + bottom_right_point.y = region_width / 2.0 - (column + 1.0) * cell_width; sb_geom_msgs::Polygon2D region_area; region_area.points.push_back(top_left_point); @@ -149,17 +158,14 @@ sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, int colu region_area.points.push_back(bottom_left_point); return region_area; - } -int RiskAnalysis::determineRow(float x) -{ +int RiskAnalysis::determineRow(float x) { int row = x / cell_height; return row; } -int RiskAnalysis::determineColumn(float y) -{ +int RiskAnalysis::determineColumn(float y) { int col = (y + region_width / 2) / cell_width; return col; } \ No newline at end of file diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index c4bec728f..39501eb71 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -7,27 +7,42 @@ #include "RiskAnalysisNode.h" -RiskAnalysisNode::RiskAnalysisNode(int argc, char **argv, std::string node_name) { +RiskAnalysisNode::RiskAnalysisNode(int argc, + char** argv, + std::string node_name) { // Setup NodeHandles ros::init(argc, argv, node_name); ros::NodeHandle nh; ros::NodeHandle private_nh("~"); // Initialise parameters - float default_width = 10; - SB_getParam(private_nh, "area_of_interest_width", area_of_interest_width, default_width); - - float default_height = 5; - SB_getParam(private_nh, "area_of_interest_height", area_of_interest_height, default_height); - - int default_vertical_divisions = 10; - SB_getParam(private_nh, "num_vertical_cell_div", num_vertical_cell_div, default_vertical_divisions); - - int default_horizontal_divisions = 30; - SB_getParam(private_nh, "area_of_interest_width", num_horizontal_cell_div, default_horizontal_divisions); + float default_width = 10; + SB_getParam(private_nh, + "area_of_interest_width", + area_of_interest_width, + default_width); + + float default_height = 5; + SB_getParam(private_nh, + "area_of_interest_height", + area_of_interest_height, + default_height); + + int default_vertical_divisions = 10; + SB_getParam(private_nh, + "num_vertical_cell_div", + num_vertical_cell_div, + default_vertical_divisions); + + int default_horizontal_divisions = 30; + SB_getParam(private_nh, + "area_of_interest_width", + num_horizontal_cell_div, + default_horizontal_divisions); int default_min_points = 100; - SB_getParam(private_nh, "region_min_points", region_min_points, default_min_points); + SB_getParam( + private_nh, "region_min_points", region_min_points, default_min_points); // In the beginning we have not published any markers seq_count = 0; @@ -39,34 +54,39 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, char **argv, std::string node_name) gradient.reserve(gradient_size); for (int i = 0; i < gradient_size; i++) { - gradient[i] = snowbots::RvizUtils::createMarkerColor(i*gradient_step, gradient_step*(gradient_size - i), 0, 1.0f); + gradient[i] = snowbots::RvizUtils::createMarkerColor( + i * gradient_step, gradient_step * (gradient_size - i), 0, 1.0f); } // Initialise Subscribers std::string topic_to_subscribe_to = "input_pointcloud"; // dummy topic name int refresh_rate = 10; pcl_subscriber = nh.subscribe( - topic_to_subscribe_to, refresh_rate, &RiskAnalysisNode::pclCallBack, this); + topic_to_subscribe_to, refresh_rate, &RiskAnalysisNode::pclCallBack, this); // Initialise Publishers - std::string risk_areas_topic = - "output_risk_areas"; // dummy topic name - uint32_t queue_size = 1; - risk_publisher = private_nh.advertise( - risk_areas_topic, queue_size); + std::string risk_areas_topic = "output_risk_areas"; // dummy topic name + uint32_t queue_size = 1; + risk_publisher = private_nh.advertise( + risk_areas_topic, queue_size); std::string risk_area_markers_topic = - "output_risk_area_markers"; // dummy topic name + "output_risk_area_markers"; // dummy topic name - risk_marker_publisher = private_nh.advertise( - risk_area_markers_topic, queue_size); + risk_marker_publisher = + private_nh.advertise( + risk_area_markers_topic, queue_size); // Initialise risk_analysis with parameters - risk_analysis = RiskAnalysis(area_of_interest_width, area_of_interest_height, num_vertical_cell_div, num_horizontal_cell_div, region_min_points); - + risk_analysis = RiskAnalysis(area_of_interest_width, + area_of_interest_height, + num_vertical_cell_div, + num_horizontal_cell_div, + region_min_points); } -void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_cloud) { +void RiskAnalysisNode::pclCallBack( +const sensor_msgs::PointCloud2ConstPtr point_cloud) { pcl::PCLPointCloud2 pcl_pc2; // convert sensor_msgs::PointCloud2 to pcl::PCLPointCloud2 @@ -79,16 +99,16 @@ void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_ visualization_msgs::MarkerArray risk_area_markers; std::string frame_id = "camera_color_optical_frame"; - std::string ns = "debug"; + std::string ns = "debug"; for (int i = 0; i < pcl_risk.areas.size(); i++) { - visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker2D( - pcl_risk.areas[i].area, - convertRiskToColor(pcl_risk.areas[i].score.data), - snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), - frame_id, - ns - ); + visualization_msgs::Marker risk_area_marker = + snowbots::RvizUtils::createPolygonMarker2D( + pcl_risk.areas[i].area, + convertRiskToColor(pcl_risk.areas[i].score.data), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), + frame_id, + ns); risk_area_markers.markers.push_back(risk_area_marker); } @@ -97,7 +117,8 @@ void RiskAnalysisNode::pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_ risk_marker_publisher.publish(risk_area_markers); } -void RiskAnalysisNode::publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas) { +void RiskAnalysisNode::publishMarkers( +mapping_msgs_urc::RiskAreaArray risk_areas) { risk_areas.header.frame_id = "camera_color_optical_frame"; risk_areas.header.seq = seq_count; @@ -108,7 +129,8 @@ void RiskAnalysisNode::publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas risk_publisher.publish(risk_areas); } -visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { +visualization_msgs::Marker::_color_type +RiskAnalysisNode::convertRiskToColor(float risk) { // Round down to the nearest integer to get risk color return gradient[(int) risk]; } \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 89c8ba8f6..330a6338d 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -13,51 +13,62 @@ /* * Function Prototype for Risk Analysis * - * RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, + * RiskAnalysis(float region_width, float region_height, int + * num_vertical_cell_div, * int num_horizontal_cell_div, int region_min_points); * */ pcl::PCLPointCloud2 empty_cloud = pcl::PCLPointCloud2(); -void testRegionDimensions(std::vector> point_region, float region_width, float region_height, - int num_vertical_cell_divs, int num_horizontal_cell_divs); +void testRegionDimensions(std::vector> point_region, + float region_width, + float region_height, + int num_vertical_cell_divs, + int num_horizontal_cell_divs); TEST(RegionCreation, DivisionsEqualToDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 10; + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; - int min_points_in_region = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, - num_vertical_cell_divs, num_horizontal_cell_divs, + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, min_points_in_region); - geometry_msgs::Polygon top_left_region = risk_analysis.getRegionAreaFromIndices(0,0); + geometry_msgs::Polygon top_left_region = + risk_analysis.getRegionAreaFromIndices(0, 0); EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); - EXPECT_FLOAT_EQ(region_width/2, top_left_region.points[0].y); + EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); - geometry_msgs::Polygon top_right_region = risk_analysis.getRegionAreaFromIndices(0,9); + geometry_msgs::Polygon top_right_region = + risk_analysis.getRegionAreaFromIndices(0, 9); EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); - EXPECT_FLOAT_EQ(-region_width/2, top_right_region.points[1].y); + EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); - geometry_msgs::Polygon bottom_right_region = risk_analysis.getRegionAreaFromIndices(9,9); + geometry_msgs::Polygon bottom_right_region = + risk_analysis.getRegionAreaFromIndices(9, 9); EXPECT_FLOAT_EQ(0, bottom_right_region.points[2].x); - EXPECT_FLOAT_EQ(-region_width/2, bottom_right_region.points[2].y); + EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); - geometry_msgs::Polygon bottom_left_region = risk_analysis.getRegionAreaFromIndices(9,0); + geometry_msgs::Polygon bottom_left_region = + risk_analysis.getRegionAreaFromIndices(9, 0); EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); - EXPECT_FLOAT_EQ(region_width/2, bottom_left_region.points[3].y); - + EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); } TEST(RegionInitialisation, DivisionsEqualToDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 10; + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; - int min_points_in_region = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, - num_vertical_cell_divs, num_horizontal_cell_divs, + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, min_points_in_region); std::vector> point_region; @@ -67,68 +78,84 @@ TEST(RegionInitialisation, DivisionsEqualToDimensions) { EXPECT_EQ(num_vertical_cell_divs, point_region.size()); EXPECT_EQ(num_horizontal_cell_divs, point_region[0].size()); - testRegionDimensions(point_region, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); + testRegionDimensions(point_region, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); } TEST(RegionAnalysis, DivisionsEqualToDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 10; + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; - int min_points_in_region = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, - num_vertical_cell_divs, num_horizontal_cell_divs, + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, min_points_in_region); - mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); + mapping_msgs_urc::RiskAreaArray pcl_risk = + risk_analysis.assessPointCloudRisk(empty_cloud); } TEST(RegionCreation, NonIntegerCellDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 100; + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; - int min_points_in_region = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, - num_vertical_cell_divs, num_horizontal_cell_divs, + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, min_points_in_region); - geometry_msgs::Polygon top_left_region = risk_analysis.getRegionAreaFromIndices(0,0); + geometry_msgs::Polygon top_left_region = + risk_analysis.getRegionAreaFromIndices(0, 0); EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); - EXPECT_FLOAT_EQ(region_width/2, top_left_region.points[0].y); + EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); - geometry_msgs::Polygon top_right_region = risk_analysis.getRegionAreaFromIndices(0,99); + geometry_msgs::Polygon top_right_region = + risk_analysis.getRegionAreaFromIndices(0, 99); EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); - EXPECT_FLOAT_EQ(-region_width/2, top_right_region.points[1].y); + EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); - geometry_msgs::Polygon bottom_right_region = risk_analysis.getRegionAreaFromIndices(99,99); + geometry_msgs::Polygon bottom_right_region = + risk_analysis.getRegionAreaFromIndices(99, 99); EXPECT_NEAR(0, bottom_right_region.points[2].x, 0.001); - EXPECT_FLOAT_EQ(-region_width/2, bottom_right_region.points[2].y); + EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); - geometry_msgs::Polygon bottom_left_region = risk_analysis.getRegionAreaFromIndices(99,0); + geometry_msgs::Polygon bottom_left_region = + risk_analysis.getRegionAreaFromIndices(99, 0); EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); - EXPECT_FLOAT_EQ(region_width/2, bottom_left_region.points[3].y); - + EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); } TEST(RegionAnalysis, NonIntegerCellDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 100; + float region_width = 10; + float region_height = 10; + int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; - int min_points_in_region = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, - num_vertical_cell_divs, num_horizontal_cell_divs, + int min_points_in_region = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, min_points_in_region); - mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); + mapping_msgs_urc::RiskAreaArray pcl_risk = + risk_analysis.assessPointCloudRisk(empty_cloud); } - - -void testRegionDimensions(std::vector> point_region, float region_width, float region_height, - int num_vertical_cell_divs, int num_horizontal_cell_divs) { - float cell_width = region_width / num_horizontal_cell_divs; +void testRegionDimensions(std::vector> point_region, + float region_width, + float region_height, + int num_vertical_cell_divs, + int num_horizontal_cell_divs) { + float cell_width = region_width / num_horizontal_cell_divs; float cell_height = region_height / num_vertical_cell_divs; for (int i = 0; i < point_region.size(); i++) { @@ -136,17 +163,20 @@ void testRegionDimensions(std::vector> point_region, geometry_msgs::Polygon cur_cell = point_region[i][j].region_area; // Top Left Point vs Bottom Left Point - EXPECT_FLOAT_EQ(cell_height, cur_cell.points[0].x - cur_cell.points[3].x); + EXPECT_FLOAT_EQ(cell_height, + cur_cell.points[0].x - cur_cell.points[3].x); // Top Right Point vs Bottom Right Point - EXPECT_FLOAT_EQ(cell_height, cur_cell.points[1].x - cur_cell.points[2].x); + EXPECT_FLOAT_EQ(cell_height, + cur_cell.points[1].x - cur_cell.points[2].x); // Top Left Point vs Top Right Point - EXPECT_FLOAT_EQ(cell_width, cur_cell.points[0].y - cur_cell.points[1].y); + EXPECT_FLOAT_EQ(cell_width, + cur_cell.points[0].y - cur_cell.points[1].y); // Bottom Left Point vs Bottom Right Point - EXPECT_FLOAT_EQ(cell_width, cur_cell.points[3].y - cur_cell.points[2].y); - + EXPECT_FLOAT_EQ(cell_width, + cur_cell.points[3].y - cur_cell.points[2].y); } } } @@ -180,7 +210,8 @@ TEST(StandardDeviation, NonIntegerDataPoints) { std::vector data_set = {5.4, 3.4, 8, 100, 20, 5.5}; - EXPECT_NEAR(34.54487018, risk_analysis.calculateStandardDeviation(data_set), 0.01); + EXPECT_NEAR( + 34.54487018, risk_analysis.calculateStandardDeviation(data_set), 0.01); } TEST(StandardDeviation, NegativeNonIntegerDataPoints) { @@ -188,7 +219,8 @@ TEST(StandardDeviation, NegativeNonIntegerDataPoints) { std::vector data_set = {-5.4, 3.4, -8, 100, -20, 5.5}; - EXPECT_NEAR(39.96858836, risk_analysis.calculateStandardDeviation(data_set), 0.01); + EXPECT_NEAR( + 39.96858836, risk_analysis.calculateStandardDeviation(data_set), 0.01); } TEST(StandardDeviation, RandomDataPoints) { @@ -196,15 +228,18 @@ TEST(StandardDeviation, RandomDataPoints) { std::vector data_set = {1, 2, -7, 5.4, 3.4, 8, 100, 20, -5}; - EXPECT_NEAR(31.21424176, risk_analysis.calculateStandardDeviation(data_set), 0.01); + EXPECT_NEAR( + 31.21424176, risk_analysis.calculateStandardDeviation(data_set), 0.01); } TEST(StandardDeviation, RandomDataPointsLargeNumbers) { RiskAnalysis risk_analysis = RiskAnalysis(); - std::vector data_set = {1, 2, -7, 5.4, 3.4, 8, 100, 20, -5000, 254000, 5, -200080}; + std::vector data_set = { + 1, 2, -7, 5.4, 3.4, 8, 100, 20, -5000, 254000, 5, -200080}; - EXPECT_NEAR(93261.48782, risk_analysis.calculateStandardDeviation(data_set), 0.01); + EXPECT_NEAR( + 93261.48782, risk_analysis.calculateStandardDeviation(data_set), 0.01); } int main(int argc, char** argv) { diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index bfd72b2ea..d950cf44c 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -106,16 +106,43 @@ namespace RvizUtils { * * @return an rviz marker */ - template - visualization_msgs::Marker + template + visualization_msgs::Marker createPolygonMarker2D(T polygon, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1); + int id = 1) { + visualization_msgs::Marker marker; + + marker.header.stamp = ros::Time::now(); + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + + marker.type = type; + marker.id = id; + + marker.header.frame_id = frame_id; + marker.ns = ns; + + marker.scale = scale; + + // Set the color + marker.color = color; + + // Setup the line strip + for (int i = 0; i < polygon.points.size(); i++) { + geometry_msgs::Point point; + point.x = polygon.points[i].x; + point.y = polygon.points[i].y; + point.z = 0; + marker.points.push_back(point); + } + return marker; + } /** * Turn a polygon into a marker for rviz * @@ -128,12 +155,12 @@ namespace RvizUtils { */ visualization_msgs::Marker createPolygonMarker3D(geometry_msgs::Polygon polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1); + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1); /** * Creates a Marker Array (array of Markers) @@ -179,8 +206,6 @@ namespace RvizUtils { */ visualization_msgs::Marker::_scale_type createMarkerScale(float x, float y, float z); - - }; }; #endif // HOLE_TRACKER_RVIZUTILS_H diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 4c9550c10..223ddb60b 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -7,36 +7,36 @@ #include using namespace visualization_msgs; -//using namespace snowbots; - -namespace snowbots{ - namespace RvizUtils { - /** - * Helper function that sets up common marker parameters - * - * @param scale the scale - * @param frame_id the frame id - * @param ns the namespace - * @param type the type of marker - * @param id the id of marker - */ - void setupMarker(visualization_msgs::Marker& marker, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); - } +// using namespace snowbots; + +namespace snowbots { +namespace RvizUtils { + /** + * Helper function that sets up common marker parameters + * + * @param scale the scale + * @param frame_id the frame id + * @param ns the namespace + * @param type the type of marker + * @param id the id of marker + */ + void setupMarker(visualization_msgs::Marker& marker, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::POINTS, + int id = 0); +} } - -Marker snowbots::RvizUtils::createMarker(std::vector points, - Marker::_color_type color, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker +snowbots::RvizUtils::createMarker(std::vector points, + Marker::_color_type color, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -50,13 +50,14 @@ Marker snowbots::RvizUtils::createMarker(std::vector point return marker; } -Marker snowbots::RvizUtils::createMarker(std::vector points, - std::vector colors, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker +snowbots::RvizUtils::createMarker(std::vector points, + std::vector colors, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -70,13 +71,14 @@ Marker snowbots::RvizUtils::createMarker(std::vector point return marker; } -Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, - std::vector colors, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker +snowbots::RvizUtils::createMarker(geometry_msgs::Point point, + std::vector colors, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -91,12 +93,12 @@ Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, } Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, - Marker::_color_type color, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { + Marker::_color_type color, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -110,42 +112,14 @@ Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, return marker; } -template -Marker -snowbots::RvizUtils::createPolygonMarker2D(T polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { - Marker marker; - - setupMarker(marker, scale, frame_id, ns, type, id); - - // Set the color - marker.color = color; - - // Setup the line strip - for (int i = 0; i < polygon.points.size(); i++) { - geometry_msgs::Point point; - point.x = polygon.points[i].x; - point.y = polygon.points[i].y; - point.z = 0; - marker.points.push_back(point); - } - - return marker; -} - -Marker -snowbots::RvizUtils::createPolygonMarker3D(geometry_msgs::Polygon polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker snowbots::RvizUtils::createPolygonMarker3D( +geometry_msgs::Polygon polygon, +visualization_msgs::Marker::_color_type color, +visualization_msgs::Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int type, +int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -193,10 +167,11 @@ snowbots::RvizUtils::createMarkerColor(float r, float g, float b, float a) { return color; } -//visualization_msgs::Marker::_scale_type -//createMarkerScale(float x, float y, float z); +// visualization_msgs::Marker::_scale_type +// createMarkerScale(float x, float y, float z); -Marker::_scale_type snowbots::RvizUtils::createMarkerScale(float x, float y, float z) { +Marker::_scale_type +snowbots::RvizUtils::createMarkerScale(float x, float y, float z) { Marker::_scale_type scale; scale.x = x; scale.y = y; @@ -205,12 +180,13 @@ Marker::_scale_type snowbots::RvizUtils::createMarkerScale(float x, float y, flo return scale; } -void snowbots::RvizUtils::setupMarker(Marker& marker, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +void snowbots::RvizUtils::setupMarker( +Marker& marker, +visualization_msgs::Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int type, +int id) { marker.header.stamp = ros::Time::now(); marker.action = Marker::ADD; marker.pose.orientation.w = 1.0; diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 644aecabd..397172ffc 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -54,12 +54,12 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { polygon.points.push_back(initialisePoint32(1, 1, 0)); visualization_msgs::Marker risk_area_marker = - snowbots::RvizUtils::createPolygonMarker3D( - polygon, - snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), - frame_id, - ns); + snowbots::RvizUtils::createPolygonMarker3D( + polygon, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), + frame_id, + ns); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -78,12 +78,12 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { polygon.points.push_back(initialisePoint32(0, 1, 0)); visualization_msgs::Marker risk_area_marker = - snowbots::RvizUtils::createPolygonMarker3D( - polygon, - snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), - frame_id, - ns); + snowbots::RvizUtils::createPolygonMarker3D( + polygon, + snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), + frame_id, + ns); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); From 800e0cdeb148980f2d45036b24925b40e9a17758 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 19 Jan 2019 10:32:04 -0800 Subject: [PATCH 14/39] added comment to detail why this is a rostest --- src/sb_utils/test/rviz_utils_rostest.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 397172ffc..15fd635c7 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -1,3 +1,8 @@ +/** + * This is a rostest instead of a normal test, because RvizUtils uses + * ros node functionalities. (eg. ros::Time::now()) + */ + #include "RvizUtils.h" #include @@ -208,4 +213,4 @@ int main(int argc, char** argv) { ros::init(argc, argv, "rviz_utils_rostest"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From b60684b3128e6a2ad97232dece05989a8363c520 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 19 Jan 2019 10:34:48 -0800 Subject: [PATCH 15/39] Added reasoning why rviz_utils uses a rostest --- src/sb_utils/test/rviz_utils_rostest.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 13dfb59fe..b5a45d749 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -1,3 +1,8 @@ +/** + * This is a rostest, because RvizUtils uses ros node functionalities. + * (eg. ros::time::now()) + */ + #include "RvizUtils.h" #include @@ -208,4 +213,4 @@ int main(int argc, char** argv) { ros::init(argc, argv, "rviz_utils_rostest"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 50ec1f804f4fa98113bfcc2b640a32047b1c6ee7 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Wed, 13 Feb 2019 18:54:17 -0800 Subject: [PATCH 16/39] Templating the polygon risk function --- .gitignore | 3 + src/sb_utils/include/RvizUtils.h | 114 +++++++++++++++++---------- src/sb_utils/src/RvizUtils.cpp | 127 +++++++++++++++++++------------ 3 files changed, 155 insertions(+), 89 deletions(-) diff --git a/.gitignore b/.gitignore index 70bd23349..e19dc898a 100644 --- a/.gitignore +++ b/.gitignore @@ -22,6 +22,9 @@ cmake-build-release .rosinstall.bak +# 3rd Party Libraries +external_libs + # 3rd Party Packages src/external_pkgs/ src/mapping_urc/multi_resolution_graph diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index ecab343f4..f0ff17aad 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -18,8 +18,7 @@ #include namespace snowbots { -class RvizUtils { - public: +namespace RvizUtils { /** * Turn points into a marker for rviz * @@ -30,7 +29,7 @@ class RvizUtils { * * @return an rviz marker */ - static visualization_msgs::Marker + visualization_msgs::Marker createMarker(std::vector points, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, @@ -50,7 +49,7 @@ class RvizUtils { * * @return an rviz marker */ - static visualization_msgs::Marker + visualization_msgs::Marker createMarker(std::vector points, std::vector colors, visualization_msgs::Marker::_scale_type scale, @@ -69,7 +68,7 @@ class RvizUtils { * * @return an rviz marker */ - static visualization_msgs::Marker + visualization_msgs::Marker createMarker(geometry_msgs::Point point, std::vector colors, visualization_msgs::Marker::_scale_type scale, @@ -88,7 +87,7 @@ class RvizUtils { * * @return an rviz marker */ - static visualization_msgs::Marker + visualization_msgs::Marker createMarker(geometry_msgs::Point point, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, @@ -107,14 +106,68 @@ class RvizUtils { * * @return an rviz marker */ - static visualization_msgs::Marker - createPolygonMarker(geometry_msgs::Polygon polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1); + template + visualization_msgs::Marker + createPolygonMarker2D(T polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1) { + visualization_msgs::Marker marker; + + marker.header.stamp = ros::Time::now(); + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + + marker.type = type; + marker.id = id; + + marker.header.frame_id = frame_id; + marker.ns = ns; + + marker.scale = scale; + + // Set the color + marker.color = color; + + // Setup the line strip + for (int i = 0; i < polygon.points.size(); i++) { + geometry_msgs::Point point; + point.x = polygon.points[i].x; + point.y = polygon.points[i].y; + point.z = 0; + marker.points.push_back(point); + } + + geometry_msgs::Point point; + point.x = polygon.points[0].x; + point.y = polygon.points[0].y; + point.z = 0; + marker.points.push_back(point); + + return marker; + } + /** + * Turn a polygon into a marker for rviz + * + * @param polygon the polygon to be converted + * @param color the color of the polygon + * @param frame_id the frame id + * @param ns the namespace + * + * @return an rviz marker + */ + visualization_msgs::Marker + createPolygonMarker3D(geometry_msgs::Polygon polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::LINE_STRIP, + int id = 1); + /** * Creates a Marker Array (array of Markers) * @@ -126,8 +179,8 @@ class RvizUtils { * * @return an rviz marker array */ - static visualization_msgs::MarkerArray createMarkerArray( - std::vector> points_arary, + visualization_msgs::MarkerArray createMarkerArray( + std::vector> points_array, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, std::string frame_id, @@ -145,7 +198,7 @@ class RvizUtils { * * @return a marker color type */ - static visualization_msgs::Marker::_color_type + visualization_msgs::Marker::_color_type createMarkerColor(float r, float g, float b, float a); /** @@ -157,31 +210,8 @@ class RvizUtils { * * @return a marker scale type */ - static visualization_msgs::Marker::_scale_type - createrMarkerScale(float x, float y, float z); - - private: - /** - * Private constructor (No reason for someone to make an instance of this - * class). - */ - RvizUtils(); - - /** - * Helper function that sets up common marker parameters - * - * @param scale the scale - * @param frame_id the frame id - * @param ns the namespace - * @param type the type of marker - * @param id the id of marker - */ - static void setupMarker(visualization_msgs::Marker& marker, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); + visualization_msgs::Marker::_scale_type + createMarkerScale(float x, float y, float z); }; }; #endif // HOLE_TRACKER_RVIZUTILS_H diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 6d2afe514..9ab55511b 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -7,15 +7,36 @@ #include using namespace visualization_msgs; -using namespace snowbots; - -Marker RvizUtils::createMarker(std::vector points, - Marker::_color_type color, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +// using namespace snowbots; + +namespace snowbots { +namespace RvizUtils { + /** + * Helper function that sets up common marker parameters + * + * @param scale the scale + * @param frame_id the frame id + * @param ns the namespace + * @param type the type of marker + * @param id the id of marker + */ + void setupMarker(visualization_msgs::Marker& marker, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type = visualization_msgs::Marker::POINTS, + int id = 0); +} +} + +Marker +snowbots::RvizUtils::createMarker(std::vector points, + Marker::_color_type color, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -29,13 +50,14 @@ Marker RvizUtils::createMarker(std::vector points, return marker; } -Marker RvizUtils::createMarker(std::vector points, - std::vector colors, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker +snowbots::RvizUtils::createMarker(std::vector points, + std::vector colors, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -49,13 +71,14 @@ Marker RvizUtils::createMarker(std::vector points, return marker; } -Marker RvizUtils::createMarker(geometry_msgs::Point point, - std::vector colors, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker +snowbots::RvizUtils::createMarker(geometry_msgs::Point point, + std::vector colors, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -69,13 +92,13 @@ Marker RvizUtils::createMarker(geometry_msgs::Point point, return marker; } -Marker RvizUtils::createMarker(geometry_msgs::Point point, - Marker::_color_type color, - Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, + Marker::_color_type color, + Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int type, + int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -89,14 +112,14 @@ Marker RvizUtils::createMarker(geometry_msgs::Point point, return marker; } -Marker -RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +Marker snowbots::RvizUtils::createPolygonMarker3D( +geometry_msgs::Polygon polygon, +visualization_msgs::Marker::_color_type color, +visualization_msgs::Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int type, +int id) { Marker marker; setupMarker(marker, scale, frame_id, ns, type, id); @@ -112,11 +135,16 @@ RvizUtils::createPolygonMarker(geometry_msgs::Polygon polygon, point.z = polygon.points[i].z; marker.points.push_back(point); } + geometry_msgs::Point point; + point.x = polygon.points[0].x; + point.y = polygon.points[0].y; + point.z = polygon.points[0].z; + marker.points.push_back(point); return marker; } -MarkerArray RvizUtils::createMarkerArray( +MarkerArray snowbots::RvizUtils::createMarkerArray( std::vector> points_array, Marker::_color_type color, Marker::_scale_type scale, @@ -134,7 +162,7 @@ int type) { } Marker::_color_type -RvizUtils::createMarkerColor(float r, float g, float b, float a) { +snowbots::RvizUtils::createMarkerColor(float r, float g, float b, float a) { Marker::_color_type color; color.r = r; color.g = g; @@ -144,7 +172,11 @@ RvizUtils::createMarkerColor(float r, float g, float b, float a) { return color; } -Marker::_scale_type RvizUtils::createrMarkerScale(float x, float y, float z) { +// visualization_msgs::Marker::_scale_type +// createMarkerScale(float x, float y, float z); + +Marker::_scale_type +snowbots::RvizUtils::createMarkerScale(float x, float y, float z) { Marker::_scale_type scale; scale.x = x; scale.y = y; @@ -153,12 +185,13 @@ Marker::_scale_type RvizUtils::createrMarkerScale(float x, float y, float z) { return scale; } -void RvizUtils::setupMarker(Marker& marker, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int type, - int id) { +void snowbots::RvizUtils::setupMarker( +Marker& marker, +visualization_msgs::Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int type, +int id) { marker.header.stamp = ros::Time::now(); marker.action = Marker::ADD; marker.pose.orientation.w = 1.0; From 33e3977cd019abc8ea47dc5c44cd7b076757e95b Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Thu, 14 Feb 2019 21:11:06 -0800 Subject: [PATCH 17/39] Fixed type for createMarkerScale --- src/decision_igvc/src/GpsManager.cpp | 2 +- src/sb_pointcloud_processing/src/LineExtractorNode.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/decision_igvc/src/GpsManager.cpp b/src/decision_igvc/src/GpsManager.cpp index fa909a8dd..ad838420a 100644 --- a/src/decision_igvc/src/GpsManager.cpp +++ b/src/decision_igvc/src/GpsManager.cpp @@ -113,7 +113,7 @@ geometry_msgs::TransformStamped global_to_local_transform) { rviz_marker_publisher.publish(snowbots::RvizUtils::createMarker( output.point, colors, - snowbots::RvizUtils::createrMarkerScale(0.5, 0.5, 0.5), + snowbots::RvizUtils::createMarkerScale(0.5, 0.5, 0.5), base_frame, "debug", visualization_msgs::Marker::POINTS)); diff --git a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp index 554d1d215..84759f50d 100644 --- a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp +++ b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp @@ -121,7 +121,7 @@ void LineExtractorNode::visualizeClusters() { convertClustersToPointsWithColors(this->clusters, cluster_points, colors); visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createrMarkerScale( + snowbots::RvizUtils::createMarkerScale( this->scale, this->scale, this->scale); std::string ns = "debug"; @@ -173,7 +173,7 @@ std::vector line_obstacles) { visualization_msgs::Marker::_color_type color = snowbots::RvizUtils::createMarkerColor(0.0, 1.0, 1.0, 1.0); visualization_msgs::Marker::_scale_type scale = - snowbots::RvizUtils::createrMarkerScale( + snowbots::RvizUtils::createMarkerScale( this->scale, this->scale, this->scale); std::string ns = "debug"; From 5dea15fd8cee299c3695bb5f34fbd72ebfb9c6d6 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Fri, 15 Feb 2019 08:29:07 -0800 Subject: [PATCH 18/39] Fixed typo in rostest --- src/sb_utils/test/rviz_utils_rostest.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index b5a45d749..121b20f02 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -62,7 +62,7 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { snowbots::RvizUtils::createPolygonMarker( polygon, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, ns); @@ -86,7 +86,7 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { snowbots::RvizUtils::createPolygonMarker( polygon, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, ns); @@ -107,7 +107,7 @@ TEST_F(RvizUtilsRosTest, singlePointMarker) { snowbots::RvizUtils::createMarker( initialisePoint(0, 0, 1), snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, ns); @@ -131,7 +131,7 @@ TEST_F(RvizUtilsRosTest, singlePointMarkerInArray) { snowbots::RvizUtils::createMarker( points, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, ns); @@ -156,7 +156,7 @@ TEST_F(RvizUtilsRosTest, multiplePointMarkersInArray) { snowbots::RvizUtils::createMarker( points, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), - snowbots::RvizUtils::createrMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, ns); @@ -173,7 +173,7 @@ TEST_F(RvizUtilsRosTest, createMarkerScale) { float z = 1; visualization_msgs::Marker::_scale_type scale; - scale = snowbots::RvizUtils::createrMarkerScale(x, y, z); + scale = snowbots::RvizUtils::createMarkerScale(x, y, z); EXPECT_FLOAT_EQ(x, scale.x); EXPECT_FLOAT_EQ(y, scale.y); From 2062d320e5137df1162613ada422503d0c479ac4 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Fri, 15 Feb 2019 08:33:10 -0800 Subject: [PATCH 19/39] Fixed determineColumn, determineRow, and added tests, and risk multiplier --- src/sb_pointcloud_processing/CMakeLists.txt | 3 +- .../include/RiskAnalysis.h | 5 +- .../include/RiskAnalysisNode.h | 2 + .../launch/realsense_transform.launch | 11 ++++ .../launch/risk_analysis.launch | 7 +- .../src/RiskAnalysis.cpp | 64 ++++++++++++------- .../src/RiskAnalysisNode.cpp | 18 ++++-- .../test/risk-analysis-test.cpp | 43 ++++++++++--- 8 files changed, 112 insertions(+), 41 deletions(-) create mode 100644 src/sb_pointcloud_processing/launch/realsense_transform.launch diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt index ed47142db..e9eded3d7 100644 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ b/src/sb_pointcloud_processing/CMakeLists.txt @@ -149,8 +149,9 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(colourspace-converter-test test/colourspace-converter-test.cpp include/ColourspaceConverter.h src/ColourspaceConverter.cpp) target_link_libraries(colourspace-converter-test ${PCL_LIBRARIES}) + catkin_add_gtest(risk-analysis-test test/risk-analysis-test.cpp src/RiskAnalysis.cpp include/RiskAnalysis.h) - target_link_libraries(risk-analysis-test ${PCL_LIBRARIES}) + target_link_libraries(risk-analysis-test ${catkin_LIBRARIES} ${PCL_LIBRARIES}) # Adding rostest to the package find_package(rostest REQUIRED) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index bdac67c1e..3bb085e23 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -24,6 +24,7 @@ #include #include + // Standard Libraries #include @@ -41,7 +42,8 @@ class RiskAnalysis { float region_height, int num_vertical_cell_div, int num_horizontal_cell_div, - int region_min_points); + int region_min_points, + float risk_multiplier); /** * Required empty constructor @@ -74,6 +76,7 @@ class RiskAnalysis { private: float region_width; float region_height; + float risk_multiplier; int num_vertical_cell_div; int num_horizontal_cell_div; diff --git a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h index a45ecb409..70830c2d7 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h @@ -21,6 +21,7 @@ // Point Cloud #include #include +#include #include #include @@ -53,6 +54,7 @@ class RiskAnalysisNode { float area_of_interest_width; float area_of_interest_height; + float risk_multiplier; int num_vertical_cell_div; int num_horizontal_cell_div; diff --git a/src/sb_pointcloud_processing/launch/realsense_transform.launch b/src/sb_pointcloud_processing/launch/realsense_transform.launch new file mode 100644 index 000000000..a75ae52c4 --- /dev/null +++ b/src/sb_pointcloud_processing/launch/realsense_transform.launch @@ -0,0 +1,11 @@ + + + + "base_link" + + + + + + + diff --git a/src/sb_pointcloud_processing/launch/risk_analysis.launch b/src/sb_pointcloud_processing/launch/risk_analysis.launch index 8f4dbe7ed..02c90237b 100644 --- a/src/sb_pointcloud_processing/launch/risk_analysis.launch +++ b/src/sb_pointcloud_processing/launch/risk_analysis.launch @@ -4,13 +4,14 @@ type="risk_analysis" output="screen"> - + - area_of_interest_width: 5 - area_of_interest_height: 5 + area_of_interest_width: 8 + area_of_interest_height: 8 num_vertical_cell_div: 50 num_horizontal_cell_div: 50 region_min_points: 10 + risk_multiplier: 40 diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index efef860dd..4eb4a14ed 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -11,13 +11,16 @@ RiskAnalysis::RiskAnalysis(float region_width, float region_height, int num_vertical_cell_div, int num_horizontal_cell_div, - int region_min_points) + int region_min_points, + float risk_multiplier) : region_width(region_width), region_height(region_height), + region_min_points(region_min_points), num_vertical_cell_div(num_vertical_cell_div), - num_horizontal_cell_div(num_horizontal_cell_div) { - cell_width = region_width / num_horizontal_cell_div; - cell_height = region_height / num_vertical_cell_div; + num_horizontal_cell_div(num_horizontal_cell_div), + risk_multiplier(risk_multiplier){ + cell_width = region_width / (double) num_horizontal_cell_div; + cell_height = region_height / (double) num_vertical_cell_div; total_cells = num_horizontal_cell_div * num_vertical_cell_div; } @@ -53,6 +56,8 @@ RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { std::vector new_region_row; for (int j = 0; j < num_horizontal_cell_div; j++) { RegionOfPoints new_region; + std::vector points; + new_region.points = points; sb_geom_msgs::Polygon2D region_area; region_area = getRegionAreaFromIndices(i, j); @@ -69,22 +74,34 @@ RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { void RiskAnalysis::fillPointRegions( pcl::PointCloud::Ptr pcl, std::vector>& regions) { - for (size_t i = 0; i < pcl->size(); i++) { - // Convert PCLPointXYZ to geometry_msgs::Point - geometry_msgs::Point cur_point; - cur_point.x = pcl->points[i].x; - cur_point.y = pcl->points[i].y; - cur_point.z = pcl->points[i].z; - - // Determine which cell the point belongs to - int col = determineRow(pcl->points[i].x); - int row = determineColumn(pcl->points[i].y); + pcl::PointXYZ origin_point; + origin_point.x = 0; + origin_point.y = 0; - bool validColumn = col >= 0 && col < regions[0].size(); - bool validRow = row >= 0 && row < regions.size(); - - if (validColumn && validRow) { - regions[row][col].points.push_back(cur_point); + for (size_t i = 0; i < pcl->size(); i++) { + // Has to be a valid point + if (!isnan(pcl->points[i].x) && pcl->points[i].x > 0 && pcl->points[i].x < region_height) { + // Convert PCLPointXYZ to geometry_msgs::Point + geometry_msgs::Point cur_point; + cur_point.x = pcl->points[i].x; + cur_point.y = pcl->points[i].y; + cur_point.z = pcl->points[i].z; + + // Determine which cell the point belongs to + int row = determineRow(pcl->points[i].x); + int col = determineColumn(pcl->points[i].y); + + bool validColumn = col >= 0 && col < regions[0].size(); + bool validRow = row >= 0 && row < regions.size(); + + if (validColumn && validRow) { +// std::cout << cur_point.x << ", "; +// std::cout << cur_point.y << "= "; +// +// std::cout << row << ","; +// std::cout << col << std::endl; + regions[row][col].points.push_back(cur_point); + } } } } @@ -103,7 +120,7 @@ std::vector> regions) { // Determine risk of the area std_msgs::Float64 risk; - risk.data = calculateStandardDeviation(z_values); + risk.data = risk_multiplier*calculateStandardDeviation(z_values); // Add risk area onto the array mapping_msgs_urc::RiskArea risk_area; @@ -161,11 +178,12 @@ sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, } int RiskAnalysis::determineRow(float x) { - int row = x / cell_height; + int row = floor((region_height - x) / cell_height); return row; } int RiskAnalysis::determineColumn(float y) { - int col = (y + region_width / 2) / cell_width; + int col = (region_width / 2.0 - y) / cell_width; + return col; -} \ No newline at end of file +} diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index 39501eb71..0cc39abb1 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -36,7 +36,7 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, int default_horizontal_divisions = 30; SB_getParam(private_nh, - "area_of_interest_width", + "num_horizontal_cell_div", num_horizontal_cell_div, default_horizontal_divisions); @@ -44,6 +44,9 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, SB_getParam( private_nh, "region_min_points", region_min_points, default_min_points); + float default_risk_multiplier = 1; + SB_getParam(private_nh, "risk_multiplier", risk_multiplier, default_risk_multiplier); + // In the beginning we have not published any markers seq_count = 0; @@ -82,7 +85,9 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, area_of_interest_height, num_vertical_cell_div, num_horizontal_cell_div, - region_min_points); + region_min_points, + risk_multiplier + ); } void RiskAnalysisNode::pclCallBack( @@ -106,9 +111,12 @@ const sensor_msgs::PointCloud2ConstPtr point_cloud) { snowbots::RvizUtils::createPolygonMarker2D( pcl_risk.areas[i].area, convertRiskToColor(pcl_risk.areas[i].score.data), - snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), + snowbots::RvizUtils::createMarkerScale(0.01, 0, 0), frame_id, - ns); + ns, + visualization_msgs::Marker::LINE_STRIP, + i + ); risk_area_markers.markers.push_back(risk_area_marker); } @@ -131,6 +139,8 @@ mapping_msgs_urc::RiskAreaArray risk_areas) { visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { + if (risk > 10) + risk = 10; // Round down to the nearest integer to get risk color return gradient[(int) risk]; } \ No newline at end of file diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 330a6338d..8035b93a7 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -38,25 +38,30 @@ TEST(RegionCreation, DivisionsEqualToDimensions) { num_horizontal_cell_divs, min_points_in_region); - geometry_msgs::Polygon top_left_region = + sb_geom_msgs::Polygon2D top_left_region = risk_analysis.getRegionAreaFromIndices(0, 0); EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); - geometry_msgs::Polygon top_right_region = + sb_geom_msgs::Polygon2D top_right_region = risk_analysis.getRegionAreaFromIndices(0, 9); EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); - geometry_msgs::Polygon bottom_right_region = + sb_geom_msgs::Polygon2D bottom_right_region = risk_analysis.getRegionAreaFromIndices(9, 9); EXPECT_FLOAT_EQ(0, bottom_right_region.points[2].x); EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); - geometry_msgs::Polygon bottom_left_region = + sb_geom_msgs::Polygon2D bottom_left_region = risk_analysis.getRegionAreaFromIndices(9, 0); EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); + + sb_geom_msgs::Polygon2D test_region = + risk_analysis.getRegionAreaFromIndices(2, 9); + + std::cout << risk_analysis.determineColumn(-4.5) << std::endl; } TEST(RegionInitialisation, DivisionsEqualToDimensions) { @@ -113,25 +118,29 @@ TEST(RegionCreation, NonIntegerCellDimensions) { num_horizontal_cell_divs, min_points_in_region); - geometry_msgs::Polygon top_left_region = + sb_geom_msgs::Polygon2D top_left_region = risk_analysis.getRegionAreaFromIndices(0, 0); EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); - geometry_msgs::Polygon top_right_region = + sb_geom_msgs::Polygon2D top_right_region = risk_analysis.getRegionAreaFromIndices(0, 99); EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); - geometry_msgs::Polygon bottom_right_region = + sb_geom_msgs::Polygon2D bottom_right_region = risk_analysis.getRegionAreaFromIndices(99, 99); EXPECT_NEAR(0, bottom_right_region.points[2].x, 0.001); EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); - geometry_msgs::Polygon bottom_left_region = + sb_geom_msgs::Polygon2D bottom_left_region = risk_analysis.getRegionAreaFromIndices(99, 0); EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); + + int row = risk_analysis.determineRow(2.8); + int col = risk_analysis.determineColumn(0); + } TEST(RegionAnalysis, NonIntegerCellDimensions) { @@ -150,6 +159,22 @@ TEST(RegionAnalysis, NonIntegerCellDimensions) { risk_analysis.assessPointCloudRisk(empty_cloud); } +TEST(RegionAnalysis, OneDiv) { + float region_width = 2; + float region_height = 2; + int num_vertical_cell_divs = 2; + int num_horizontal_cell_divs = 1; + int min_points_in_region = -1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, + min_points_in_region); + + mapping_msgs_urc::RiskAreaArray pcl_risk = + risk_analysis.assessPointCloudRisk(empty_cloud); +} + void testRegionDimensions(std::vector> point_region, float region_width, float region_height, @@ -160,7 +185,7 @@ void testRegionDimensions(std::vector> point_region, for (int i = 0; i < point_region.size(); i++) { for (int j = 0; j < point_region[0].size(); j++) { - geometry_msgs::Polygon cur_cell = point_region[i][j].region_area; + sb_geom_msgs::Polygon2D cur_cell = point_region[i][j].region_area; // Top Left Point vs Bottom Left Point EXPECT_FLOAT_EQ(cell_height, From 322334e20636473dba04376636a75c54472ba2e6 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Fri, 15 Feb 2019 08:59:26 -0800 Subject: [PATCH 20/39] Refactored zed_transform to pcl_transform and added a new param --- src/sb_gazebo/CMakeLists.txt | 3 +- src/sb_gazebo/launch/elsax_control.launch | 2 +- src/sb_gazebo/launch/jfrost_control.launch | 2 +- src/sb_pointcloud_processing/CMakeLists.txt | 55 ++++++++++++------- .../src/pcl_transform.cpp} | 36 ++++++++---- 5 files changed, 63 insertions(+), 35 deletions(-) rename src/{sb_gazebo/src/zed_transform.cpp => sb_pointcloud_processing/src/pcl_transform.cpp} (63%) diff --git a/src/sb_gazebo/CMakeLists.txt b/src/sb_gazebo/CMakeLists.txt index 70e3c0f6a..212231407 100755 --- a/src/sb_gazebo/CMakeLists.txt +++ b/src/sb_gazebo/CMakeLists.txt @@ -41,13 +41,12 @@ include_directories( ## Declare a C++ executable # add_executable(gazebo_node src/gazebo_node.cpp) -add_executable(zed_transform src/zed_transform.cpp) + ## Specify libraries to link a library or executable target against # target_link_libraries(gazebo_node # ${catkin_LIBRARIES} # ) -target_link_libraries(zed_transform ${catkin_LIBRARIES} ${sb_utils_LIBRARIES}) ############# diff --git a/src/sb_gazebo/launch/elsax_control.launch b/src/sb_gazebo/launch/elsax_control.launch index 5c7fa044a..4ee8cb3e8 100755 --- a/src/sb_gazebo/launch/elsax_control.launch +++ b/src/sb_gazebo/launch/elsax_control.launch @@ -19,7 +19,7 @@ - + "zed_pointcloud" diff --git a/src/sb_gazebo/launch/jfrost_control.launch b/src/sb_gazebo/launch/jfrost_control.launch index bc44d2ed6..226990e44 100755 --- a/src/sb_gazebo/launch/jfrost_control.launch +++ b/src/sb_gazebo/launch/jfrost_control.launch @@ -19,7 +19,7 @@ - + "zed_pointcloud" diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt index c8faa74d1..ae71a3539 100644 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ b/src/sb_pointcloud_processing/CMakeLists.txt @@ -29,6 +29,11 @@ find_package(catkin REQUIRED COMPONENTS pcl_msgs sensor_msgs mapping_igvc + geometry_msgs + tf2 + tf2_ros + tf2_msgs + tf2_sensor_msgs ) find_package(PCL 1.3 REQUIRED COMPONENTS common @@ -69,47 +74,55 @@ add_executable(line_extractor_node ) add_library(sb_pointcloud_processing - include/rgb_to_hsv.h - src/rgb_to_hsv.cpp - include/ColourspaceConverter.h - src/ColourspaceConverter.cpp -) - -target_link_libraries(sb_pointcloud_processing - ${catkin_LIBRARIES} - ${sb_utils_LIBRARIES} - ${PCL_LIBRARIES} + include/rgb_to_hsv.h + src/rgb_to_hsv.cpp + include/ColourspaceConverter.h + src/ColourspaceConverter.cpp ) add_executable(test_pcl_generator_node src/test_pcl_generator_node.cpp test/TestUtils.h - ) +) + +add_executable(pcl_transform src/pcl_transform.cpp) ## Specify libraries to link a library or executable target against target_link_libraries(line_extractor_node - ${catkin_LIBRARIES} - ${PCL_COMMON_LIBRARIES} - ${PCL_IO_LIBRARIES} + ${catkin_LIBRARIES} + ${PCL_COMMON_LIBRARIES} + ${PCL_IO_LIBRARIES} ) add_dependencies(line_extractor_node - ${mapping_igvc_EXPORTED_TARGETS} + ${mapping_igvc_EXPORTED_TARGETS} ) +target_link_libraries(sb_pointcloud_processing + ${catkin_LIBRARIES} + ${sb_utils_LIBRARIES} + ${PCL_LIBRARIES} +) + + install( - TARGETS - sb_pointcloud_processing - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS + sb_pointcloud_processing + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) target_link_libraries(test_pcl_generator_node ${catkin_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} - ) +) + +target_link_libraries(pcl_transform + ${catkin_LIBRARIES} + ${sb_utils_LIBRARIES} +) ############# ## Testing ## diff --git a/src/sb_gazebo/src/zed_transform.cpp b/src/sb_pointcloud_processing/src/pcl_transform.cpp similarity index 63% rename from src/sb_gazebo/src/zed_transform.cpp rename to src/sb_pointcloud_processing/src/pcl_transform.cpp index a08a2a03d..292f52680 100644 --- a/src/sb_gazebo/src/zed_transform.cpp +++ b/src/sb_pointcloud_processing/src/pcl_transform.cpp @@ -14,38 +14,54 @@ #include ros::Publisher pub; -std::string output_frame; tf2_ros::Buffer tf_buffer; +std::string output_frame; + +// The amount of time (s) for a new pointcloud transform to be published. +double transform_period; + void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) { try { // Create an empty pointcloud sensor_msgs::PointCloud2 output; // Transform the pointcloud to the requested frame geometry_msgs::TransformStamped tf_stamped = tf_buffer.lookupTransform( - msg->header.frame_id, output_frame, ros::Time(0), ros::Duration(1.0)); + msg->header.frame_id, output_frame, ros::Time(0), ros::Duration(0.1)); + tf2::doTransform(*msg, output, tf_stamped); // Publish the transformed pointcloud pub.publish(output); - } catch (tf2::TransformException ex){ + + ros::Duration(transform_period).sleep(); + } catch (tf2::TransformException ex) { ROS_WARN("%s", ex.what()); ros::Duration(1.0).sleep(); } } -int main(int argc, char** argv){ +int main(int argc, char** argv) { ros::init(argc, argv, "pointcloud_transformer"); ros::NodeHandle private_nh("~"); - if (!SB_getParam(private_nh, "output_frame", output_frame)){ + if (!SB_getParam(private_nh, "output_frame", output_frame)) { // Error and exit if we didn't get a frame to transform to. - // We need this to transform anything, and there is no reasonable default - ROS_ERROR("Param 'output_frame' not provided. " \ - "Can't transform anything without a frame to transform it to"); + // We need this to transform anything, and there is no reasonable + // default + ROS_ERROR( + "Param 'output_frame' not provided. " + "Can't transform anything without a frame to transform it to"); return 1; } - ros::Subscriber sub = private_nh.subscribe("/input_pointcloud", 1, pointCloudCallback); - pub = private_nh.advertise("/output_pointcloud", 1); + + double default_transform_period = 0; + SB_getParam( + private_nh, "transform_period", transform_period, default_transform_period); + + ros::Subscriber sub = + private_nh.subscribe("/input_pointcloud", 1, pointCloudCallback); + pub = + private_nh.advertise("/output_pointcloud", 1); tf2_ros::TransformListener tfListener(tf_buffer); ros::spin(); return 0; From 4ff2c93a2e32dd5b96e58a9b17e6a2f3da0c8324 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 16 Feb 2019 09:33:36 -0800 Subject: [PATCH 21/39] Fixed rviz polygon tests to account for cycles --- src/sb_utils/test/rviz_utils_rostest.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 121b20f02..157f3ae92 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -59,7 +59,7 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { polygon.points.push_back(initialisePoint32(1, 1, 0)); visualization_msgs::Marker risk_area_marker = - snowbots::RvizUtils::createPolygonMarker( + snowbots::RvizUtils::createPolygonMarker2D( polygon, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), @@ -70,6 +70,11 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); EXPECT_EQ(ns, risk_area_marker.ns); + // Risk area marker should be a cycle. + risk_area_marker.points.front() = risk_area_marker.points.back(); + + // All other points should be equal. + risk_area_marker.points.erase(risk_area_marker.points.end()); EXPECT_POINT_EQ(polygon.points, risk_area_marker.points); } @@ -83,7 +88,7 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { polygon.points.push_back(initialisePoint32(0, 1, 0)); visualization_msgs::Marker risk_area_marker = - snowbots::RvizUtils::createPolygonMarker( + snowbots::RvizUtils::createPolygonMarker2D( polygon, snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), @@ -94,6 +99,11 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); EXPECT_EQ(ns, risk_area_marker.ns); + // Risk area marker should be a cycle. + risk_area_marker.points.front() = risk_area_marker.points.back(); + + // All other points should be equal. + risk_area_marker.points.erase(risk_area_marker.points.end()); EXPECT_POINT_EQ(polygon.points, risk_area_marker.points); } From 7fbeeb33ff049e8ca1a0a47ae46bb2febde6d175 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 16 Feb 2019 13:21:24 -0800 Subject: [PATCH 22/39] Change realsense tranform from base_link to camera_link --- .../launch/realsense_transform.launch | 4 ++-- src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/sb_pointcloud_processing/launch/realsense_transform.launch b/src/sb_pointcloud_processing/launch/realsense_transform.launch index a75ae52c4..2d1c2c8ae 100644 --- a/src/sb_pointcloud_processing/launch/realsense_transform.launch +++ b/src/sb_pointcloud_processing/launch/realsense_transform.launch @@ -1,11 +1,11 @@ - + "base_link" - + diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index 0cc39abb1..c18eacefe 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -103,7 +103,7 @@ const sensor_msgs::PointCloud2ConstPtr point_cloud) { publishMarkers(pcl_risk); visualization_msgs::MarkerArray risk_area_markers; - std::string frame_id = "camera_color_optical_frame"; + std::string frame_id = "base_link"; std::string ns = "debug"; for (int i = 0; i < pcl_risk.areas.size(); i++) { From ee7df3bda0b50bec2af38f3772bc6b2ddf3399c8 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 16 Feb 2019 13:23:52 -0800 Subject: [PATCH 23/39] Fixed output frame_id for pcl_transform --- src/sb_pointcloud_processing/src/pcl_transform.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sb_pointcloud_processing/src/pcl_transform.cpp b/src/sb_pointcloud_processing/src/pcl_transform.cpp index 292f52680..0bc74d630 100644 --- a/src/sb_pointcloud_processing/src/pcl_transform.cpp +++ b/src/sb_pointcloud_processing/src/pcl_transform.cpp @@ -31,6 +31,8 @@ void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) { tf2::doTransform(*msg, output, tf_stamped); + output.header.frame_id = output_frame; + // Publish the transformed pointcloud pub.publish(output); From 3e21c8f11b7f3b22650b1793e3bdb49d6c114d5e Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 16 Feb 2019 14:01:47 -0800 Subject: [PATCH 24/39] Removed the node for rviz_utils_rostest --- src/sb_utils/test/rviz_utils_rostest.test | 3 +++ src/sb_utils/test/rviz_utils_test.test | 8 -------- 2 files changed, 3 insertions(+), 8 deletions(-) create mode 100644 src/sb_utils/test/rviz_utils_rostest.test delete mode 100644 src/sb_utils/test/rviz_utils_test.test diff --git a/src/sb_utils/test/rviz_utils_rostest.test b/src/sb_utils/test/rviz_utils_rostest.test new file mode 100644 index 000000000..66ee907da --- /dev/null +++ b/src/sb_utils/test/rviz_utils_rostest.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/sb_utils/test/rviz_utils_test.test b/src/sb_utils/test/rviz_utils_test.test deleted file mode 100644 index a0be3ef14..000000000 --- a/src/sb_utils/test/rviz_utils_test.test +++ /dev/null @@ -1,8 +0,0 @@ - - - - - \ No newline at end of file From 06f155360360c5a08583c49220881d0f87d2aa0d Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Thu, 21 Feb 2019 14:00:52 -0800 Subject: [PATCH 25/39] Fixed file naming inside rviz_utils_rostest --- src/sb_utils/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sb_utils/CMakeLists.txt b/src/sb_utils/CMakeLists.txt index 47ec811ef..6a9c6bf94 100644 --- a/src/sb_utils/CMakeLists.txt +++ b/src/sb_utils/CMakeLists.txt @@ -70,7 +70,7 @@ if (CATKIN_ENABLE_TESTING) # Adding gtests to the package catkin_add_gtest(rviz_utils_rostest - test/rviz_utils_test.test + test/rviz_utils_rostest.test test/rviz_utils_rostest.cpp src/RvizUtils.cpp include/RvizUtils.h From 2c5286223ac8d072ac871b167c90956dd01a3876 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Thu, 21 Feb 2019 14:53:31 -0800 Subject: [PATCH 26/39] Documentation comments --- .../include/RiskAnalysis.h | 2 ++ .../src/RiskAnalysis.cpp | 23 +++++++++++-------- .../src/RiskAnalysisNode.cpp | 5 ++-- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index 3bb085e23..ff8593902 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -74,6 +74,8 @@ class RiskAnalysis { int determineColumn(float y); private: + double MAX_RISK = 1; + float region_width; float region_height; float risk_multiplier; diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index 4eb4a14ed..436bc60e0 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -52,6 +52,7 @@ std::vector> RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { std::vector> regions; + // Create the empty cells that fit the specified parameters for (int i = 0; i < num_vertical_cell_div; i++) { std::vector new_region_row; for (int j = 0; j < num_horizontal_cell_div; j++) { @@ -74,10 +75,8 @@ RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { void RiskAnalysis::fillPointRegions( pcl::PointCloud::Ptr pcl, std::vector>& regions) { - pcl::PointXYZ origin_point; - origin_point.x = 0; - origin_point.y = 0; + // Add all valid points to their respective regions for (size_t i = 0; i < pcl->size(); i++) { // Has to be a valid point if (!isnan(pcl->points[i].x) && pcl->points[i].x > 0 && pcl->points[i].x < region_height) { @@ -91,15 +90,10 @@ std::vector>& regions) { int row = determineRow(pcl->points[i].x); int col = determineColumn(pcl->points[i].y); + // Only add in the point if it's in a valid region. bool validColumn = col >= 0 && col < regions[0].size(); bool validRow = row >= 0 && row < regions.size(); - if (validColumn && validRow) { -// std::cout << cur_point.x << ", "; -// std::cout << cur_point.y << "= "; -// -// std::cout << row << ","; -// std::cout << col << std::endl; regions[row][col].points.push_back(cur_point); } } @@ -122,6 +116,9 @@ std::vector> regions) { std_msgs::Float64 risk; risk.data = risk_multiplier*calculateStandardDeviation(z_values); + // Cap the risk score to RISK_MAX + risk.data = std::min(risk.data, MAX_RISK); + // Add risk area onto the array mapping_msgs_urc::RiskArea risk_area; risk_area.score = risk; @@ -152,6 +149,13 @@ float RiskAnalysis::calculateStandardDeviation(std::vector values) { sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, int column) { + // Height corresponds to the x-axis, width corresponds to the y-axis + // +X = UP, +Y = LEFT (ROS Coordinates) + + // Left most region has a column index of 0 and increases as it moves along the y-axis in the negative direction + + // Top most region has a row index of 0 and increases as you moves along the x-axis in the negative direction + sb_geom_msgs::Point2D top_left_point; top_left_point.x = region_height - row * cell_height; top_left_point.y = region_width / 2.0 - column * cell_width; @@ -184,6 +188,5 @@ int RiskAnalysis::determineRow(float x) { int RiskAnalysis::determineColumn(float y) { int col = (region_width / 2.0 - y) / cell_width; - return col; } diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index c18eacefe..48d6468fc 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -106,6 +106,7 @@ const sensor_msgs::PointCloud2ConstPtr point_cloud) { std::string frame_id = "base_link"; std::string ns = "debug"; + // Create the rviz markers for (int i = 0; i < pcl_risk.areas.size(); i++) { visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker2D( @@ -139,8 +140,6 @@ mapping_msgs_urc::RiskAreaArray risk_areas) { visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { - if (risk > 10) - risk = 10; // Round down to the nearest integer to get risk color - return gradient[(int) risk]; + return gradient[(int) risk*10.0]; } \ No newline at end of file From c6d0fa16d4769a888f8cfcf4a699464619e1af27 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 09:57:50 -0800 Subject: [PATCH 27/39] No longer templating and now forcing marker_id specification --- src/decision_igvc/CMakeLists.txt | 1 + .../src/LineExtractorNode.cpp | 2 +- src/sb_utils/CMakeLists.txt | 1 + src/sb_utils/include/RvizUtils.h | 60 ++++------------- src/sb_utils/src/RvizUtils.cpp | 67 ++++++++++++++----- 5 files changed, 67 insertions(+), 64 deletions(-) diff --git a/src/decision_igvc/CMakeLists.txt b/src/decision_igvc/CMakeLists.txt index e64ef0b39..105c19145 100644 --- a/src/decision_igvc/CMakeLists.txt +++ b/src/decision_igvc/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS gps_common tf2_geometry_msgs sb_utils + mapping_msgs_urc ) find_package(OpenCV REQUIRED) diff --git a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp index 84759f50d..3765c636b 100644 --- a/src/sb_pointcloud_processing/src/LineExtractorNode.cpp +++ b/src/sb_pointcloud_processing/src/LineExtractorNode.cpp @@ -127,7 +127,7 @@ void LineExtractorNode::visualizeClusters() { std::string ns = "debug"; visualization_msgs::Marker marker = snowbots::RvizUtils::createMarker( - cluster_points, colors, scale, this->frame_id, ns); + cluster_points, colors, scale, this->frame_id, ns, 0); rviz_cluster_publisher.publish(marker); } diff --git a/src/sb_utils/CMakeLists.txt b/src/sb_utils/CMakeLists.txt index 6a9c6bf94..8dabda448 100644 --- a/src/sb_utils/CMakeLists.txt +++ b/src/sb_utils/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-std=c++14) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp + mapping_msgs_urc ) diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index f0ff17aad..160ea54b9 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -17,6 +17,9 @@ #include #include +#include "sb_geom_msgs/Point2D.h" +#include "sb_geom_msgs/Polygon2D.h" + namespace snowbots { namespace RvizUtils { /** @@ -35,8 +38,8 @@ namespace RvizUtils { visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); + int marker_id, + int type = visualization_msgs::Marker::POINTS); /** * Turn points into a marker for rviz @@ -55,8 +58,8 @@ namespace RvizUtils { visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); + int marker_id, + int type = visualization_msgs::Marker::POINTS); /** * Turn a point into a marker for rviz @@ -93,8 +96,8 @@ namespace RvizUtils { visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); + int marker_id, + int type = visualization_msgs::Marker::POINTS ); /** * Turn a polygon into a marker for rviz @@ -106,49 +109,14 @@ namespace RvizUtils { * * @return an rviz marker */ - template visualization_msgs::Marker - createPolygonMarker2D(T polygon, + createPolygonMarker2D(sb_geom_msgs::Polygon2D polygon, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1) { - visualization_msgs::Marker marker; - - marker.header.stamp = ros::Time::now(); - marker.action = visualization_msgs::Marker::ADD; - marker.pose.orientation.w = 1.0; - - marker.type = type; - marker.id = id; - - marker.header.frame_id = frame_id; - marker.ns = ns; - - marker.scale = scale; - - // Set the color - marker.color = color; - - // Setup the line strip - for (int i = 0; i < polygon.points.size(); i++) { - geometry_msgs::Point point; - point.x = polygon.points[i].x; - point.y = polygon.points[i].y; - point.z = 0; - marker.points.push_back(point); - } - - geometry_msgs::Point point; - point.x = polygon.points[0].x; - point.y = polygon.points[0].y; - point.z = 0; - marker.points.push_back(point); - - return marker; - } + int marker_id, + int type = visualization_msgs::Marker::LINE_STRIP); /** * Turn a polygon into a marker for rviz * @@ -165,8 +133,8 @@ namespace RvizUtils { visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::LINE_STRIP, - int id = 1); + int marker_id, + int type = visualization_msgs::Marker::LINE_STRIP); /** * Creates a Marker Array (array of Markers) diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 9ab55511b..845b15731 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -24,8 +24,8 @@ namespace RvizUtils { visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, - int type = visualization_msgs::Marker::POINTS, - int id = 0); + int marker_id, + int type = visualization_msgs::Marker::POINTS) ; } } @@ -35,11 +35,11 @@ snowbots::RvizUtils::createMarker(std::vector points, Marker::_scale_type scale, std::string frame_id, std::string ns, - int type, - int id) { + int marker_id, + int type) { Marker marker; - setupMarker(marker, scale, frame_id, ns, type, id); + setupMarker(marker, scale, frame_id, ns, marker_id, type); // Set the color marker.color = color; @@ -56,11 +56,11 @@ snowbots::RvizUtils::createMarker(std::vector points, Marker::_scale_type scale, std::string frame_id, std::string ns, - int type, - int id) { + int marker_id, + int type) { Marker marker; - setupMarker(marker, scale, frame_id, ns, type, id); + setupMarker(marker, scale, frame_id, ns, marker_id, type); // Set the colors marker.colors = colors; @@ -77,11 +77,11 @@ snowbots::RvizUtils::createMarker(geometry_msgs::Point point, Marker::_scale_type scale, std::string frame_id, std::string ns, - int type, - int id) { + int marker_id, + int type) { Marker marker; - setupMarker(marker, scale, frame_id, ns, type, id); + setupMarker(marker, scale, frame_id, ns, marker_id, type); // Set the color marker.colors = colors; @@ -97,11 +97,11 @@ Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, Marker::_scale_type scale, std::string frame_id, std::string ns, - int type, - int id) { + int marker_id, + int type) { Marker marker; - setupMarker(marker, scale, frame_id, ns, type, id); + setupMarker(marker, scale, frame_id, ns, marker_id, type); // Set the color marker.color = color; @@ -112,17 +112,50 @@ Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, return marker; } +visualization_msgs::Marker +snowbots::RvizUtils::createPolygonMarker2D(sb_geom_msgs::Polygon2D polygon, + visualization_msgs::Marker::_color_type color, + visualization_msgs::Marker::_scale_type scale, + std::string frame_id, + std::string ns, + int marker_id, + int type) { + visualization_msgs::Marker marker; + + setupMarker(marker, scale, frame_id, ns, marker_id, type); + + // Set the color + marker.color = color; + + // Setup the line strip + for (int i = 0; i < polygon.points.size(); i++) { + geometry_msgs::Point point; + point.x = polygon.points[i].x; + point.y = polygon.points[i].y; + point.z = 0; + marker.points.push_back(point); + } + + geometry_msgs::Point point; + point.x = polygon.points[0].x; + point.y = polygon.points[0].y; + point.z = 0; + marker.points.push_back(point); + + return marker; +} + Marker snowbots::RvizUtils::createPolygonMarker3D( geometry_msgs::Polygon polygon, visualization_msgs::Marker::_color_type color, visualization_msgs::Marker::_scale_type scale, std::string frame_id, std::string ns, -int type, -int id) { +int marker_id, +int type) { Marker marker; - setupMarker(marker, scale, frame_id, ns, type, id); + setupMarker(marker, scale, frame_id, ns, marker_id, type); // Set the color marker.color = color; From 929edc02420698e78dc57c276c8cca840a25fe88 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 10:23:38 -0800 Subject: [PATCH 28/39] Fixed ros_tests to work with sb_geom_msgs::Polygon2D --- src/sb_utils/test/rviz_utils_rostest.cpp | 39 +++++++++++++----------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/sb_utils/test/rviz_utils_rostest.cpp b/src/sb_utils/test/rviz_utils_rostest.cpp index 157f3ae92..d2e9d90f7 100644 --- a/src/sb_utils/test/rviz_utils_rostest.cpp +++ b/src/sb_utils/test/rviz_utils_rostest.cpp @@ -30,11 +30,10 @@ class RvizUtilsRosTest : public testing::Test { template void EXPECT_POINT_EQ(std::vector points1, std::vector points2); -geometry_msgs::Point32 initialisePoint32(float x, float y, float z) { - geometry_msgs::Point32 point; +sb_geom_msgs::Point2D initialisePoint(float x, float y) { + sb_geom_msgs::Point2D point; point.x = x; point.y = y; - point.z = z; return point; } @@ -52,11 +51,11 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { std::string frame_id = "camera_color_optical_frame"; std::string ns = "debug"; - geometry_msgs::Polygon polygon; - polygon.points.push_back(initialisePoint32(0, 0, 0)); - polygon.points.push_back(initialisePoint32(1, 0, 0)); - polygon.points.push_back(initialisePoint32(0, 1, 0)); - polygon.points.push_back(initialisePoint32(1, 1, 0)); + sb_geom_msgs::Polygon2D polygon; + polygon.points.push_back(initialisePoint(0, 0)); + polygon.points.push_back(initialisePoint(1, 0)); + polygon.points.push_back(initialisePoint(0, 1)); + polygon.points.push_back(initialisePoint(1, 1)); visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker2D( @@ -64,7 +63,8 @@ TEST_F(RvizUtilsRosTest, boxPolygonMarker) { snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, - ns); + ns, + 0); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -82,10 +82,10 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { std::string frame_id = "camera_color_optical_frame"; std::string ns = "debug"; - geometry_msgs::Polygon polygon; - polygon.points.push_back(initialisePoint32(0, 0, 0)); - polygon.points.push_back(initialisePoint32(1, 0, 0)); - polygon.points.push_back(initialisePoint32(0, 1, 0)); + sb_geom_msgs::Polygon2D polygon; + polygon.points.push_back(initialisePoint(0, 0)); + polygon.points.push_back(initialisePoint(1, 0)); + polygon.points.push_back(initialisePoint(0, 1)); visualization_msgs::Marker risk_area_marker = snowbots::RvizUtils::createPolygonMarker2D( @@ -93,7 +93,8 @@ TEST_F(RvizUtilsRosTest, trianglePolygonMarker) { snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, - ns); + ns, + 0); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -119,7 +120,8 @@ TEST_F(RvizUtilsRosTest, singlePointMarker) { snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, - ns); + ns, + 0); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -143,7 +145,8 @@ TEST_F(RvizUtilsRosTest, singlePointMarkerInArray) { snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, - ns); + ns, + 0); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -168,7 +171,8 @@ TEST_F(RvizUtilsRosTest, multiplePointMarkersInArray) { snowbots::RvizUtils::createMarkerColor(1.0f, 0, 0, 1.0f), snowbots::RvizUtils::createMarkerScale(0.1, 0, 0), frame_id, - ns); + ns, + 0); // Make sure message metadata is correct EXPECT_EQ(frame_id, risk_area_marker.header.frame_id); @@ -214,7 +218,6 @@ void EXPECT_POINT_EQ(std::vector points1, std::vector points2) { for (int i = 0; i < points1.size(); i++) { EXPECT_EQ(points1[i].x, points2[i].x); EXPECT_EQ(points1[i].y, points2[i].y); - EXPECT_EQ(points1[i].z, points2[i].z); } } } From 5a0bf47d06811a7472a5aa834b94b44b195200ed Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 11:27:12 -0800 Subject: [PATCH 29/39] Added more function comments --- .../include/RiskAnalysis.h | 25 +++++++++++++++++-- .../src/RiskAnalysis.cpp | 4 +-- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index ff8593902..bb9c496fe 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -51,14 +51,35 @@ class RiskAnalysis { RiskAnalysis(); /** - * Analyses a pointcloud + * Assigns risk to regions with enough information (min_points_in_region). + * Uses standard deviation of point height to measure risk. + * + * @param point_cloud the point cloud of the ground + * @return regions with associated risk */ mapping_msgs_urc::RiskAreaArray assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); + /** + * Creates a 2D vector of RegionOfPoints where... + * # rows = num_vertical_cell_div + * # cols = num_horizontal_cell_div + * + * Each region's height and width correspond to cell_width and + * cell_height + * + * The points are intialised as empty + * + * @return 2D vector of RegionOfPoints + */ std::vector> - initialisePointRegions(pcl::PCLPointCloud2 point_cloud); + initialisePointRegions(); + /** + * Fills each region's points with the specified + * @param pcl + * @param regions + */ void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector>& regions); diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index 436bc60e0..25288f7e4 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -34,7 +34,7 @@ RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) { // Initialise regions with an area and associated points std::vector> regions = - initialisePointRegions(point_cloud); + initialisePointRegions(); // Convert to PCLPointCloud pcl::PointCloud::Ptr pcl( @@ -49,7 +49,7 @@ RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) { } std::vector> -RiskAnalysis::initialisePointRegions(pcl::PCLPointCloud2 point_cloud) { +RiskAnalysis::initialisePointRegions() { std::vector> regions; // Create the empty cells that fit the specified parameters From dd7916dc6e03a72a7fc124856a1c22e37abf165e Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 12:05:23 -0800 Subject: [PATCH 30/39] Fixed tests, and added more function comments --- .../include/RiskAnalysis.h | 40 ++++++++++++++++++- .../include/RiskAnalysisNode.h | 12 ++++++ .../src/RiskAnalysisNode.cpp | 4 +- .../test/risk-analysis-test.cpp | 30 ++++++++++---- 4 files changed, 76 insertions(+), 10 deletions(-) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index bb9c496fe..3b44b7433 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -76,27 +76,64 @@ class RiskAnalysis { initialisePointRegions(); /** - * Fills each region's points with the specified + * Fills each region's vector of points with the input point cloud. + * The region chosen is dependent on the point's x and y values. + * * @param pcl * @param regions */ void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector>& regions); + /** + * Assigns risk to regions with enough information (min_points_in_region). + * Uses standard deviation of point height (z-value) to measure risk. + * + * @param regions 2D vector + * @return regions with associated risk + */ mapping_msgs_urc::RiskAreaArray analysePointRegions(std::vector> regions); + /** + * Calculates the standard deviation of a given vector of values + * + * @param values + * @return float standard deviation of inputted floats + */ float calculateStandardDeviation(std::vector values); + /** + * Creates a region in a location specified by the inputted row and column. + * + * The dimensions of the region are equal to cell_height and cell_width. + * + * @param row + * @param column + * @return sb_geom_msgs::Polygon2D region + */ sb_geom_msgs::Polygon2D getRegionAreaFromIndices(int row, int column); + /** + * Given an x value, return the correct row that the point should be in. + * + * @param x the x-value of a point + * @return int the row that corresponds to the x-value + */ int determineRow(float x); + /** + * Given a y value, return the correct column that the point should be in. + * + * @param y the y-value of a point + * @return int the column that corresponds to the y-value + */ int determineColumn(float y); private: double MAX_RISK = 1; + // Dimensions of the region float region_width; float region_height; float risk_multiplier; @@ -106,6 +143,7 @@ class RiskAnalysis { int region_min_points; + // Dimension of cells float cell_width; float cell_height; int total_cells; diff --git a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h index 70830c2d7..94e3f019d 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h @@ -46,8 +46,20 @@ class RiskAnalysisNode { */ void pclCallBack(const sensor_msgs::PointCloud2ConstPtr point_cloud); + /** + * Adds in all the header information for the message then publishes + * the risk_areas + * + * @param risk_areas + */ void publishMarkers(mapping_msgs_urc::RiskAreaArray risk_areas); + /** + * Returns a color corresponding to the inputted risk + * + * @param risk + * @return visualization_msgs::Marker::_color_type color corresponding to inputted risk + */ visualization_msgs::Marker::_color_type convertRiskToColor(float risk); RiskAnalysis risk_analysis; diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index 48d6468fc..6391a2bfa 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -115,8 +115,8 @@ const sensor_msgs::PointCloud2ConstPtr point_cloud) { snowbots::RvizUtils::createMarkerScale(0.01, 0, 0), frame_id, ns, - visualization_msgs::Marker::LINE_STRIP, - i + i, + visualization_msgs::Marker::LINE_STRIP ); risk_area_markers.markers.push_back(risk_area_marker); diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 8035b93a7..6135f78e0 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -32,11 +32,13 @@ TEST(RegionCreation, DivisionsEqualToDimensions) { int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; int min_points_in_region = 1; + float risk_multiplier = 1; RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); sb_geom_msgs::Polygon2D top_left_region = risk_analysis.getRegionAreaFromIndices(0, 0); @@ -70,15 +72,17 @@ TEST(RegionInitialisation, DivisionsEqualToDimensions) { int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; int min_points_in_region = 1; + float risk_multiplier = 1; RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); std::vector> point_region; - point_region = risk_analysis.initialisePointRegions(empty_cloud); + point_region = risk_analysis.initialisePointRegions(); EXPECT_EQ(num_vertical_cell_divs, point_region.size()); EXPECT_EQ(num_horizontal_cell_divs, point_region[0].size()); @@ -96,11 +100,14 @@ TEST(RegionAnalysis, DivisionsEqualToDimensions) { int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; int min_points_in_region = 1; + float risk_multiplier = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); @@ -112,11 +119,14 @@ TEST(RegionCreation, NonIntegerCellDimensions) { int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; int min_points_in_region = 1; + float risk_multiplier = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); sb_geom_msgs::Polygon2D top_left_region = risk_analysis.getRegionAreaFromIndices(0, 0); @@ -149,11 +159,14 @@ TEST(RegionAnalysis, NonIntegerCellDimensions) { int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; int min_points_in_region = 1; + float risk_multiplier = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); @@ -165,11 +178,14 @@ TEST(RegionAnalysis, OneDiv) { int num_vertical_cell_divs = 2; int num_horizontal_cell_divs = 1; int min_points_in_region = -1; + float risk_multiplier = 1; + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, - min_points_in_region); + min_points_in_region, + risk_multiplier); mapping_msgs_urc::RiskAreaArray pcl_risk = risk_analysis.assessPointCloudRisk(empty_cloud); From a15b3822bb6297b75453b186f8f8a5f5bdc6cfbe Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 12:09:46 -0800 Subject: [PATCH 31/39] Ran fix formatting --- src/sb_utils/include/RvizUtils.h | 2 +- src/sb_utils/src/RvizUtils.cpp | 36 ++++++++++++++++---------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/sb_utils/include/RvizUtils.h b/src/sb_utils/include/RvizUtils.h index 160ea54b9..3dbd6edbd 100644 --- a/src/sb_utils/include/RvizUtils.h +++ b/src/sb_utils/include/RvizUtils.h @@ -97,7 +97,7 @@ namespace RvizUtils { std::string frame_id, std::string ns, int marker_id, - int type = visualization_msgs::Marker::POINTS ); + int type = visualization_msgs::Marker::POINTS); /** * Turn a polygon into a marker for rviz diff --git a/src/sb_utils/src/RvizUtils.cpp b/src/sb_utils/src/RvizUtils.cpp index 845b15731..20c08f57f 100644 --- a/src/sb_utils/src/RvizUtils.cpp +++ b/src/sb_utils/src/RvizUtils.cpp @@ -25,7 +25,7 @@ namespace RvizUtils { std::string frame_id, std::string ns, int marker_id, - int type = visualization_msgs::Marker::POINTS) ; + int type = visualization_msgs::Marker::POINTS); } } @@ -112,36 +112,36 @@ Marker snowbots::RvizUtils::createMarker(geometry_msgs::Point point, return marker; } -visualization_msgs::Marker -snowbots::RvizUtils::createPolygonMarker2D(sb_geom_msgs::Polygon2D polygon, - visualization_msgs::Marker::_color_type color, - visualization_msgs::Marker::_scale_type scale, - std::string frame_id, - std::string ns, - int marker_id, - int type) { +visualization_msgs::Marker snowbots::RvizUtils::createPolygonMarker2D( +sb_geom_msgs::Polygon2D polygon, +visualization_msgs::Marker::_color_type color, +visualization_msgs::Marker::_scale_type scale, +std::string frame_id, +std::string ns, +int marker_id, +int type) { visualization_msgs::Marker marker; setupMarker(marker, scale, frame_id, ns, marker_id, type); - + // Set the color marker.color = color; - + // Setup the line strip for (int i = 0; i < polygon.points.size(); i++) { - geometry_msgs::Point point; - point.x = polygon.points[i].x; - point.y = polygon.points[i].y; - point.z = 0; - marker.points.push_back(point); + geometry_msgs::Point point; + point.x = polygon.points[i].x; + point.y = polygon.points[i].y; + point.z = 0; + marker.points.push_back(point); } - + geometry_msgs::Point point; point.x = polygon.points[0].x; point.y = polygon.points[0].y; point.z = 0; marker.points.push_back(point); - + return marker; } From fc24b5c472945f0d44ca53305ccf20b634618412 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 13:58:41 -0800 Subject: [PATCH 32/39] Added better tests for region initialisation --- .../test/risk-analysis-test.cpp | 193 +++++++++++------- 1 file changed, 122 insertions(+), 71 deletions(-) diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 6135f78e0..9b705f391 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -10,60 +10,57 @@ #include #include -/* - * Function Prototype for Risk Analysis - * - * RiskAnalysis(float region_width, float region_height, int - * num_vertical_cell_div, - * int num_horizontal_cell_div, int region_min_points); +pcl::PCLPointCloud2 empty_cloud = pcl::PCLPointCloud2(); + +/** + * Runs tests to verify the inputted point_region has the proper dimensions specified + * by all the other inputs * + * @param point_region + * @param region_width + * @param region_height + * @param num_vertical_cell_divs + * @param num_horizontal_cell_divs */ - -pcl::PCLPointCloud2 empty_cloud = pcl::PCLPointCloud2(); void testRegionDimensions(std::vector> point_region, float region_width, float region_height, int num_vertical_cell_divs, int num_horizontal_cell_divs); -TEST(RegionCreation, DivisionsEqualToDimensions) { - float region_width = 10; - float region_height = 10; - int num_vertical_cell_divs = 10; - int num_horizontal_cell_divs = 10; - int min_points_in_region = 1; - float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, - region_height, - num_vertical_cell_divs, - num_horizontal_cell_divs, - min_points_in_region, - risk_multiplier); - - sb_geom_msgs::Polygon2D top_left_region = - risk_analysis.getRegionAreaFromIndices(0, 0); - EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); - EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); - - sb_geom_msgs::Polygon2D top_right_region = - risk_analysis.getRegionAreaFromIndices(0, 9); - EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); - EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); - - sb_geom_msgs::Polygon2D bottom_right_region = - risk_analysis.getRegionAreaFromIndices(9, 9); - EXPECT_FLOAT_EQ(0, bottom_right_region.points[2].x); - EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); - sb_geom_msgs::Polygon2D bottom_left_region = - risk_analysis.getRegionAreaFromIndices(9, 0); - EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); - EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); - - sb_geom_msgs::Polygon2D test_region = - risk_analysis.getRegionAreaFromIndices(2, 9); +/** + * + * @param risk_analysis + * @param region_width + * @param region_height + * @param num_vertical_cell_divs + * @param num_horizontal_cell_divs + */ +void testRegionLocations(RiskAnalysis risk_analysis, + float region_width, + float region_height, + int num_vertical_cell_divs, + int num_horizontal_cell_divs); +/** + * Runs tests to determine equality of points + */ +void EXPECT_POINT_EQ(sb_geom_msgs::Point2D expected_val, sb_geom_msgs::Point2D actual_val) { + float abs_error = 0.0001; - std::cout << risk_analysis.determineColumn(-4.5) << std::endl; + EXPECT_NEAR(expected_val.x, actual_val.x, abs_error); + EXPECT_NEAR(expected_val.y, actual_val.y, abs_error); +} + + /** + * Creates a point with specified values + */ +sb_geom_msgs::Point2D createPoint(float x, float y) { + sb_geom_msgs::Point2D point; + point.x = x; + point.y = y; + + return point; } TEST(RegionInitialisation, DivisionsEqualToDimensions) { @@ -92,6 +89,8 @@ TEST(RegionInitialisation, DivisionsEqualToDimensions) { region_height, num_vertical_cell_divs, num_horizontal_cell_divs); + + testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); } TEST(RegionAnalysis, DivisionsEqualToDimensions) { @@ -113,7 +112,7 @@ TEST(RegionAnalysis, DivisionsEqualToDimensions) { risk_analysis.assessPointCloudRisk(empty_cloud); } -TEST(RegionCreation, NonIntegerCellDimensions) { +TEST(RegionInitialisation, NonIntegerCellDimensions) { float region_width = 10; float region_height = 10; int num_vertical_cell_divs = 100; @@ -128,28 +127,20 @@ TEST(RegionCreation, NonIntegerCellDimensions) { min_points_in_region, risk_multiplier); - sb_geom_msgs::Polygon2D top_left_region = - risk_analysis.getRegionAreaFromIndices(0, 0); - EXPECT_FLOAT_EQ(region_height, top_left_region.points[0].x); - EXPECT_FLOAT_EQ(region_width / 2, top_left_region.points[0].y); + std::vector> point_region; - sb_geom_msgs::Polygon2D top_right_region = - risk_analysis.getRegionAreaFromIndices(0, 99); - EXPECT_FLOAT_EQ(region_height, top_right_region.points[1].x); - EXPECT_FLOAT_EQ(-region_width / 2, top_right_region.points[1].y); + point_region = risk_analysis.initialisePointRegions(); - sb_geom_msgs::Polygon2D bottom_right_region = - risk_analysis.getRegionAreaFromIndices(99, 99); - EXPECT_NEAR(0, bottom_right_region.points[2].x, 0.001); - EXPECT_FLOAT_EQ(-region_width / 2, bottom_right_region.points[2].y); + EXPECT_EQ(num_vertical_cell_divs, point_region.size()); + EXPECT_EQ(num_horizontal_cell_divs, point_region[0].size()); - sb_geom_msgs::Polygon2D bottom_left_region = - risk_analysis.getRegionAreaFromIndices(99, 0); - EXPECT_FLOAT_EQ(0, bottom_left_region.points[3].x); - EXPECT_FLOAT_EQ(region_width / 2, bottom_left_region.points[3].y); + testRegionDimensions(point_region, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); - int row = risk_analysis.determineRow(2.8); - int col = risk_analysis.determineColumn(0); + testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); } @@ -172,6 +163,37 @@ TEST(RegionAnalysis, NonIntegerCellDimensions) { risk_analysis.assessPointCloudRisk(empty_cloud); } +TEST(RegionInitialisation, OneDiv) { + float region_width = 2; + float region_height = 2; + int num_vertical_cell_divs = 2; + int num_horizontal_cell_divs = 1; + int min_points_in_region = -1; + float risk_multiplier = 1; + + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, + min_points_in_region, + risk_multiplier); + + std::vector> point_region; + + point_region = risk_analysis.initialisePointRegions(); + + EXPECT_EQ(num_vertical_cell_divs, point_region.size()); + EXPECT_EQ(num_horizontal_cell_divs, point_region[0].size()); + + testRegionDimensions(point_region, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); + + testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); +} + TEST(RegionAnalysis, OneDiv) { float region_width = 2; float region_height = 2; @@ -199,29 +221,58 @@ void testRegionDimensions(std::vector> point_region, float cell_width = region_width / num_horizontal_cell_divs; float cell_height = region_height / num_vertical_cell_divs; + float abs_error = 0.001; for (int i = 0; i < point_region.size(); i++) { for (int j = 0; j < point_region[0].size(); j++) { sb_geom_msgs::Polygon2D cur_cell = point_region[i][j].region_area; // Top Left Point vs Bottom Left Point - EXPECT_FLOAT_EQ(cell_height, - cur_cell.points[0].x - cur_cell.points[3].x); + EXPECT_NEAR(cell_height, cur_cell.points[0].x - cur_cell.points[3].x, abs_error); // Top Right Point vs Bottom Right Point - EXPECT_FLOAT_EQ(cell_height, - cur_cell.points[1].x - cur_cell.points[2].x); + EXPECT_NEAR(cell_height, cur_cell.points[1].x - cur_cell.points[2].x, abs_error); // Top Left Point vs Top Right Point - EXPECT_FLOAT_EQ(cell_width, - cur_cell.points[0].y - cur_cell.points[1].y); + EXPECT_NEAR(cell_width, cur_cell.points[0].y - cur_cell.points[1].y, abs_error); // Bottom Left Point vs Bottom Right Point - EXPECT_FLOAT_EQ(cell_width, - cur_cell.points[3].y - cur_cell.points[2].y); + EXPECT_NEAR(cell_width, cur_cell.points[3].y - cur_cell.points[2].y, abs_error); } } } +void testRegionLocations(RiskAnalysis risk_analysis, + float region_width, + float region_height, + int num_vertical_cell_divs, + int num_horizontal_cell_divs) { + float cell_width = region_width / num_horizontal_cell_divs; + float cell_height = region_height / num_vertical_cell_divs; + + float cur_cell_top = region_height; + for (int i = 0; i < num_vertical_cell_divs; i++) { + float cur_cell_left = region_width / 2; + + for (int j = 0; j < num_horizontal_cell_divs; j++) { + sb_geom_msgs::Polygon2D cur_region = + risk_analysis.getRegionAreaFromIndices(i, j); + + sb_geom_msgs::Point2D top_left_point = createPoint(cur_cell_top, cur_cell_left); + sb_geom_msgs::Point2D top_right_point = createPoint(cur_cell_top, cur_cell_left - cell_width); + sb_geom_msgs::Point2D bottom_right_point = createPoint(cur_cell_top - cell_height, cur_cell_left - cell_width); + sb_geom_msgs::Point2D bottom_left_point = createPoint(cur_cell_top - cell_height, cur_cell_left); + + EXPECT_POINT_EQ(top_left_point, cur_region.points[0]); + EXPECT_POINT_EQ(top_right_point, cur_region.points[1]); + EXPECT_POINT_EQ(bottom_right_point, cur_region.points[2]); + EXPECT_POINT_EQ(bottom_left_point, cur_region.points[3]); + cur_cell_left -= cell_width; + } + + cur_cell_top -= cell_height; + } +} + TEST(StandardDeviation, OneDataPoint) { RiskAnalysis risk_analysis = RiskAnalysis(); From 5f11cf140972f36fa9643d1490de2d3f882a880d Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 23 Feb 2019 14:15:11 -0800 Subject: [PATCH 33/39] Updated sb_utils to build messages first --- src/sb_utils/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sb_utils/CMakeLists.txt b/src/sb_utils/CMakeLists.txt index 8dabda448..33ee274ec 100644 --- a/src/sb_utils/CMakeLists.txt +++ b/src/sb_utils/CMakeLists.txt @@ -61,6 +61,8 @@ add_library(sb_utils target_link_libraries(sb_utils ${catkin_LIBRARIES}) +add_dependencies(sb_utils ${catkin_EXPORTED_TARGETS}) + ############# ## Testing ## From 010a909f5339d6dad56ae2298a34eaf8e68325dd Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 2 Mar 2019 10:31:21 -0800 Subject: [PATCH 34/39] Added transform_period --- src/sb_pointcloud_processing/launch/realsense_transform.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sb_pointcloud_processing/launch/realsense_transform.launch b/src/sb_pointcloud_processing/launch/realsense_transform.launch index 2d1c2c8ae..23facf8a8 100644 --- a/src/sb_pointcloud_processing/launch/realsense_transform.launch +++ b/src/sb_pointcloud_processing/launch/realsense_transform.launch @@ -1,7 +1,8 @@ - + "base_link" + 1 From 2a647554ec697f403bba0b0fb5ae0ff1d33eb9f6 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 2 Mar 2019 11:54:22 -0800 Subject: [PATCH 35/39] Added constructor comments --- src/sb_pointcloud_processing/include/RiskAnalysis.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index 3b44b7433..56ebba3c8 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -36,7 +36,14 @@ typedef struct { class RiskAnalysis { public: /** - * Constructor + * Constructor for RiskAnalysis + * + * @param region_width determines the range in the y-axis [-region_width/2, region_width/2] + * @param region_height determines the range in the x-axis [0, region_height] + * @param num_vertical_cell_div determines the number of rows + * @param num_horizontal_cell_div determines the number of columns + * @param region_min_points the minimum amount of points to conclude riskiness of a risk area + * @param risk_multiplier a multiplier applied to the risk after calculating the risk of the region */ RiskAnalysis(float region_width, float region_height, From eac647039de2f7c024088dbaa24abebe3ba68a10 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 2 Mar 2019 12:01:31 -0800 Subject: [PATCH 36/39] Fixed some documentation inside RiskAnalysis.h --- .../include/RiskAnalysis.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index 56ebba3c8..ec347a0c1 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -68,9 +68,9 @@ class RiskAnalysis { assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud); /** - * Creates a 2D vector of RegionOfPoints where... - * # rows = num_vertical_cell_div - * # cols = num_horizontal_cell_div + * Creates a 2D row-column indexed vector of RegionOfPoints where... + * first_index = # rows = num_vertical_cell_div + * second_index = # cols = num_horizontal_cell_div * * Each region's height and width correspond to cell_width and * cell_height @@ -83,11 +83,12 @@ class RiskAnalysis { initialisePointRegions(); /** - * Fills each region's vector of points with the input point cloud. - * The region chosen is dependent on the point's x and y values. + * Fills each region's vector of points with the points + * from the point cloud that are located inside that region's + * area. * - * @param pcl - * @param regions + * @param pcl point cloud + * @param regions 2D row-column indexed vector of RegionOfPoints */ void fillPointRegions(pcl::PointCloud::Ptr pcl, std::vector>& regions); From 15c396fe16c99f7389a230fa0e52530efe66b43d Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 2 Mar 2019 12:48:37 -0800 Subject: [PATCH 37/39] Added more comments for the realsense_transform file --- src/sb_pointcloud_processing/launch/realsense_transform.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/sb_pointcloud_processing/launch/realsense_transform.launch b/src/sb_pointcloud_processing/launch/realsense_transform.launch index 23facf8a8..b8ca66cee 100644 --- a/src/sb_pointcloud_processing/launch/realsense_transform.launch +++ b/src/sb_pointcloud_processing/launch/realsense_transform.launch @@ -1,5 +1,6 @@ + "base_link" 1 @@ -8,5 +9,7 @@ + + From 9ec0dfe97edc104f39520c3e8c24a65b75c839ed Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 2 Mar 2019 13:41:51 -0800 Subject: [PATCH 38/39] Moved end of file down --- src/sb_pointcloud_processing/src/RiskAnalysis.cpp | 6 ++++++ src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp | 2 +- src/sb_pointcloud_processing/test/risk-analysis-test.cpp | 2 +- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index 25288f7e4..6e80671a7 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -136,10 +136,16 @@ float RiskAnalysis::calculateStandardDeviation(std::vector values) { float mean = sum / values.size(); std::vector diff(values.size()); + + // Calculates the different of each value in values from the mean + // and stores it in the diff vector std::transform(values.begin(), values.end(), diff.begin(), std::bind2nd(std::minus(), mean)); + + // Squares all the differences inside the diff vector, then totals + // them all into the sq_sum. double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); double stdev = std::sqrt(sq_sum / values.size()); diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index 6391a2bfa..266e5487f 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -142,4 +142,4 @@ visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { // Round down to the nearest integer to get risk color return gradient[(int) risk*10.0]; -} \ No newline at end of file +} diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 9b705f391..451b8c91e 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -337,4 +337,4 @@ TEST(StandardDeviation, RandomDataPointsLargeNumbers) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 32567f985277eb687636606134ef0934d6a4f517 Mon Sep 17 00:00:00 2001 From: Robyn Castro Date: Sat, 9 Mar 2019 10:30:23 -0800 Subject: [PATCH 39/39] Fixed formatting --- src/sb_pointcloud_processing/CMakeLists.txt | 4 - .../include/RiskAnalysis.h | 16 +-- .../include/RiskAnalysisNode.h | 5 +- .../src/RiskAnalysis.cpp | 20 +-- .../src/RiskAnalysisNode.cpp | 11 +- .../test/risk-analysis-test.cpp | 114 +++++++++++------- 6 files changed, 96 insertions(+), 74 deletions(-) diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/src/sb_pointcloud_processing/CMakeLists.txt index 89428dae5..a6c314450 100644 --- a/src/sb_pointcloud_processing/CMakeLists.txt +++ b/src/sb_pointcloud_processing/CMakeLists.txt @@ -98,10 +98,6 @@ add_executable(risk_analysis include/RiskAnalysis.h include/RiskAnalysisNode.h ) -add_executable(test_pcl_generator_node - src/test_pcl_generator_node.cpp - test/TestUtils.h -) add_executable(pcl_transform src/pcl_transform.cpp) diff --git a/src/sb_pointcloud_processing/include/RiskAnalysis.h b/src/sb_pointcloud_processing/include/RiskAnalysis.h index ec347a0c1..df4eca667 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysis.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysis.h @@ -24,7 +24,6 @@ #include #include - // Standard Libraries #include @@ -38,12 +37,16 @@ class RiskAnalysis { /** * Constructor for RiskAnalysis * - * @param region_width determines the range in the y-axis [-region_width/2, region_width/2] - * @param region_height determines the range in the x-axis [0, region_height] + * @param region_width determines the range in the y-axis [-region_width/2, + * region_width/2] + * @param region_height determines the range in the x-axis [0, + * region_height] * @param num_vertical_cell_div determines the number of rows * @param num_horizontal_cell_div determines the number of columns - * @param region_min_points the minimum amount of points to conclude riskiness of a risk area - * @param risk_multiplier a multiplier applied to the risk after calculating the risk of the region + * @param region_min_points the minimum amount of points to conclude + * riskiness of a risk area + * @param risk_multiplier a multiplier applied to the risk after calculating + * the risk of the region */ RiskAnalysis(float region_width, float region_height, @@ -79,8 +82,7 @@ class RiskAnalysis { * * @return 2D vector of RegionOfPoints */ - std::vector> - initialisePointRegions(); + std::vector> initialisePointRegions(); /** * Fills each region's vector of points with the points diff --git a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h index 94e3f019d..54340ecef 100644 --- a/src/sb_pointcloud_processing/include/RiskAnalysisNode.h +++ b/src/sb_pointcloud_processing/include/RiskAnalysisNode.h @@ -21,8 +21,8 @@ // Point Cloud #include #include -#include #include +#include #include // Risk Analysis @@ -58,7 +58,8 @@ class RiskAnalysisNode { * Returns a color corresponding to the inputted risk * * @param risk - * @return visualization_msgs::Marker::_color_type color corresponding to inputted risk + * @return visualization_msgs::Marker::_color_type color corresponding to + * inputted risk */ visualization_msgs::Marker::_color_type convertRiskToColor(float risk); diff --git a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp index 6e80671a7..1cb0d2cc6 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysis.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysis.cpp @@ -18,7 +18,7 @@ RiskAnalysis::RiskAnalysis(float region_width, region_min_points(region_min_points), num_vertical_cell_div(num_vertical_cell_div), num_horizontal_cell_div(num_horizontal_cell_div), - risk_multiplier(risk_multiplier){ + risk_multiplier(risk_multiplier) { cell_width = region_width / (double) num_horizontal_cell_div; cell_height = region_height / (double) num_vertical_cell_div; total_cells = num_horizontal_cell_div * num_vertical_cell_div; @@ -33,8 +33,7 @@ RiskAnalysis::assessPointCloudRisk(pcl::PCLPointCloud2 point_cloud) { mapping_msgs_urc::RiskAreaArray risk_areas; // Initialise regions with an area and associated points - std::vector> regions = - initialisePointRegions(); + std::vector> regions = initialisePointRegions(); // Convert to PCLPointCloud pcl::PointCloud::Ptr pcl( @@ -75,11 +74,11 @@ RiskAnalysis::initialisePointRegions() { void RiskAnalysis::fillPointRegions( pcl::PointCloud::Ptr pcl, std::vector>& regions) { - // Add all valid points to their respective regions for (size_t i = 0; i < pcl->size(); i++) { // Has to be a valid point - if (!isnan(pcl->points[i].x) && pcl->points[i].x > 0 && pcl->points[i].x < region_height) { + if (!isnan(pcl->points[i].x) && pcl->points[i].x > 0 && + pcl->points[i].x < region_height) { // Convert PCLPointXYZ to geometry_msgs::Point geometry_msgs::Point cur_point; cur_point.x = pcl->points[i].x; @@ -92,7 +91,7 @@ std::vector>& regions) { // Only add in the point if it's in a valid region. bool validColumn = col >= 0 && col < regions[0].size(); - bool validRow = row >= 0 && row < regions.size(); + bool validRow = row >= 0 && row < regions.size(); if (validColumn && validRow) { regions[row][col].points.push_back(cur_point); } @@ -114,7 +113,8 @@ std::vector> regions) { // Determine risk of the area std_msgs::Float64 risk; - risk.data = risk_multiplier*calculateStandardDeviation(z_values); + risk.data = + risk_multiplier * calculateStandardDeviation(z_values); // Cap the risk score to RISK_MAX risk.data = std::min(risk.data, MAX_RISK); @@ -158,9 +158,11 @@ sb_geom_msgs::Polygon2D RiskAnalysis::getRegionAreaFromIndices(int row, // Height corresponds to the x-axis, width corresponds to the y-axis // +X = UP, +Y = LEFT (ROS Coordinates) - // Left most region has a column index of 0 and increases as it moves along the y-axis in the negative direction + // Left most region has a column index of 0 and increases as it moves along + // the y-axis in the negative direction - // Top most region has a row index of 0 and increases as you moves along the x-axis in the negative direction + // Top most region has a row index of 0 and increases as you moves along the + // x-axis in the negative direction sb_geom_msgs::Point2D top_left_point; top_left_point.x = region_height - row * cell_height; diff --git a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp index 266e5487f..30cad5144 100644 --- a/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp +++ b/src/sb_pointcloud_processing/src/RiskAnalysisNode.cpp @@ -45,7 +45,8 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, private_nh, "region_min_points", region_min_points, default_min_points); float default_risk_multiplier = 1; - SB_getParam(private_nh, "risk_multiplier", risk_multiplier, default_risk_multiplier); + SB_getParam( + private_nh, "risk_multiplier", risk_multiplier, default_risk_multiplier); // In the beginning we have not published any markers seq_count = 0; @@ -86,8 +87,7 @@ RiskAnalysisNode::RiskAnalysisNode(int argc, num_vertical_cell_div, num_horizontal_cell_div, region_min_points, - risk_multiplier - ); + risk_multiplier); } void RiskAnalysisNode::pclCallBack( @@ -116,8 +116,7 @@ const sensor_msgs::PointCloud2ConstPtr point_cloud) { frame_id, ns, i, - visualization_msgs::Marker::LINE_STRIP - ); + visualization_msgs::Marker::LINE_STRIP); risk_area_markers.markers.push_back(risk_area_marker); } @@ -141,5 +140,5 @@ mapping_msgs_urc::RiskAreaArray risk_areas) { visualization_msgs::Marker::_color_type RiskAnalysisNode::convertRiskToColor(float risk) { // Round down to the nearest integer to get risk color - return gradient[(int) risk*10.0]; + return gradient[(int) risk * 10.0]; } diff --git a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp index 451b8c91e..5d45751b1 100644 --- a/src/sb_pointcloud_processing/test/risk-analysis-test.cpp +++ b/src/sb_pointcloud_processing/test/risk-analysis-test.cpp @@ -13,7 +13,8 @@ pcl::PCLPointCloud2 empty_cloud = pcl::PCLPointCloud2(); /** - * Runs tests to verify the inputted point_region has the proper dimensions specified + * Runs tests to verify the inputted point_region has the proper dimensions + * specified * by all the other inputs * * @param point_region @@ -28,7 +29,6 @@ void testRegionDimensions(std::vector> point_region, int num_vertical_cell_divs, int num_horizontal_cell_divs); - /** * * @param risk_analysis @@ -45,21 +45,22 @@ void testRegionLocations(RiskAnalysis risk_analysis, /** * Runs tests to determine equality of points */ -void EXPECT_POINT_EQ(sb_geom_msgs::Point2D expected_val, sb_geom_msgs::Point2D actual_val) { +void EXPECT_POINT_EQ(sb_geom_msgs::Point2D expected_val, + sb_geom_msgs::Point2D actual_val) { float abs_error = 0.0001; EXPECT_NEAR(expected_val.x, actual_val.x, abs_error); EXPECT_NEAR(expected_val.y, actual_val.y, abs_error); } - - /** - * Creates a point with specified values - */ + +/** + * Creates a point with specified values + */ sb_geom_msgs::Point2D createPoint(float x, float y) { sb_geom_msgs::Point2D point; point.x = x; point.y = y; - + return point; } @@ -69,7 +70,7 @@ TEST(RegionInitialisation, DivisionsEqualToDimensions) { int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; int min_points_in_region = 1; - float risk_multiplier = 1; + float risk_multiplier = 1; RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, @@ -90,7 +91,11 @@ TEST(RegionInitialisation, DivisionsEqualToDimensions) { num_vertical_cell_divs, num_horizontal_cell_divs); - testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); + testRegionLocations(risk_analysis, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); } TEST(RegionAnalysis, DivisionsEqualToDimensions) { @@ -99,9 +104,9 @@ TEST(RegionAnalysis, DivisionsEqualToDimensions) { int num_vertical_cell_divs = 10; int num_horizontal_cell_divs = 10; int min_points_in_region = 1; - float risk_multiplier = 1; + float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, @@ -118,9 +123,9 @@ TEST(RegionInitialisation, NonIntegerCellDimensions) { int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; int min_points_in_region = 1; - float risk_multiplier = 1; + float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, @@ -140,8 +145,11 @@ TEST(RegionInitialisation, NonIntegerCellDimensions) { num_vertical_cell_divs, num_horizontal_cell_divs); - testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); - + testRegionLocations(risk_analysis, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); } TEST(RegionAnalysis, NonIntegerCellDimensions) { @@ -150,9 +158,9 @@ TEST(RegionAnalysis, NonIntegerCellDimensions) { int num_vertical_cell_divs = 100; int num_horizontal_cell_divs = 100; int min_points_in_region = 1; - float risk_multiplier = 1; + float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, + RiskAnalysis risk_analysis = RiskAnalysis(region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs, @@ -169,14 +177,14 @@ TEST(RegionInitialisation, OneDiv) { int num_vertical_cell_divs = 2; int num_horizontal_cell_divs = 1; int min_points_in_region = -1; - float risk_multiplier = 1; + float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, - region_height, - num_vertical_cell_divs, - num_horizontal_cell_divs, - min_points_in_region, - risk_multiplier); + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, + min_points_in_region, + risk_multiplier); std::vector> point_region; @@ -191,7 +199,11 @@ TEST(RegionInitialisation, OneDiv) { num_vertical_cell_divs, num_horizontal_cell_divs); - testRegionLocations(risk_analysis, region_width, region_height, num_vertical_cell_divs, num_horizontal_cell_divs); + testRegionLocations(risk_analysis, + region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs); } TEST(RegionAnalysis, OneDiv) { @@ -200,17 +212,17 @@ TEST(RegionAnalysis, OneDiv) { int num_vertical_cell_divs = 2; int num_horizontal_cell_divs = 1; int min_points_in_region = -1; - float risk_multiplier = 1; + float risk_multiplier = 1; - RiskAnalysis risk_analysis = RiskAnalysis(region_width, - region_height, - num_vertical_cell_divs, - num_horizontal_cell_divs, - min_points_in_region, - risk_multiplier); + RiskAnalysis risk_analysis = RiskAnalysis(region_width, + region_height, + num_vertical_cell_divs, + num_horizontal_cell_divs, + min_points_in_region, + risk_multiplier); mapping_msgs_urc::RiskAreaArray pcl_risk = - risk_analysis.assessPointCloudRisk(empty_cloud); + risk_analysis.assessPointCloudRisk(empty_cloud); } void testRegionDimensions(std::vector> point_region, @@ -227,16 +239,22 @@ void testRegionDimensions(std::vector> point_region, sb_geom_msgs::Polygon2D cur_cell = point_region[i][j].region_area; // Top Left Point vs Bottom Left Point - EXPECT_NEAR(cell_height, cur_cell.points[0].x - cur_cell.points[3].x, abs_error); + EXPECT_NEAR(cell_height, + cur_cell.points[0].x - cur_cell.points[3].x, + abs_error); // Top Right Point vs Bottom Right Point - EXPECT_NEAR(cell_height, cur_cell.points[1].x - cur_cell.points[2].x, abs_error); + EXPECT_NEAR(cell_height, + cur_cell.points[1].x - cur_cell.points[2].x, + abs_error); // Top Left Point vs Top Right Point - EXPECT_NEAR(cell_width, cur_cell.points[0].y - cur_cell.points[1].y, abs_error); + EXPECT_NEAR( + cell_width, cur_cell.points[0].y - cur_cell.points[1].y, abs_error); // Bottom Left Point vs Bottom Right Point - EXPECT_NEAR(cell_width, cur_cell.points[3].y - cur_cell.points[2].y, abs_error); + EXPECT_NEAR( + cell_width, cur_cell.points[3].y - cur_cell.points[2].y, abs_error); } } } @@ -248,19 +266,23 @@ void testRegionLocations(RiskAnalysis risk_analysis, int num_horizontal_cell_divs) { float cell_width = region_width / num_horizontal_cell_divs; float cell_height = region_height / num_vertical_cell_divs; - + float cur_cell_top = region_height; for (int i = 0; i < num_vertical_cell_divs; i++) { float cur_cell_left = region_width / 2; - + for (int j = 0; j < num_horizontal_cell_divs; j++) { sb_geom_msgs::Polygon2D cur_region = - risk_analysis.getRegionAreaFromIndices(i, j); - - sb_geom_msgs::Point2D top_left_point = createPoint(cur_cell_top, cur_cell_left); - sb_geom_msgs::Point2D top_right_point = createPoint(cur_cell_top, cur_cell_left - cell_width); - sb_geom_msgs::Point2D bottom_right_point = createPoint(cur_cell_top - cell_height, cur_cell_left - cell_width); - sb_geom_msgs::Point2D bottom_left_point = createPoint(cur_cell_top - cell_height, cur_cell_left); + risk_analysis.getRegionAreaFromIndices(i, j); + + sb_geom_msgs::Point2D top_left_point = + createPoint(cur_cell_top, cur_cell_left); + sb_geom_msgs::Point2D top_right_point = + createPoint(cur_cell_top, cur_cell_left - cell_width); + sb_geom_msgs::Point2D bottom_right_point = + createPoint(cur_cell_top - cell_height, cur_cell_left - cell_width); + sb_geom_msgs::Point2D bottom_left_point = + createPoint(cur_cell_top - cell_height, cur_cell_left); EXPECT_POINT_EQ(top_left_point, cur_region.points[0]); EXPECT_POINT_EQ(top_right_point, cur_region.points[1]);