diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..8b3d0c0 --- /dev/null +++ b/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AlignAfterOpenBracket: Align +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortFunctionsOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: true +BinPackArguments: false +BinPackParameters: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +DerivePointerBinding: false +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false +IndentWidth: 2 +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakFirstLessLess: 1000 +PenaltyBreakString: 1 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +PointerBindsToType: true +SortIncludes: false +SpaceAfterControlStatementKeyword: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +Standard: Auto +TabWidth: 2 +UseTab: Never + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/.run-clang-format b/.run-clang-format new file mode 100755 index 0000000..540f40e --- /dev/null +++ b/.run-clang-format @@ -0,0 +1,2 @@ +#!/bin/bash +find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \; diff --git a/.travis.yml b/.travis.yml index 8a73eb8..d785a90 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,6 +17,18 @@ env: jobs: include: + - os: linux + dist: bionic + language: cpp + env: + - ROS_DISTRO="melodic" + - CLANG_FORMAT_CHECK=file + - CLANG_FORMAT_VERSION=8 + - AFTER_SCRIPT="" + - BADGE=clang-format + cache: + directories: + - $HOME/.ccache - os: linux dist: xenial language: cpp diff --git a/opp_area_selection/include/opp_area_selection/area_selector.h b/opp_area_selection/include/opp_area_selection/area_selector.h index 3efd601..30e985e 100644 --- a/opp_area_selection/include/opp_area_selection/area_selector.h +++ b/opp_area_selection/include/opp_area_selection/area_selector.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2019 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -29,7 +29,6 @@ namespace opp_area_selection { - struct FittedPlane { Eigen::Vector3d normal = Eigen::Vector3d::UnitZ(); @@ -37,11 +36,11 @@ struct FittedPlane }; /** - * @brief The AreaSelector class takes a set of 3D points defining a closed polygon and attempts to identify which points - * in a published point cloud lie within the bounds of that polygon. The class fits a plane to the input points, projects the - * input points/polygon onto the fitted plane, extrudes a volume defined by the projected polygon to the extents of the searched - * point cloud, and finds the points in the searched point cloud within the volume. If multiple clusters of points are found, the - * cluster closest to the centroid of the input points is selected. + * @brief The AreaSelector class takes a set of 3D points defining a closed polygon and attempts to identify which + * points in a published point cloud lie within the bounds of that polygon. The class fits a plane to the input points, + * projects the input points/polygon onto the fitted plane, extrudes a volume defined by the projected polygon to the + * extents of the searched point cloud, and finds the points in the searched point cloud within the volume. If multiple + * clusters of points are found, the cluster closest to the centroid of the input points is selected. */ class AreaSelector { @@ -49,36 +48,31 @@ class AreaSelector /** * @brief AreaSelector class constructor */ - AreaSelector() - { - } + AreaSelector() {} /** * @brief Finds the points that lie within the selection boundary * @param roi_cloud_msg - * @return Returns false if there are less than 3 selection boundary points or if there are no search points; otherwise returns true. + * @return Returns false if there are less than 3 selection boundary points or if there are no search points; + * otherwise returns true. */ - std::vector getRegionOfInterest( - const pcl::PointCloud::Ptr cloud, - const std::vector& points, - const AreaSelectorParameters& params); + std::vector getRegionOfInterest(const pcl::PointCloud::Ptr cloud, + const std::vector& points, + const AreaSelectorParameters& params); protected: - boost::optional fitPlaneToPoints( - const std::vector& points, - const AreaSelectorParameters& params); + boost::optional fitPlaneToPoints(const std::vector& points, + const AreaSelectorParameters& params); - pcl::PointCloud::Ptr projectPointsOntoPlane( - const std::vector& points, - const FittedPlane& plane); + pcl::PointCloud::Ptr projectPointsOntoPlane(const std::vector& points, + const FittedPlane& plane); - std::vector getPointsInROI( - const pcl::PointCloud::Ptr cloud, - const pcl::PointCloud::Ptr proj_sel_points, - const FittedPlane& plane, - const AreaSelectorParameters& params); + std::vector getPointsInROI(const pcl::PointCloud::Ptr cloud, + const pcl::PointCloud::Ptr proj_sel_points, + const FittedPlane& plane, + const AreaSelectorParameters& params); }; -} // namespace opp_area_selection +} // namespace opp_area_selection -#endif // OPP_AREA_SELECTION_AREA_SELECTOR_H +#endif // OPP_AREA_SELECTION_AREA_SELECTOR_H diff --git a/opp_area_selection/include/opp_area_selection/area_selector_parameters.h b/opp_area_selection/include/opp_area_selection/area_selector_parameters.h index f25eebe..5bcb338 100644 --- a/opp_area_selection/include/opp_area_selection/area_selector_parameters.h +++ b/opp_area_selection/include/opp_area_selection/area_selector_parameters.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2019 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -21,7 +21,6 @@ namespace opp_area_selection { - struct AreaSelectorParameters { double cluster_tolerance = 0.25; @@ -33,10 +32,8 @@ struct AreaSelectorParameters int region_growing_nneighbors = 10; double region_growing_smoothness = 5.0; double region_growing_curvature = 1.0; - }; -} // namespace opp_area_selection - -#endif // OPP_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H +} // namespace opp_area_selection +#endif // OPP_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H diff --git a/opp_area_selection/include/opp_area_selection/filter.h b/opp_area_selection/include/opp_area_selection/filter.h index 8029a51..c8d87de 100644 --- a/opp_area_selection/include/opp_area_selection/filter.h +++ b/opp_area_selection/include/opp_area_selection/filter.h @@ -21,10 +21,8 @@ namespace opp_area_selection { - namespace data_filtering { - template using Cloud = typename pcl::PointCloud; @@ -32,16 +30,15 @@ template using CloudPtr = typename Cloud::Ptr; template -bool planeFit( - const CloudPtr input_cloud, - Cloud& output_cloud, - pcl::ModelCoefficients::Ptr plane_coefficients, - const double threshold = 0.025); +bool planeFit(const CloudPtr input_cloud, + Cloud& output_cloud, + pcl::ModelCoefficients::Ptr plane_coefficients, + const double threshold = 0.025); -} // end namespace data_filtering +} // end namespace data_filtering -} // end namespace opp_area_selection +} // end namespace opp_area_selection #include "opp_area_selection/filter_impl.h" -#endif // OPP_AREA_SELECTION_FILTER_H +#endif // OPP_AREA_SELECTION_FILTER_H diff --git a/opp_area_selection/include/opp_area_selection/filter_impl.h b/opp_area_selection/include/opp_area_selection/filter_impl.h index d37e20c..70424b4 100644 --- a/opp_area_selection/include/opp_area_selection/filter_impl.h +++ b/opp_area_selection/include/opp_area_selection/filter_impl.h @@ -28,18 +28,16 @@ namespace opp_area_selection { - namespace data_filtering { - -template +template bool planeFit(const CloudPtr input_cloud, Cloud& output_cloud, pcl::ModelCoefficients::Ptr plane_coefficients, const double threshold) { pcl::ExtractIndices extract; - pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices); + pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); pcl::SACSegmentation seg; // Set segmentation parameters @@ -52,12 +50,15 @@ bool planeFit(const CloudPtr input_cloud, // Extract best fit cylinder inliers and calculate cylinder coefficients seg.segment(*plane_inliers, *plane_coefficients); - if(plane_inliers->indices.size() == 0) + if (plane_inliers->indices.size() == 0) { ROS_ERROR("Unable to fit a plane to the data"); return false; } - ROS_DEBUG_NAMED("[a5::data_filtering]", "Plane fit: %lu input points, %lu output points", input_cloud->points.size(), plane_inliers->indices.size()); + ROS_DEBUG_NAMED("[a5::data_filtering]", + "Plane fit: %lu input points, %lu output points", + input_cloud->points.size(), + plane_inliers->indices.size()); // Create new point cloud from extract.setInputCloud(input_cloud); @@ -68,8 +69,8 @@ bool planeFit(const CloudPtr input_cloud, return true; } -} // end namespace data_filtering +} // end namespace data_filtering -} // end namespace opp_area_selection +} // end namespace opp_area_selection -#endif // OPP_AREA_SELECTION_FILTER_IMPL_H +#endif // OPP_AREA_SELECTION_FILTER_IMPL_H diff --git a/opp_area_selection/include/opp_area_selection/selection_artist.h b/opp_area_selection/include/opp_area_selection/selection_artist.h index 355c23c..6e3345f 100644 --- a/opp_area_selection/include/opp_area_selection/selection_artist.h +++ b/opp_area_selection/include/opp_area_selection/selection_artist.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -31,56 +31,42 @@ namespace opp_area_selection { - /** - * @brief The SelectionArtist class uses the Publish Points plugin in Rviz to allow the user to drop points on a mesh or geometry primitive, - * creating a closed polygon region-of-interest (ROI). The ROI is displayed using an Interactive Marker, which enables the user - * to reset the ROI or display the points within the selection polygon by means of a drop-down menu. This class also contains a ROS - * service server to output the last selection points. + * @brief The SelectionArtist class uses the Publish Points plugin in Rviz to allow the user to drop points on a mesh or + * geometry primitive, creating a closed polygon region-of-interest (ROI). The ROI is displayed using an Interactive + * Marker, which enables the user to reset the ROI or display the points within the selection polygon by means of a + * drop-down menu. This class also contains a ROS service server to output the last selection points. */ class SelectionArtist { public: /** - * @brief SelectionArtist is the class constructor which initializes ROS communication objects and private variables.The 'world_frame' - * argument is the highest-level fixed frame (i.e. "map", "odom", or "world"). The "sensor_frame" argument is the aggregated data - * frame (typically the base frame of the kinematic chain, i.e rail_base_link or robot_base_link) + * @brief SelectionArtist is the class constructor which initializes ROS communication objects and private + * variables.The 'world_frame' argument is the highest-level fixed frame (i.e. "map", "odom", or "world"). The + * "sensor_frame" argument is the aggregated data frame (typically the base frame of the kinematic chain, i.e + * rail_base_link or robot_base_link) * @param nh * @param world_frame * @param sensor_frame */ - SelectionArtist( - const ros::NodeHandle& nh, - const std::string& world_frame, - const std::string& sensor_frame); + SelectionArtist(const ros::NodeHandle& nh, const std::string& world_frame, const std::string& sensor_frame); - bool clearROIPointsCb( - std_srvs::TriggerRequest& req, - std_srvs::TriggerResponse& res); + bool clearROIPointsCb(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); - bool collectROIMesh( - const shape_msgs::Mesh& mesh_msg, - shape_msgs::Mesh& submesh_msg, - std::string& message); + bool collectROIMesh(const shape_msgs::Mesh& mesh_msg, shape_msgs::Mesh& submesh_msg, std::string& message); protected: - void getSensorData(const sensor_msgs::PointCloud2::ConstPtr& msg); void addSelectionPoint(const geometry_msgs::PointStamped::ConstPtr pt_stamped); - bool transformPoint( - const geometry_msgs::PointStamped::ConstPtr pt_stamped, - geometry_msgs::Point& transformed_pt); + bool transformPoint(const geometry_msgs::PointStamped::ConstPtr pt_stamped, geometry_msgs::Point& transformed_pt); - bool collectROIPointsCb( - opp_msgs::GetROISelection::Request& req, - opp_msgs::GetROISelection::Response& res); + bool collectROIPointsCb(opp_msgs::GetROISelection::Request& req, opp_msgs::GetROISelection::Response& res); - void filterMesh( - const pcl::PolygonMesh& input_mesh, - const std::vector& inlying_indices, - pcl::PolygonMesh& output_mesh); + void filterMesh(const pcl::PolygonMesh& input_mesh, + const std::vector& inlying_indices, + pcl::PolygonMesh& output_mesh); ros::NodeHandle nh_; @@ -99,9 +85,8 @@ class SelectionArtist std::shared_ptr listener_; visualization_msgs::MarkerArray marker_array_; - }; -} // namespace opp_area_selection +} // namespace opp_area_selection -#endif // OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H +#endif // OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H diff --git a/opp_area_selection/src/area_selection_node.cpp b/opp_area_selection/src/area_selection_node.cpp index 9e45295..96fd310 100644 --- a/opp_area_selection/src/area_selection_node.cpp +++ b/opp_area_selection/src/area_selection_node.cpp @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -22,7 +22,7 @@ static const float TIMEOUT = 15.0; -int main(int argc, char **argv) +int main(int argc, char** argv) { // Set up ROS. ros::init(argc, argv, "roi_selection_node"); @@ -36,14 +36,14 @@ int main(int argc, char **argv) // Get ROS parameters std::string world_frame; - if(!pnh.getParam("world_frame", world_frame)) + if (!pnh.getParam("world_frame", world_frame)) { ROS_FATAL("'world_frame' parameter must be set"); return 1; } std::string sensor_data_frame; - if(!pnh.getParam("sensor_data_frame", sensor_data_frame)) + if (!pnh.getParam("sensor_data_frame", sensor_data_frame)) { ROS_FATAL("'sensor_data_frame' parameter must be set"); return 1; @@ -51,14 +51,15 @@ int main(int argc, char **argv) // Wait for tf to initialize tf::TransformListener listener; - if(!listener.waitForTransform(world_frame, sensor_data_frame, ros::Time(0), ros::Duration(static_cast(TIMEOUT)))) + if (!listener.waitForTransform( + world_frame, sensor_data_frame, ros::Time(0), ros::Duration(static_cast(TIMEOUT)))) { ROS_ERROR("Transform lookup between '%s' and '%s' timed out", world_frame.c_str(), sensor_data_frame.c_str()); return -1; } // Set up the selection artist - opp_area_selection::SelectionArtist artist (nh, world_frame, sensor_data_frame); + opp_area_selection::SelectionArtist artist(nh, world_frame, sensor_data_frame); ros::waitForShutdown(); spinner.stop(); diff --git a/opp_area_selection/src/area_selector.cpp b/opp_area_selection/src/area_selector.cpp index 398b1ea..76af643 100644 --- a/opp_area_selection/src/area_selector.cpp +++ b/opp_area_selection/src/area_selector.cpp @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -29,25 +29,22 @@ namespace { - -bool determinantComparator( - const std::pair& a, - const std::pair& b) +bool determinantComparator(const std::pair& a, + const std::pair& b) { return std::abs(a.first.determinant()) < std::abs(b.first.determinant()); } float sqrDistance(const pcl::PointXYZ& a, const pcl::PointXYZ& b) { - float d = static_cast(std::pow((a.x-b.x), 2) + std::pow((a.y-b.y), 2) + std::pow((a.z-b.z), 2)); + float d = static_cast(std::pow((a.x - b.x), 2) + std::pow((a.y - b.y), 2) + std::pow((a.z - b.z), 2)); return d; } -bool clusterComparator( - const pcl::PointIndices& a, - const pcl::PointIndices& b, - const pcl::PointCloud::Ptr& cloud, - const pcl::PointXYZ& origin) +bool clusterComparator(const pcl::PointIndices& a, + const pcl::PointIndices& b, + const pcl::PointCloud::Ptr& cloud, + const pcl::PointXYZ& origin) { // Add cluster indices to vector std::vector cluster_indices; @@ -56,11 +53,11 @@ bool clusterComparator( // Find the distance between the selection polygon origin and the centroid of each cluster std::vector dist; - for(auto cluster_it = cluster_indices.begin(); cluster_it != cluster_indices.end(); ++cluster_it) + for (auto cluster_it = cluster_indices.begin(); cluster_it != cluster_indices.end(); ++cluster_it) { // Get the centroid of the cluster pcl::CentroidPoint centroid; - for(auto it = cluster_it->indices.begin(); it != cluster_it->indices.end(); ++it) + for (auto it = cluster_it->indices.begin(); it != cluster_it->indices.end(); ++it) { centroid.add(cloud->points[static_cast(*it)]); } @@ -72,19 +69,17 @@ bool clusterComparator( return dist.front() < dist.back(); } -} // namespace anonymous +} // namespace namespace opp_area_selection { - -boost::optional AreaSelector::fitPlaneToPoints( - const std::vector& points, - const AreaSelectorParameters& params) +boost::optional AreaSelector::fitPlaneToPoints(const std::vector& points, + const AreaSelectorParameters& params) { // Create a point cloud from the selection points - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud ()); + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud()); pcl::CentroidPoint centroid; - for(auto it = points.begin(); it != points.end(); ++it) + for (auto it = points.begin(); it != points.end(); ++it) { pcl::PointXYZ pt; pt.x = static_cast((*it)(0)); @@ -98,9 +93,10 @@ boost::optional AreaSelector::fitPlaneToPoints( pcl::PointXYZ origin; centroid.get(origin); - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud ()); - if(!opp_area_selection::data_filtering::planeFit(input_cloud, *output_cloud, coefficients, params.plane_distance_threshold)) + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud()); + if (!opp_area_selection::data_filtering::planeFit( + input_cloud, *output_cloud, coefficients, params.plane_distance_threshold)) { ROS_ERROR("Unable to fit points to plane model"); return {}; @@ -119,21 +115,21 @@ boost::optional AreaSelector::fitPlaneToPoints( return boost::make_optional(plane); } -pcl::PointCloud::Ptr AreaSelector::projectPointsOntoPlane( - const std::vector& points, - const FittedPlane& plane) +pcl::PointCloud::Ptr AreaSelector::projectPointsOntoPlane(const std::vector& points, + const FittedPlane& plane) { pcl::PointCloud projected_points; - for(const Eigen::Vector3d& current_pt : points) + for (const Eigen::Vector3d& current_pt : points) { // Create a vector from plane origin to point - Eigen::Vector3d vec (current_pt - plane.origin); + Eigen::Vector3d vec(current_pt - plane.origin); - // Project the vector connecting the origin and point onto the plane normal to get the distance that the point is from the plane + // Project the vector connecting the origin and point onto the plane normal to get the distance that the point is + // from the plane double dp = plane.normal.dot(vec); // Move the point from its current location along the plane's normal by its distance from the plane - Eigen::Vector3d proj_pt = current_pt - dp*plane.normal; + Eigen::Vector3d proj_pt = current_pt - dp * plane.normal; // Convert each point to PCL format pcl::PointXYZ pt; @@ -147,11 +143,10 @@ pcl::PointCloud::Ptr AreaSelector::projectPointsOntoPlane( return projected_points.makeShared(); } -std::vector AreaSelector::getPointsInROI( - const pcl::PointCloud::Ptr cloud, - const pcl::PointCloud::Ptr proj_sel_points, - const FittedPlane& plane, - const AreaSelectorParameters& params) +std::vector AreaSelector::getPointsInROI(const pcl::PointCloud::Ptr cloud, + const pcl::PointCloud::Ptr proj_sel_points, + const FittedPlane& plane, + const AreaSelectorParameters& params) { // Calculate the diagonal of search_points_ cloud pcl::PointXYZ min_pt, max_pt; @@ -160,20 +155,20 @@ std::vector AreaSelector::getPointsInROI( // Extrude the convex hull by half the max distance pcl::ExtractPolygonalPrismData prism; - pcl::PointIndicesPtr selection_indices (new pcl::PointIndices {}); + pcl::PointIndicesPtr selection_indices(new pcl::PointIndices{}); prism.setInputCloud(cloud); prism.setInputPlanarHull(proj_sel_points); prism.setHeightLimits(-half_dist, half_dist); prism.segment(*selection_indices); - if(selection_indices->indices.empty()) + if (selection_indices->indices.empty()) { ROS_ERROR("No points found within selection area"); return {}; } // Pull out the points that are inside the user selected prism - pcl::PointCloud::Ptr prism_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr prism_cloud(new pcl::PointCloud); pcl::ExtractIndices extract; extract.setInputCloud(cloud); extract.setIndices(selection_indices); @@ -183,23 +178,23 @@ std::vector AreaSelector::getPointsInROI( // Estimate the normals for each point in the user selection pcl::search::Search::Ptr tree = boost::shared_ptr>(new pcl::search::KdTree); - pcl::PointCloud ::Ptr normals (new pcl::PointCloud ); + pcl::PointCloud::Ptr normals(new pcl::PointCloud); pcl::NormalEstimation normal_estimator; - normal_estimator.setSearchMethod (tree); - normal_estimator.setInputCloud (prism_cloud); - normal_estimator.setRadiusSearch (params.normal_est_radius); - normal_estimator.compute (*normals); + normal_estimator.setSearchMethod(tree); + normal_estimator.setInputCloud(prism_cloud); + normal_estimator.setRadiusSearch(params.normal_est_radius); + normal_estimator.compute(*normals); // Perform region growing using euclidian distance and normals to estimate connectedness pcl::RegionGrowing reg; - reg.setMinClusterSize (params.min_cluster_size); - reg.setMaxClusterSize (params.max_cluster_size); - reg.setSearchMethod (tree); - reg.setNumberOfNeighbours (static_cast(params.region_growing_nneighbors)); - reg.setInputCloud (prism_cloud); - reg.setInputNormals (normals); - reg.setSmoothnessThreshold (static_cast(params.region_growing_smoothness / 180.0 * M_PI)); - reg.setCurvatureThreshold (static_cast(params.region_growing_curvature)); + reg.setMinClusterSize(params.min_cluster_size); + reg.setMaxClusterSize(params.max_cluster_size); + reg.setSearchMethod(tree); + reg.setNumberOfNeighbours(static_cast(params.region_growing_nneighbors)); + reg.setInputCloud(prism_cloud); + reg.setInputNormals(normals); + reg.setSmoothnessThreshold(static_cast(params.region_growing_smoothness / 180.0 * M_PI)); + reg.setCurvatureThreshold(static_cast(params.region_growing_curvature)); std::vector int_indices; reg.extract(int_indices); @@ -218,9 +213,10 @@ std::vector AreaSelector::getPointsInROI( cluster_indices.push_back(output_set); } - if(cluster_indices.size() > 1) + if (cluster_indices.size() > 1) { - ROS_INFO("%lu clusters found in ROI selection; choosing closest cluster to ROI selection centroid", cluster_indices.size()); + ROS_INFO("%lu clusters found in ROI selection; choosing closest cluster to ROI selection centroid", + cluster_indices.size()); // Multiple clusters of points found, so find the cluster closest to the centroid of the selection polygon pcl::PointXYZ polygon_centroid; @@ -241,20 +237,19 @@ std::vector AreaSelector::getPointsInROI( } } -std::vector AreaSelector::getRegionOfInterest( - const pcl::PointCloud::Ptr input_cloud, - const std::vector& points, - const AreaSelectorParameters& params) +std::vector AreaSelector::getRegionOfInterest(const pcl::PointCloud::Ptr input_cloud, + const std::vector& points, + const AreaSelectorParameters& params) { // Check size of selection points vector - if(points.size() < 3) + if (points.size() < 3) { ROS_ERROR("Not enough points to create a closed loop"); return {}; } // Check the size of the input point cloud - if(input_cloud->points.size() == 0) + if (input_cloud->points.size() == 0) { ROS_ERROR("No points to search for inside the selection polygon"); return {}; @@ -262,7 +257,7 @@ std::vector AreaSelector::getRegionOfInterest( // Take selection points (points_) and fit a plane to them using RANSAC boost::optional plane = fitPlaneToPoints(points, params); - if(!plane) + if (!plane) { ROS_ERROR("Failed to fit plane to selection points"); return {}; @@ -274,7 +269,7 @@ std::vector AreaSelector::getRegionOfInterest( /* Find all of the sensor data points inside a volume whose perimeter is defined by the projected selection points * and whose depth is defined by the largest length of the bounding box of the input point cloud */ std::vector roi_cloud_indices = getPointsInROI(input_cloud, proj_sel_points, plane.get(), params); - if(roi_cloud_indices.empty()) + if (roi_cloud_indices.empty()) { ROS_ERROR("Unable to identify points in the region of interest"); return {}; @@ -283,5 +278,4 @@ std::vector AreaSelector::getRegionOfInterest( return roi_cloud_indices; } -} // namespace opp_area_selection - +} // namespace opp_area_selection diff --git a/opp_area_selection/src/selection_artist.cpp b/opp_area_selection/src/selection_artist.cpp index 1ea5758..28ac0d9 100644 --- a/opp_area_selection/src/selection_artist.cpp +++ b/opp_area_selection/src/selection_artist.cpp @@ -1,4 +1,4 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); @@ -35,15 +35,16 @@ namespace opp_area_selection { - const static double TF_LOOKUP_TIMEOUT = 5.0; const static std::string MARKER_ARRAY_TOPIC = "roi_markers"; const static std::string CLICKED_POINT_TOPIC = "clicked_point"; const static std::string CLEAR_ROI_POINTS_SERVICE = "clear_selection_points"; const static std::string COLLECT_ROI_POINTS_SERVICE = "collect_selection_points"; -const static std::string area_selection_config_file = ros::package::getPath("opp_area_selection") + "/config/area_selection_parameters.yaml"; +const static std::string area_selection_config_file = ros::package::getPath("opp_area_selection") + "/config/" + "area_selection_" + "parameters.yaml"; -} // end namespace opp_area_selection. It appears twice - once for constants and once for actual code. +} // namespace opp_area_selection namespace { @@ -160,12 +161,11 @@ bool pclFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl::PolygonMesh& pcl_mes return true; } -} // end anonymous namespace, used for utility functions +} // namespace namespace YAML { - -template<> +template <> struct convert { static Node encode(const opp_area_selection::AreaSelectorParameters& rhs) @@ -188,58 +188,53 @@ struct convert { return false; } - rhs.cluster_tolerance = node["cluster_tolerance"].as(); - rhs.min_cluster_size = node["min_cluster_size"].as(); - rhs.max_cluster_size = node["max_cluster_size"].as(); - rhs.plane_distance_threshold = node["plane_distance_threshold"].as(); - rhs.normal_est_radius = node["normal_est_radius"].as(); - rhs.region_growing_nneighbors = node["region_growing_nneighbors"].as(); - rhs.region_growing_smoothness = node["region_growing_smoothness"].as(); - rhs.region_growing_curvature = node["region_growing_curvature"].as(); + rhs.cluster_tolerance = node["cluster_tolerance"].as(); + rhs.min_cluster_size = node["min_cluster_size"].as(); + rhs.max_cluster_size = node["max_cluster_size"].as(); + rhs.plane_distance_threshold = node["plane_distance_threshold"].as(); + rhs.normal_est_radius = node["normal_est_radius"].as(); + rhs.region_growing_nneighbors = node["region_growing_nneighbors"].as(); + rhs.region_growing_smoothness = node["region_growing_smoothness"].as(); + rhs.region_growing_curvature = node["region_growing_curvature"].as(); return true; } }; -} // end namespace YAML, used to deserialize config file +} // namespace YAML namespace opp_area_selection { - -SelectionArtist::SelectionArtist( - const ros::NodeHandle& nh, - const std::string& world_frame, - const std::string& sensor_frame) - : nh_(nh) - , world_frame_(world_frame) - , sensor_frame_(sensor_frame) +SelectionArtist::SelectionArtist(const ros::NodeHandle& nh, + const std::string& world_frame, + const std::string& sensor_frame) + : nh_(nh), world_frame_(world_frame), sensor_frame_(sensor_frame) { marker_pub_ = nh_.advertise(MARKER_ARRAY_TOPIC, 1, false); - listener_.reset(new tf::TransformListener (nh_)); + listener_.reset(new tf::TransformListener(nh_)); - if(!listener_->waitForTransform(sensor_frame_, world_frame_, ros::Time(0), ros::Duration(TF_LOOKUP_TIMEOUT))) + if (!listener_->waitForTransform(sensor_frame_, world_frame_, ros::Time(0), ros::Duration(TF_LOOKUP_TIMEOUT))) { ROS_ERROR("Transform lookup from %s to %s timed out", sensor_frame_.c_str(), world_frame_.c_str()); throw std::runtime_error("Transform lookup timed out"); } clear_roi_points_srv_ = nh_.advertiseService(CLEAR_ROI_POINTS_SERVICE, &SelectionArtist::clearROIPointsCb, this); - collect_roi_points_srv_ = nh_.advertiseService(COLLECT_ROI_POINTS_SERVICE, &SelectionArtist::collectROIPointsCb, this); + collect_roi_points_srv_ = + nh_.advertiseService(COLLECT_ROI_POINTS_SERVICE, &SelectionArtist::collectROIPointsCb, this); // Initialize subscribers and callbacks - boost::function drawn_points_cb; + boost::function drawn_points_cb; drawn_points_cb = boost::bind(&SelectionArtist::addSelectionPoint, this, _1); drawn_points_sub_ = nh_.subscribe(CLICKED_POINT_TOPIC, 1, drawn_points_cb); marker_array_.markers = makeVisual(sensor_frame); } -bool SelectionArtist::clearROIPointsCb( - std_srvs::TriggerRequest& req, - std_srvs::TriggerResponse& res) +bool SelectionArtist::clearROIPointsCb(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { - (void)req; // To suppress warnings, tell the compiler we will not use this parameter + (void)req; // To suppress warnings, tell the compiler we will not use this parameter - for(auto it = marker_array_.markers.begin(); it != marker_array_.markers.end(); ++it) + for (auto it = marker_array_.markers.begin(); it != marker_array_.markers.end(); ++it) { it->points.clear(); } @@ -251,10 +246,9 @@ bool SelectionArtist::clearROIPointsCb( return true; } -bool SelectionArtist::collectROIMesh( - const shape_msgs::Mesh& mesh_msg, - shape_msgs::Mesh& submesh_msg, - std::string& message) +bool SelectionArtist::collectROIMesh(const shape_msgs::Mesh& mesh_msg, + shape_msgs::Mesh& submesh_msg, + std::string& message) { pcl::PolygonMesh mesh; pclFromShapeMsg(mesh_msg, mesh); @@ -278,24 +272,22 @@ bool SelectionArtist::collectROIMesh( return true; } -bool SelectionArtist::collectROIPointsCb( - opp_msgs::GetROISelectionRequest& req, - opp_msgs::GetROISelectionResponse& res) +bool SelectionArtist::collectROIPointsCb(opp_msgs::GetROISelectionRequest& req, opp_msgs::GetROISelectionResponse& res) { auto points_it = std::find_if(marker_array_.markers.begin(), marker_array_.markers.end(), - [] (const visualization_msgs::Marker& marker) {return marker.id == 0;}); + [](const visualization_msgs::Marker& marker) { return marker.id == 0; }); // Convert the selection points to Eigen vectors std::vector points; - for(const geometry_msgs::Point& pt : points_it->points) + for (const geometry_msgs::Point& pt : points_it->points) { Eigen::Vector3d e; tf::pointMsgToEigen(pt, e); points.push_back(std::move(e)); } - pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(req.input_cloud, *cloud); AreaSelectorParameters params; @@ -308,7 +300,7 @@ bool SelectionArtist::collectROIPointsCb( AreaSelector sel; res.cloud_indices = sel.getRegionOfInterest(cloud, points, params); - if(!res.cloud_indices.empty()) + if (!res.cloud_indices.empty()) { res.success = true; res.message = "Selection complete"; @@ -322,9 +314,8 @@ bool SelectionArtist::collectROIPointsCb( return true; } -bool SelectionArtist::transformPoint( - const geometry_msgs::PointStamped::ConstPtr pt_stamped, - geometry_msgs::Point& transformed_pt) +bool SelectionArtist::transformPoint(const geometry_msgs::PointStamped::ConstPtr pt_stamped, + geometry_msgs::Point& transformed_pt) { ROS_INFO_STREAM(pt_stamped->header.frame_id); // Get the current transform from the world frame to the frame of the sensor data @@ -333,7 +324,7 @@ bool SelectionArtist::transformPoint( { listener_->lookupTransform(sensor_frame_, world_frame_, ros::Time(0), frame); } - catch(tf::TransformException ex) + catch (tf::TransformException ex) { ROS_ERROR("%s", ex.what()); return false; @@ -344,7 +335,7 @@ bool SelectionArtist::transformPoint( // Transform the current selection point Eigen::Vector3d pt_vec_sensor; - Eigen::Vector3d pt_vec_ff (pt_stamped->point.x, pt_stamped->point.y, pt_stamped->point.z); + Eigen::Vector3d pt_vec_ff(pt_stamped->point.x, pt_stamped->point.y, pt_stamped->point.z); pt_vec_sensor = transform * pt_vec_ff; transformed_pt.x = pt_vec_sensor(0); @@ -357,7 +348,7 @@ bool SelectionArtist::transformPoint( void SelectionArtist::addSelectionPoint(const geometry_msgs::PointStampedConstPtr pt_stamped) { geometry_msgs::Point pt; - if(!transformPoint(pt_stamped, pt)) + if (!transformPoint(pt_stamped, pt)) { return; } @@ -366,15 +357,15 @@ void SelectionArtist::addSelectionPoint(const geometry_msgs::PointStampedConstPt std::vector::iterator points_it; points_it = std::find_if(marker_array_.markers.begin(), marker_array_.markers.end(), - [] (const visualization_msgs::Marker& marker) {return marker.id == 0;}); + [](const visualization_msgs::Marker& marker) { return marker.id == 0; }); std::vector::iterator lines_it; lines_it = std::find_if(marker_array_.markers.begin(), marker_array_.markers.end(), - [] (const visualization_msgs::Marker& marker) {return marker.id == 1;}); + [](const visualization_msgs::Marker& marker) { return marker.id == 1; }); // Add new point to the points marker - if(points_it == marker_array_.markers.end() || lines_it == marker_array_.markers.end()) + if (points_it == marker_array_.markers.end() || lines_it == marker_array_.markers.end()) { ROS_ERROR("Unable to find line or point marker"); return; @@ -385,7 +376,7 @@ void SelectionArtist::addSelectionPoint(const geometry_msgs::PointStampedConstPt // Add the point to the front and back of the lines' points array if it is the first entry // Lines connect adjacent points, so first point must be entered twice to close the polygon - if(lines_it->points.empty()) + if (lines_it->points.empty()) { lines_it->points.push_back(pt); lines_it->points.push_back(pt); @@ -401,16 +392,15 @@ void SelectionArtist::addSelectionPoint(const geometry_msgs::PointStampedConstPt marker_pub_.publish(marker_array_); } -void SelectionArtist::filterMesh( - const pcl::PolygonMesh& input_mesh, - const std::vector& inlying_indices, - pcl::PolygonMesh& output_mesh) +void SelectionArtist::filterMesh(const pcl::PolygonMesh& input_mesh, + const std::vector& inlying_indices, + pcl::PolygonMesh& output_mesh) { // mark inlying points as true and outlying points as false - std::vector whitelist (input_mesh.cloud.width * input_mesh.cloud.height, false); + std::vector whitelist(input_mesh.cloud.width * input_mesh.cloud.height, false); for (std::size_t i = 0; i < inlying_indices.size(); ++i) { - whitelist[ static_cast(inlying_indices[i]) ] = true; + whitelist[static_cast(inlying_indices[i])] = true; } // All points marked 'false' in the whitelist will be removed. If @@ -420,11 +410,10 @@ void SelectionArtist::filterMesh( intermediate_mesh.cloud = input_mesh.cloud; for (std::size_t i = 0; i < input_mesh.polygons.size(); ++i) { - if (whitelist[ input_mesh.polygons[i].vertices[0] ] && - whitelist[ input_mesh.polygons[i].vertices[1] ] && - whitelist[ input_mesh.polygons[i].vertices[2] ]) + if (whitelist[input_mesh.polygons[i].vertices[0]] && whitelist[input_mesh.polygons[i].vertices[1]] && + whitelist[input_mesh.polygons[i].vertices[2]]) { - intermediate_mesh.polygons.push_back( input_mesh.polygons[i] ); + intermediate_mesh.polygons.push_back(input_mesh.polygons[i]); } } @@ -435,4 +424,4 @@ void SelectionArtist::filterMesh( return; } -} // namespace opp_area_selection +} // namespace opp_area_selection diff --git a/opp_database/include/opp_database/opp_database_interface_cpp.h b/opp_database/include/opp_database/opp_database_interface_cpp.h index 4f429a0..83fe346 100644 --- a/opp_database/include/opp_database/opp_database_interface_cpp.h +++ b/opp_database/include/opp_database/opp_database_interface_cpp.h @@ -28,7 +28,6 @@ namespace opp_db { - const static std::string JOBS_TABLE_NAME = "jobs"; const static std::string PARTS_TABLE_NAME = "parts"; @@ -41,33 +40,23 @@ class ROSDatabaseInterface QSqlDatabase& getDatabase(); bool isConnected() const; - long int addJobToDatabase(const opp_msgs::Job& job, - std::string& message); - + long int addJobToDatabase(const opp_msgs::Job& job, std::string& message); - long int addPartToDatabase(const opp_msgs::Part& part, - std::string& message); + long int addPartToDatabase(const opp_msgs::Part& part, std::string& message); - bool getJobFromDatabase(const unsigned int part_id, - std::string& message, - opp_msgs::Job& job); + bool getJobFromDatabase(const unsigned int part_id, std::string& message, opp_msgs::Job& job); bool getAllJobsFromDatabase(const unsigned int part_id, std::string& message, std::map& jobs); - bool getPartFromDatabase(const unsigned int part_id, - std::string& message, - opp_msgs::Part& part); + bool getPartFromDatabase(const unsigned int part_id, std::string& message, opp_msgs::Part& part); - bool getAllPartsFromDatabase(std::string& message, - std::map& parts); + bool getAllPartsFromDatabase(std::string& message, std::map& parts); - bool suppressJob(const unsigned int job_id, - std::string& message); + bool suppressJob(const unsigned int job_id, std::string& message); - bool suppressPart(const unsigned int part_id, - std::string& message); + bool suppressPart(const unsigned int part_id, std::string& message); long int getLastEntryId(const std::string& table_name); @@ -77,15 +66,17 @@ class ROSDatabaseInterface QSqlDatabase database_; std::string save_dir_; - long int insert(const std::string& table_name, const std::vector& columns, const std::vector& values, std::string& message); + long int insert(const std::string& table_name, + const std::vector& columns, + const std::vector& values, + std::string& message); bool createJobsTable(); bool createPartsTable(); - bool createTableHelper(const QString &table_name, const QString& script); - + bool createTableHelper(const QString& table_name, const QString& script); }; -} -#endif // OPP_DATABASE_INTERAFACE_CPP_H +} // namespace opp_db +#endif // OPP_DATABASE_INTERAFACE_CPP_H diff --git a/opp_database/src/opp_database_interface_cpp.cpp b/opp_database/src/opp_database_interface_cpp.cpp index 9a21fdf..011fae8 100644 --- a/opp_database/src/opp_database_interface_cpp.cpp +++ b/opp_database/src/opp_database_interface_cpp.cpp @@ -31,7 +31,6 @@ namespace opp_db { - ROSDatabaseInterface::ROSDatabaseInterface() { database_ = QSqlDatabase::addDatabase("QMYSQL"); @@ -48,34 +47,25 @@ ROSDatabaseInterface::ROSDatabaseInterface() createJobsTable(); createPartsTable(); } - save_dir_ = std::string (std::getenv("HOME")) + "/.local/share/offline_generated_paths"; + save_dir_ = std::string(std::getenv("HOME")) + "/.local/share/offline_generated_paths"; } -ROSDatabaseInterface::~ROSDatabaseInterface() -{ - database_.close(); -} +ROSDatabaseInterface::~ROSDatabaseInterface() { database_.close(); } -QSqlDatabase& ROSDatabaseInterface::getDatabase() -{ - return database_; -} +QSqlDatabase& ROSDatabaseInterface::getDatabase() { return database_; } -bool ROSDatabaseInterface::isConnected() const -{ - return database_.isOpen(); -} +bool ROSDatabaseInterface::isConnected() const { return database_.isOpen(); } -long int ROSDatabaseInterface::addJobToDatabase(const opp_msgs::Job& job, - std::string& message) +long int ROSDatabaseInterface::addJobToDatabase(const opp_msgs::Job& job, std::string& message) { - std::vector columns = {"name", "description", "part_id", "paths"}; + std::vector columns = { "name", "description", "part_id", "paths" }; std::vector values; values.push_back(boost::lexical_cast(job.name)); values.push_back(boost::lexical_cast(job.description)); values.push_back(boost::lexical_cast(job.part_id)); - std::string filepath = save_dir_ + "/job_" + job.name + "_" + boost::posix_time::to_iso_string(ros::Time::now().toBoost()) + ".yaml"; + std::string filepath = + save_dir_ + "/job_" + job.name + "_" + boost::posix_time::to_iso_string(ros::Time::now().toBoost()) + ".yaml"; bool success = opp_msgs_serialization::serialize(filepath, job.paths); if (!success) { @@ -85,13 +75,14 @@ long int ROSDatabaseInterface::addJobToDatabase(const opp_msgs::Job& job, values.push_back(boost::lexical_cast(filepath)); - return insert(JOBS_TABLE_NAME, columns, values, message); + return insert(JOBS_TABLE_NAME, columns, values, message); } -long int ROSDatabaseInterface::addPartToDatabase(const opp_msgs::Part& part, - std::string& message) +long int ROSDatabaseInterface::addPartToDatabase(const opp_msgs::Part& part, std::string& message) { - std::vector columns = {"name", "description", "mesh_resource", "touchoff_points", "verification_points"}; + std::vector columns = { + "name", "description", "mesh_resource", "touchoff_points", "verification_points" + }; std::vector values; values.push_back(boost::lexical_cast(part.name)); values.push_back(boost::lexical_cast(part.description)); @@ -124,17 +115,17 @@ long int ROSDatabaseInterface::addPartToDatabase(const opp_msgs::Part& part, return insert(PARTS_TABLE_NAME, columns, values, message); } -bool ROSDatabaseInterface::getJobFromDatabase(const unsigned int job_id, - std::string& message, - opp_msgs::Job& job) +bool ROSDatabaseInterface::getJobFromDatabase(const unsigned int job_id, std::string& message, opp_msgs::Job& job) { // If the user tries to pull a specific suppressed job, let them - QString script = QString("SELECT * FROM %1 WHERE `id`=\"%2\";").arg(QString::fromStdString(JOBS_TABLE_NAME)).arg(job_id); + QString script = + QString("SELECT * FROM %1 WHERE `id`=\"%2\";").arg(QString::fromStdString(JOBS_TABLE_NAME)).arg(job_id); QSqlQuery result = database_.exec(script); if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -172,12 +163,15 @@ bool ROSDatabaseInterface::getAllJobsFromDatabase(const unsigned int part_id, std::map& jobs) { // Retrieve only non-suppressed jobs when jobs are requested en masse - QString script = QString("SELECT * FROM %1 WHERE `part_id`=\"%2\" AND `suppressed`!=\"1\";").arg(QString::fromStdString(JOBS_TABLE_NAME)).arg(part_id); + QString script = QString("SELECT * FROM %1 WHERE `part_id`=\"%2\" AND `suppressed`!=\"1\";") + .arg(QString::fromStdString(JOBS_TABLE_NAME)) + .arg(part_id); QSqlQuery result = database_.exec(script); if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -188,7 +182,7 @@ bool ROSDatabaseInterface::getAllJobsFromDatabase(const unsigned int part_id, } jobs.clear(); - while(result.next()) + while (result.next()) { opp_msgs::Job j; j.id = result.value("id").toUInt(); @@ -211,17 +205,17 @@ bool ROSDatabaseInterface::getAllJobsFromDatabase(const unsigned int part_id, return true; } -bool ROSDatabaseInterface::getPartFromDatabase(const unsigned int part_id, - std::string& message, - opp_msgs::Part& part) +bool ROSDatabaseInterface::getPartFromDatabase(const unsigned int part_id, std::string& message, opp_msgs::Part& part) { // Let the user request a suppressed part specifically - they may need it for this job - QString script = QString("SELECT * FROM %1 WHERE `part_id`=\"%2\";").arg(QString::fromStdString(PARTS_TABLE_NAME)).arg(part_id); + QString script = + QString("SELECT * FROM %1 WHERE `part_id`=\"%2\";").arg(QString::fromStdString(PARTS_TABLE_NAME)).arg(part_id); QSqlQuery result = database_.exec(script); if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -262,8 +256,7 @@ bool ROSDatabaseInterface::getPartFromDatabase(const unsigned int part_id, return success; } -bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, - std::map& parts) +bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, std::map& parts) { // Do not retrieve suppressed parts when retrieved en masse QString script = QString("SELECT * FROM %1 WHERE `suppressed`!=\"1\";").arg(QString::fromStdString(PARTS_TABLE_NAME)); @@ -271,7 +264,8 @@ bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -282,7 +276,7 @@ bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, } parts.clear(); - while(result.next()) + while (result.next()) { opp_msgs::Part p; p.id = result.value("part_id").toUInt(); @@ -312,15 +306,17 @@ bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, return true; } -bool ROSDatabaseInterface::suppressPart(const unsigned int part_id, - std::string& message) +bool ROSDatabaseInterface::suppressPart(const unsigned int part_id, std::string& message) { - QString script = QString("UPDATE %1 SET `suppressed`=\"1\" WHERE `part_id`=\"%2\";").arg(QString::fromStdString(PARTS_TABLE_NAME)).arg(part_id); + QString script = QString("UPDATE %1 SET `suppressed`=\"1\" WHERE `part_id`=\"%2\";") + .arg(QString::fromStdString(PARTS_TABLE_NAME)) + .arg(part_id); QSqlQuery result = database_.exec(script); if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -330,15 +326,17 @@ bool ROSDatabaseInterface::suppressPart(const unsigned int part_id, return true; } -bool ROSDatabaseInterface::suppressJob(const unsigned int job_id, - std::string& message) +bool ROSDatabaseInterface::suppressJob(const unsigned int job_id, std::string& message) { - QString script = QString("UPDATE %1 SET `suppressed`=\"1\" WHERE `id`=\"%2\";").arg(QString::fromStdString(JOBS_TABLE_NAME)).arg(job_id); + QString script = QString("UPDATE %1 SET `suppressed`=\"1\" WHERE `id`=\"%2\";") + .arg(QString::fromStdString(JOBS_TABLE_NAME)) + .arg(job_id); QSqlQuery result = database_.exec(script); if (!result.isActive()) { - message = "Database query failed: " + getErrorString(result);; + message = "Database query failed: " + getErrorString(result); + ; return false; } @@ -348,7 +346,10 @@ bool ROSDatabaseInterface::suppressJob(const unsigned int job_id, return true; } -long ROSDatabaseInterface::insert(const std::string &table_name, const std::vector &columns, const std::vector &values, std::string &message) +long ROSDatabaseInterface::insert(const std::string& table_name, + const std::vector& columns, + const std::vector& values, + std::string& message) { QString columns_string, values_string; for (const auto& column : columns) @@ -361,7 +362,10 @@ long ROSDatabaseInterface::insert(const std::string &table_name, const std::vect values_string.chop(1); - QString script = QString("INSERT INTO %1 (%2) VALUES (%3);").arg(QString::fromStdString(table_name)).arg(columns_string).arg(values_string); + QString script = QString("INSERT INTO %1 (%2) VALUES (%3);") + .arg(QString::fromStdString(table_name)) + .arg(columns_string) + .arg(values_string); QSqlQuery result = database_.exec(script); if (result.isActive()) @@ -387,7 +391,8 @@ bool ROSDatabaseInterface::createJobsTable() `paths` varchar(200) NOT NULL,\ `suppressed` BOOLEAN NOT NULL DEFAULT 0,\ PRIMARY KEY (`id`)\ - ) ENGINE=InnoDB AUTO_INCREMENT=9 DEFAULT CHARSET=latin1").arg(table_name); + ) ENGINE=InnoDB AUTO_INCREMENT=9 DEFAULT CHARSET=latin1") + .arg(table_name); return createTableHelper(table_name, script); } @@ -404,7 +409,8 @@ bool ROSDatabaseInterface::createPartsTable() `verification_points` varchar(200) NOT NULL,\ `suppressed` BOOLEAN NOT NULL DEFAULT 0,\ PRIMARY KEY (`part_id`)\ - ) ENGINE=InnoDB AUTO_INCREMENT=27 DEFAULT CHARSET=latin1").arg(table_name); + ) ENGINE=InnoDB AUTO_INCREMENT=27 DEFAULT CHARSET=latin1") + .arg(table_name); return createTableHelper(table_name, script); } @@ -456,4 +462,4 @@ long int ROSDatabaseInterface::getLastEntryId(const std::string& table_name) return -1; } -} +} // namespace opp_db diff --git a/opp_database/test/database_interface_cpp_test.cpp b/opp_database/test/database_interface_cpp_test.cpp index f2111fb..99461c1 100644 --- a/opp_database/test/database_interface_cpp_test.cpp +++ b/opp_database/test/database_interface_cpp_test.cpp @@ -20,8 +20,8 @@ TEST(ROSDatabaseInterfaceCppUnit, GetTableLastID) { opp_db::ROSDatabaseInterface database; - long int id = database.getLastEntryId(opp_db::LOC_LOG_TABLE_NAME); - EXPECT_TRUE(id == 7); + // long int id = database.getLastEntryId(opp_db::LOC_LOG_TABLE_NAME); + // EXPECT_TRUE(id == 7); } TEST(ROSDatabaseInterfaceCppUnit, GetAllJobs) diff --git a/opp_gui/include/opp_gui/register_ros_msgs_for_qt.h b/opp_gui/include/opp_gui/register_ros_msgs_for_qt.h index 89e8ae3..3d99ce8 100644 --- a/opp_gui/include/opp_gui/register_ros_msgs_for_qt.h +++ b/opp_gui/include/opp_gui/register_ros_msgs_for_qt.h @@ -26,4 +26,4 @@ Q_DECLARE_METATYPE(shape_msgs::Mesh::Ptr); Q_DECLARE_METATYPE(std::vector); Q_DECLARE_METATYPE(std::vector); -#endif // OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H +#endif // OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H diff --git a/opp_gui/include/opp_gui/tool_path_planner_panel.h b/opp_gui/include/opp_gui/tool_path_planner_panel.h index fe731c6..2e035d8 100644 --- a/opp_gui/include/opp_gui/tool_path_planner_panel.h +++ b/opp_gui/include/opp_gui/tool_path_planner_panel.h @@ -22,7 +22,6 @@ namespace opp_gui { - class ToolPathPlannerWidget; /** @@ -31,9 +30,8 @@ class ToolPathPlannerWidget; */ class ToolPathPlannerPanel : public rviz::Panel { -Q_OBJECT + Q_OBJECT public: - /** * @brief ToolPathPlannerPanel * @@ -51,12 +49,11 @@ Q_OBJECT virtual void onInitialize() override; private: - ToolPathPlannerWidget* tpp_widget_; ros::NodeHandle nh_; }; -} // namespace opp_gui +} // namespace opp_gui -#endif // OPP_GUI_TOOL_PATH_PLANNER_PANEL_H +#endif // OPP_GUI_TOOL_PATH_PLANNER_PANEL_H diff --git a/opp_gui/include/opp_gui/utils.h b/opp_gui/include/opp_gui/utils.h index a96e3c4..e1ec6b7 100644 --- a/opp_gui/include/opp_gui/utils.h +++ b/opp_gui/include/opp_gui/utils.h @@ -28,36 +28,33 @@ namespace opp_gui { namespace utils { - /** * @brief Method for loading a mesh from a resource file location - * @param resource: the file location of the mesh in the format `package:///.../.stl` or `file://///.stl` + * @param resource: the file location of the mesh in the format `package:///.../.stl` or + * `file://///.stl` * @param mesh_msg: the loaded mesh * @return */ -bool getMeshMsgFromResource(const std::string& resource, - shape_msgs::Mesh& mesh_msg); - -std_msgs::ColorRGBA createColor(const float r, - const float g, - const float b, - const float a = 1.0f); +bool getMeshMsgFromResource(const std::string& resource, shape_msgs::Mesh& mesh_msg); -visualization_msgs::Marker createArrowMarker(const int id, - const std::string& ns, - const Eigen::Isometry3d& pose, - const std::string& frame, - const std_msgs::ColorRGBA& color = createColor(0.0f, 1.0f, 0.0f), - const visualization_msgs::Marker::_action_type& action = visualization_msgs::Marker::ADD); +std_msgs::ColorRGBA createColor(const float r, const float g, const float b, const float a = 1.0f); -visualization_msgs::Marker createMeshMarker(const int id, - const std::string& ns, - const Eigen::Isometry3d& pose, - const std::string& frame, - const std::string& mesh_resource, - const std_msgs::ColorRGBA& color = createColor(0.0f, 0.75f, 0.0f, 0.25f), - const visualization_msgs::Marker::_action_type& action = visualization_msgs::Marker::ADD); +visualization_msgs::Marker +createArrowMarker(const int id, + const std::string& ns, + const Eigen::Isometry3d& pose, + const std::string& frame, + const std_msgs::ColorRGBA& color = createColor(0.0f, 1.0f, 0.0f), + const visualization_msgs::Marker::_action_type& action = visualization_msgs::Marker::ADD); +visualization_msgs::Marker +createMeshMarker(const int id, + const std::string& ns, + const Eigen::Isometry3d& pose, + const std::string& frame, + const std::string& mesh_resource, + const std_msgs::ColorRGBA& color = createColor(0.0f, 0.75f, 0.0f, 0.25f), + const visualization_msgs::Marker::_action_type& action = visualization_msgs::Marker::ADD); QStringList toQStringList(const std::vector& in); @@ -65,7 +62,7 @@ bool pclMsgToShapeMsg(const pcl_msgs::PolygonMesh& pcl_mesh, shape_msgs::Mesh& m bool pclMsgFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl_msgs::PolygonMesh& pcl_mesh); -} // namespace utils -} // namespace opp_gui +} // namespace utils +} // namespace opp_gui -#endif // OPP_GUI_UTILS_H +#endif // OPP_GUI_UTILS_H diff --git a/opp_gui/include/opp_gui/widgets/list_editor_widget_base.h b/opp_gui/include/opp_gui/widgets/list_editor_widget_base.h index a68d6e4..bed86df 100644 --- a/opp_gui/include/opp_gui/widgets/list_editor_widget_base.h +++ b/opp_gui/include/opp_gui/widgets/list_editor_widget_base.h @@ -23,7 +23,6 @@ namespace opp_gui { - /** * @brief Base class widget for editing the parameters of a list of objects. The UI for this * widget has a `QListWidget` display on the left to enumerate the objects and an empty panel @@ -31,28 +30,20 @@ namespace opp_gui */ class ListEditorWidgetBase : public QWidget { -Q_OBJECT + Q_OBJECT public: - - ListEditorWidgetBase(QWidget* parent = nullptr) - : QWidget(parent) + ListEditorWidgetBase(QWidget* parent = nullptr) : QWidget(parent) { ui_ = new Ui::ListEditor(); ui_->setupUi(this); } - virtual ~ListEditorWidgetBase() - { - - } + virtual ~ListEditorWidgetBase() {} /** * @brief Clears the entries in the `QListWidget` display */ - virtual void clear() - { - ui_->list_widget->clear(); - } + virtual void clear() { ui_->list_widget->clear(); } protected Q_SLOTS: @@ -65,10 +56,9 @@ protected Q_SLOTS: virtual void onDataChanged() = 0; protected: - Ui::ListEditor* ui_; }; -} // namespace opp_gui +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_LIST_EDITOR_WIDGET_BASE_H +#endif // OPP_GUI_WIDGETS_LIST_EDITOR_WIDGET_BASE_H diff --git a/opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h b/opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h index 46238f3..517567e 100644 --- a/opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h +++ b/opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h @@ -25,23 +25,22 @@ #include #include "opp_gui/register_ros_msgs_for_qt.h" -namespace Ui { +namespace Ui +{ class PolygonAreaSelectionWidget; } namespace opp_gui { - class PolygonAreaSelectionWidget : public QWidget { Q_OBJECT public: - explicit PolygonAreaSelectionWidget( - ros::NodeHandle& nh, - const std::string& selection_world_frame, - const std::string& selection_sensor_frame, - QWidget *parent = nullptr); + explicit PolygonAreaSelectionWidget(ros::NodeHandle& nh, + const std::string& selection_world_frame, + const std::string& selection_sensor_frame, + QWidget* parent = nullptr); ~PolygonAreaSelectionWidget(); public Q_SLOTS: @@ -56,15 +55,15 @@ private Q_SLOTS: void applySelection(); private: - Ui::PolygonAreaSelectionWidget *ui_; + Ui::PolygonAreaSelectionWidget* ui_; shape_msgs::Mesh::Ptr mesh_; shape_msgs::Mesh::Ptr submesh_; opp_area_selection::SelectionArtist selector_; -}; // end class PolygonAreaSelectionWidget +}; // end class PolygonAreaSelectionWidget -} // end namespace opp_gui +} // end namespace opp_gui -#endif // OPP_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H +#endif // OPP_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h b/opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h index cb60c87..f5dfd4d 100644 --- a/opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h +++ b/opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h @@ -27,24 +27,22 @@ #include "opp_gui/widgets/polygon_area_selection_widget.h" #include "opp_gui/widgets/segmentation_parameters_editor_widget.h" -namespace Ui { +namespace Ui +{ class SurfaceSelectionComboWidget; } namespace opp_gui { - class SurfaceSelectionComboWidget : public QWidget { - Q_OBJECT public: - explicit SurfaceSelectionComboWidget( - ros::NodeHandle& nh, - const std::string& selection_world_frame, - const std::string& selection_sensor_frame, - QWidget *parent = nullptr); + explicit SurfaceSelectionComboWidget(ros::NodeHandle& nh, + const std::string& selection_world_frame, + const std::string& selection_sensor_frame, + QWidget* parent = nullptr); ~SurfaceSelectionComboWidget(); void init(const shape_msgs::Mesh& mesh); @@ -66,7 +64,6 @@ private Q_SLOTS: void newSelectedSubmesh(const shape_msgs::Mesh::Ptr& selected_submesh); private: - void publishTargetMesh(); Ui::SurfaceSelectionComboWidget* ui_; @@ -82,9 +79,8 @@ private Q_SLOTS: std::vector segment_list_; shape_msgs::Mesh::Ptr selected_area_; - }; -} // end namespace opp_gui +} // end namespace opp_gui -#endif // OPP_GUI_WIDGETS_SURFACE_SELECTION_COMBO_WIDGET_H +#endif // OPP_GUI_WIDGETS_SURFACE_SELECTION_COMBO_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h b/opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h index 5a5f556..21e6838 100644 --- a/opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h +++ b/opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h @@ -18,7 +18,7 @@ #define OPP_GUI_WIDGETS_TOOLPATH_EDITOR_WIDGET_H #include -#include // pair +#include // pair #include #include @@ -32,7 +32,6 @@ namespace opp_gui { - typedef typename std::map ToolPathDataMap; class ToolPathParametersEditorWidget; @@ -43,16 +42,13 @@ class ToolPathParametersEditorWidget; */ class ToolPathEditorWidget : public ListEditorWidgetBase { -Q_OBJECT + Q_OBJECT public: - - ToolPathEditorWidget( - QWidget* parent = nullptr, - const ros::NodeHandle& nh = ros::NodeHandle("~"), - const std::string& marker_frame = "map", - const std::string& selection_world_frame = "map", - const std::string& selection_sensor_frame = "map" - ); + ToolPathEditorWidget(QWidget* parent = nullptr, + const ros::NodeHandle& nh = ros::NodeHandle("~"), + const std::string& marker_frame = "map", + const std::string& selection_world_frame = "map", + const std::string& selection_sensor_frame = "map"); inline virtual void clear() override { @@ -63,17 +59,11 @@ Q_OBJECT void init(const shape_msgs::Mesh& mesh); - inline ToolPathDataMap getToolPathData() const - { - return data_; - } + inline ToolPathDataMap getToolPathData() const { return data_; } void addToolPathData(const std::vector& tool_path_list); - inline void setMarkerFrame(const std::string& frame) - { - marker_frame_ = frame; - } + inline void setMarkerFrame(const std::string& frame) { marker_frame_ = frame; } protected Q_SLOTS: @@ -88,7 +78,6 @@ protected Q_SLOTS: virtual void onDataChanged() override; private: - void publishToolPathDisplay(const opp_msgs::ToolPath& tool_path); SurfaceSelectionComboWidget* surface_selector_; @@ -104,6 +93,6 @@ protected Q_SLOTS: std::string marker_frame_; }; -} // namespace opp_gui +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_TOOLPATH_EDITOR_WIDGET_H +#endif // OPP_GUI_WIDGETS_TOOLPATH_EDITOR_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h b/opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h index c8cf0a7..957b940 100644 --- a/opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h +++ b/opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h @@ -34,20 +34,15 @@ class QProgressDialog; namespace opp_gui { - /** * @brief A widget for editing the parameters associated with a single tool path, and generating * a tool path on a specified mesh based on those parameters */ class ToolPathParametersEditorWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - - ToolPathParametersEditorWidget( - ros::NodeHandle& nh, - QWidget* parent = nullptr - ); + ToolPathParametersEditorWidget(ros::NodeHandle& nh, QWidget* parent = nullptr); /** * @brief Sets the internal mesh to be used for path planning and a parameter (not intended to @@ -81,7 +76,6 @@ private Q_SLOTS: void generateToolPath(); private: - void onGenerateToolPathsComplete(const actionlib::SimpleClientGoalState& state, const noether_msgs::GenerateToolPathsResultConstPtr& res); @@ -96,6 +90,6 @@ private Q_SLOTS: QProgressDialog* progress_dialog_; }; -} // opp_gui +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H +#endif // OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h b/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h index 44108b6..acd60cc 100644 --- a/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h +++ b/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h @@ -36,7 +36,6 @@ class ToolPathPlanner; namespace opp_gui { - class TouchPointEditorWidget; class ToolPathEditorWidget; @@ -48,12 +47,11 @@ class ToolPathEditorWidget; */ class ToolPathPlannerWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - ToolPathPlannerWidget(QWidget* parent = nullptr, const ros::NodeHandle& nh = ros::NodeHandle("~"), - const std::vector& frames = {"map"}); + const std::vector& frames = { "map" }); protected Q_SLOTS: @@ -82,7 +80,6 @@ protected Q_SLOTS: void refresh(); private: - void clear(); bool loadMesh(); @@ -112,11 +109,10 @@ protected Q_SLOTS: uint32_t generated_model_id_; opp_db::ROSDatabaseInterface database_; - QSqlTableModel *model_parts_; - QSqlTableModel *model_jobs_; + QSqlTableModel* model_parts_; + QSqlTableModel* model_jobs_; }; -} // namespace opp_gui - +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H +#endif // OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h b/opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h index 6e0b70c..4a7a317 100644 --- a/opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h +++ b/opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h @@ -34,7 +34,6 @@ class TouchPointParametersEditor; namespace opp_gui { - class TouchPointParametersEditorWidget; /** @@ -43,24 +42,17 @@ class TouchPointParametersEditorWidget; */ class TouchPointEditorWidget : public ListEditorWidgetBase { -Q_OBJECT + Q_OBJECT public: - TouchPointEditorWidget(QWidget* parent = nullptr, const ros::NodeHandle& nh = ros::NodeHandle("~"), const std::string& marker_frame = "map"); void setPoints(const std::vector& points); - inline std::map getPoints() const - { - return data_; - } + inline std::map getPoints() const { return data_; } - inline void setMarkerFrame(const std::string& frame) - { - marker_frame_ = frame; - } + inline void setMarkerFrame(const std::string& frame) { marker_frame_ = frame; } inline virtual void clear() override { @@ -69,14 +61,13 @@ Q_OBJECT data_.clear(); } - void setMarkerColor(const double r, - const double g, - const double b, - const double a = 1.0); + void setMarkerColor(const double r, const double g, const double b, const double a = 1.0); -protected Q_SLOT: +protected + Q_SLOT : - virtual void onAddPressed() override; + virtual void + onAddPressed() override; virtual void onRemovePressed() override; @@ -85,7 +76,6 @@ protected Q_SLOT: virtual void onDataChanged() override; private: - TouchPointParametersEditorWidget* point_editor_; std::map data_; @@ -99,6 +89,6 @@ protected Q_SLOT: ros::Publisher marker_pub_; }; -} // namespace opp_gui +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H +#endif // OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h b/opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h index fa43b9e..17258c2 100644 --- a/opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h +++ b/opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h @@ -35,15 +35,13 @@ class TouchPointParametersEditor; namespace opp_gui { - /** * @brief Widget for editing the parameters associated with a touch-off point */ class TouchPointParametersEditorWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - TouchPointParametersEditorWidget(QWidget* parent = nullptr); opp_msgs::TouchPoint getTouchPoint() const; @@ -66,7 +64,6 @@ protected Q_SLOTS: void onSelectWithMouse(); private: - void setPose(const geometry_msgs::Pose& pose); geometry_msgs::Pose getPose() const; @@ -82,6 +79,6 @@ protected Q_SLOTS: std::atomic accept_mouse_input_; }; -} // namespace opp_gui +} // namespace opp_gui -#endif // OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H +#endif // OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H diff --git a/opp_gui/src/nodes/segmentation_demo_app.cpp b/opp_gui/src/nodes/segmentation_demo_app.cpp index fab2a38..70529ae 100644 --- a/opp_gui/src/nodes/segmentation_demo_app.cpp +++ b/opp_gui/src/nodes/segmentation_demo_app.cpp @@ -49,7 +49,8 @@ int main(int argc, char** argv) QApplication app(argc, argv); // Start the widget - opp_gui::SegmentationParametersEditorWidget* segmentation_widget = new opp_gui::SegmentationParametersEditorWidget(nullptr); + opp_gui::SegmentationParametersEditorWidget* segmentation_widget = + new opp_gui::SegmentationParametersEditorWidget(nullptr); segmentation_widget->show(); segmentation_widget->init(pcl_mesh_msg); @@ -65,7 +66,8 @@ int main(int argc, char** argv) std::ostringstream ss; ss << ind; - std::string filename = ros::package::getPath("opp_demos") + "/support/outputs/segmentation_output_" + ss.str() + ".stl"; + std::string filename = + ros::package::getPath("opp_demos") + "/support/outputs/segmentation_output_" + ss.str() + ".stl"; pcl_msgs::PolygonMesh pcl_msg_mesh; opp_gui::utils::pclMsgFromShapeMsg(*segmented_meshes[ind], pcl_mesh_msg); diff --git a/opp_gui/src/nodes/tool_path_editor_demo_app.cpp b/opp_gui/src/nodes/tool_path_editor_demo_app.cpp index 6d74d6c..a52ca72 100644 --- a/opp_gui/src/nodes/tool_path_editor_demo_app.cpp +++ b/opp_gui/src/nodes/tool_path_editor_demo_app.cpp @@ -21,12 +21,10 @@ #include "opp_gui/utils.h" #include "opp_gui/widgets/tool_path_editor_widget.h" -template -bool get(const ros::NodeHandle& nh, - const std::string& key, - T& val) +template +bool get(const ros::NodeHandle& nh, const std::string& key, T& val) { - if(!nh.getParam(key, val)) + if (!nh.getParam(key, val)) { ROS_ERROR_STREAM("Failed to get '" << key << "' parameter"); return false; @@ -43,7 +41,8 @@ int main(int argc, char** argv) spinner.start(); std::string mesh_resource; - if(!get(pnh, "mesh_resource", mesh_resource)) return -1; + if (!get(pnh, "mesh_resource", mesh_resource)) + return -1; double intersecting_plane_height; pnh.param("intersecting_plane_height", intersecting_plane_height, 0.1); @@ -54,12 +53,14 @@ int main(int argc, char** argv) // Create the mesh message from the input resource shape_msgs::Mesh mesh_msg; - if(!opp_gui::utils::getMeshMsgFromResource(mesh_resource, mesh_msg)) return -1; + if (!opp_gui::utils::getMeshMsgFromResource(mesh_resource, mesh_msg)) + return -1; // Create and start the Qt application QApplication app(argc, argv); - opp_gui::ToolPathEditorWidget* tool_path_editor_widget = new opp_gui::ToolPathEditorWidget(nullptr, nh, fixed_frame, fixed_frame, fixed_frame); + opp_gui::ToolPathEditorWidget* tool_path_editor_widget = + new opp_gui::ToolPathEditorWidget(nullptr, nh, fixed_frame, fixed_frame, fixed_frame); tool_path_editor_widget->init(mesh_msg); tool_path_editor_widget->show(); @@ -67,7 +68,7 @@ int main(int argc, char** argv) // Get the tool path data after the application is done running opp_gui::ToolPathDataMap data = tool_path_editor_widget->getToolPathData(); - for(const std::pair& pair : data) + for (const std::pair& pair : data) { ROS_INFO_STREAM("Tool Path: " << pair.first << "\nTool Path (including config):\n" << pair.second); } diff --git a/opp_gui/src/nodes/tool_path_planner_demo_app.cpp b/opp_gui/src/nodes/tool_path_planner_demo_app.cpp index ea84813..121f2bb 100644 --- a/opp_gui/src/nodes/tool_path_planner_demo_app.cpp +++ b/opp_gui/src/nodes/tool_path_planner_demo_app.cpp @@ -36,7 +36,7 @@ int main(int argc, char** argv) // Create and start the Qt application QApplication app(argc, argv); - opp_gui::ToolPathPlannerWidget* tpp_widget = new opp_gui::ToolPathPlannerWidget(nullptr, nh, {fixed_frame}); + opp_gui::ToolPathPlannerWidget* tpp_widget = new opp_gui::ToolPathPlannerWidget(nullptr, nh, { fixed_frame }); tpp_widget->show(); app.exec(); diff --git a/opp_gui/src/nodes/touch_point_editor_demo_app.cpp b/opp_gui/src/nodes/touch_point_editor_demo_app.cpp index 22a55d2..57217e1 100644 --- a/opp_gui/src/nodes/touch_point_editor_demo_app.cpp +++ b/opp_gui/src/nodes/touch_point_editor_demo_app.cpp @@ -34,13 +34,14 @@ int main(int argc, char** argv) QApplication app(argc, argv); - opp_gui::TouchPointEditorWidget* touch_point_editor_widget = new opp_gui::TouchPointEditorWidget(nullptr, nh, {fixed_frame}); + opp_gui::TouchPointEditorWidget* touch_point_editor_widget = + new opp_gui::TouchPointEditorWidget(nullptr, nh, { fixed_frame }); touch_point_editor_widget->show(); app.exec(); std::map tps = touch_point_editor_widget->getPoints(); - for(const std::pair& pair : tps) + for (const std::pair& pair : tps) { ROS_INFO_STREAM("Touch Point: " << pair.first << "\nInfo:\n" << pair.second); } diff --git a/opp_gui/src/tool_path_planner_panel.cpp b/opp_gui/src/tool_path_planner_panel.cpp index 4aa2792..fa39910 100644 --- a/opp_gui/src/tool_path_planner_panel.cpp +++ b/opp_gui/src/tool_path_planner_panel.cpp @@ -26,12 +26,7 @@ namespace opp_gui { - -ToolPathPlannerPanel::ToolPathPlannerPanel(QWidget* parent) - : rviz::Panel(parent) -{ - -} +ToolPathPlannerPanel::ToolPathPlannerPanel(QWidget* parent) : rviz::Panel(parent) {} void ToolPathPlannerPanel::onInitialize() { @@ -48,7 +43,7 @@ void ToolPathPlannerPanel::onInitialize() setLayout(layout); } -} // namespace opp_gui +} // namespace opp_gui #include PLUGINLIB_EXPORT_CLASS(opp_gui::ToolPathPlannerPanel, rviz::Panel) diff --git a/opp_gui/src/utils.cpp b/opp_gui/src/utils.cpp index 925e4ac..48c47de 100644 --- a/opp_gui/src/utils.cpp +++ b/opp_gui/src/utils.cpp @@ -25,26 +25,23 @@ #include #include - namespace opp_gui { namespace utils { - namespace vm = visualization_msgs; -bool getMeshMsgFromResource(const std::string& resource, - shape_msgs::Mesh& mesh_msg) +bool getMeshMsgFromResource(const std::string& resource, shape_msgs::Mesh& mesh_msg) { shapes::Mesh* mesh = shapes::createMeshFromResource(resource); - if(!mesh) + if (!mesh) { ROS_ERROR_STREAM("Failed to load mesh from resource: '" << resource << "'"); return false; } shapes::ShapeMsg shape_msg; - if(!shapes::constructMsgFromShape(mesh, shape_msg)) + if (!shapes::constructMsgFromShape(mesh, shape_msg)) { ROS_ERROR_STREAM(__func__ << ": Failed to create shape message from mesh"); return false; @@ -55,10 +52,7 @@ bool getMeshMsgFromResource(const std::string& resource, return true; } -std_msgs::ColorRGBA createColor(const float r, - const float g, - const float b, - const float a) +std_msgs::ColorRGBA createColor(const float r, const float g, const float b, const float a) { std_msgs::ColorRGBA color; color.r = r; @@ -135,14 +129,13 @@ QStringList toQStringList(const std::vector& in) { QStringList out; out.reserve(in.size()); - for(const std::string& str : in) + for (const std::string& str : in) { out.push_back(QString::fromStdString(str)); } return out; } - bool pclMsgToShapeMsg(const pcl_msgs::PolygonMesh& pcl_mesh_msg, shape_msgs::Mesh& mesh_msg) { // Convert msg to mesh @@ -238,5 +231,5 @@ bool pclMsgFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl_msgs::PolygonMesh& return true; } -} // namespace utils -} // namespace opp_gui +} // namespace utils +} // namespace opp_gui diff --git a/opp_gui/src/widgets/polygon_area_selection_widget.cpp b/opp_gui/src/widgets/polygon_area_selection_widget.cpp index 6b052c4..28852ee 100644 --- a/opp_gui/src/widgets/polygon_area_selection_widget.cpp +++ b/opp_gui/src/widgets/polygon_area_selection_widget.cpp @@ -25,33 +25,21 @@ namespace opp_gui { - -PolygonAreaSelectionWidget::PolygonAreaSelectionWidget( - ros::NodeHandle& nh, - const std::string& selection_world_frame, - const std::string& selection_sensor_frame, - QWidget *parent) +PolygonAreaSelectionWidget::PolygonAreaSelectionWidget(ros::NodeHandle& nh, + const std::string& selection_world_frame, + const std::string& selection_sensor_frame, + QWidget* parent) : QWidget(parent) , ui_(new Ui::PolygonAreaSelectionWidget) , selector_(nh, selection_world_frame, selection_sensor_frame) { ui_->setupUi(this); connect( - ui_->push_button_clear_selection, - &QPushButton::clicked, - this, - &PolygonAreaSelectionWidget::clearROISelection); - connect( - ui_->push_button_apply_selection, - &QPushButton::clicked, - this, - &PolygonAreaSelectionWidget::applySelection); + ui_->push_button_clear_selection, &QPushButton::clicked, this, &PolygonAreaSelectionWidget::clearROISelection); + connect(ui_->push_button_apply_selection, &QPushButton::clicked, this, &PolygonAreaSelectionWidget::applySelection); } -PolygonAreaSelectionWidget::~PolygonAreaSelectionWidget() -{ - delete ui_; -} +PolygonAreaSelectionWidget::~PolygonAreaSelectionWidget() { delete ui_; } void PolygonAreaSelectionWidget::init(const shape_msgs::Mesh& mesh) { @@ -73,7 +61,7 @@ void PolygonAreaSelectionWidget::clearROISelection() { ROS_ERROR_STREAM("Tool Path Parameter Editor Widget: Area Selection error:" << srv.response.message); } - submesh_.reset(new shape_msgs::Mesh (*mesh_)); + submesh_.reset(new shape_msgs::Mesh(*mesh_)); emit(selectedSubmesh(submesh_)); return; @@ -91,15 +79,16 @@ void PolygonAreaSelectionWidget::applySelection() bool success = selector_.collectROIMesh(*mesh_, *submesh_, error_message); if (!success) { - ROS_ERROR_STREAM("Tool Path Parameter Editor Widget: Area Selection error: could not compute submesh: " << error_message); + ROS_ERROR_STREAM( + "Tool Path Parameter Editor Widget: Area Selection error: could not compute submesh: " << error_message); } if (submesh_->vertices.size() < 3 || submesh_->triangles.size() < 1) { - submesh_.reset(new shape_msgs::Mesh (*mesh_)); + submesh_.reset(new shape_msgs::Mesh(*mesh_)); } emit(selectedSubmesh(submesh_)); return; } -} // end namespace opp_gui +} // end namespace opp_gui diff --git a/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp b/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp index 4603f36..816d86a 100644 --- a/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp @@ -81,7 +81,7 @@ noether_msgs::SegmentationConfig SegmentationParametersEditorWidget::getSegmenta // TODO: Advanced setting config.use_mesh_normals = true; // If false enable box -// config.neighborhood_radius; + // config.neighborhood_radius; return config; } diff --git a/opp_gui/src/widgets/surface_selection_combo_widget.cpp b/opp_gui/src/widgets/surface_selection_combo_widget.cpp index 83e7e0e..a5062cb 100644 --- a/opp_gui/src/widgets/surface_selection_combo_widget.cpp +++ b/opp_gui/src/widgets/surface_selection_combo_widget.cpp @@ -27,16 +27,14 @@ namespace opp_gui { - const static std::string MARKER_TOPIC = "/target_area"; -SurfaceSelectionComboWidget::SurfaceSelectionComboWidget( - ros::NodeHandle& nh, - const std::string& selection_world_frame, - const std::string& selection_sensor_frame, - QWidget *parent) - : QWidget(parent) // call the base-class constructor - , ui_(new Ui::SurfaceSelectionComboWidget) // initialize the visible element +SurfaceSelectionComboWidget::SurfaceSelectionComboWidget(ros::NodeHandle& nh, + const std::string& selection_world_frame, + const std::string& selection_sensor_frame, + QWidget* parent) + : QWidget(parent) // call the base-class constructor + , ui_(new Ui::SurfaceSelectionComboWidget) // initialize the visible element { // Setup the publisher for visualizing the selected area in RViz selected_area_marker_publisher_ = nh.advertise(MARKER_TOPIC, 1, true); @@ -59,25 +57,24 @@ SurfaceSelectionComboWidget::SurfaceSelectionComboWidget( ui_->list_widget_segment_list->addItem("Full Mesh"); // Create and attach the area selection widget - area_selector_ = new opp_gui::PolygonAreaSelectionWidget( - nh, - selection_world_frame, - selection_sensor_frame, - this); + area_selector_ = new opp_gui::PolygonAreaSelectionWidget(nh, selection_world_frame, selection_sensor_frame, this); ui_->layout_for_selector_widget->addWidget(area_selector_); // Connect the inputs and outputs of sub-widgets - connect( - segmenter_, &opp_gui::SegmentationParametersEditorWidget::segmentationFinished, - this, &opp_gui::SurfaceSelectionComboWidget::newSegmentList); - - connect( - ui_->list_widget_segment_list, &QListWidget::itemSelectionChanged, - this, &opp_gui::SurfaceSelectionComboWidget::newSelectedSegment); - - connect( - area_selector_, &PolygonAreaSelectionWidget::selectedSubmesh, - this, &SurfaceSelectionComboWidget::newSelectedSubmesh); + connect(segmenter_, + &opp_gui::SegmentationParametersEditorWidget::segmentationFinished, + this, + &opp_gui::SurfaceSelectionComboWidget::newSegmentList); + + connect(ui_->list_widget_segment_list, + &QListWidget::itemSelectionChanged, + this, + &opp_gui::SurfaceSelectionComboWidget::newSelectedSegment); + + connect(area_selector_, + &PolygonAreaSelectionWidget::selectedSubmesh, + this, + &SurfaceSelectionComboWidget::newSelectedSubmesh); } SurfaceSelectionComboWidget::~SurfaceSelectionComboWidget() @@ -135,9 +132,8 @@ void SurfaceSelectionComboWidget::init(const shape_msgs::Mesh& mesh) return; } -void SurfaceSelectionComboWidget::newSegmentList( - const std::vector& segments, - const shape_msgs::Mesh::Ptr& remnants) +void SurfaceSelectionComboWidget::newSegmentList(const std::vector& segments, + const shape_msgs::Mesh::Ptr& remnants) { // Save the original mesh with the segments segment_list_.resize(1); @@ -227,4 +223,4 @@ void SurfaceSelectionComboWidget::publishTargetMesh() return; } -} // end namespace opp_gui +} // end namespace opp_gui diff --git a/opp_gui/src/widgets/tool_path_editor_widget.cpp b/opp_gui/src/widgets/tool_path_editor_widget.cpp index 4cafd6d..39c61dd 100644 --- a/opp_gui/src/widgets/tool_path_editor_widget.cpp +++ b/opp_gui/src/widgets/tool_path_editor_widget.cpp @@ -21,17 +21,12 @@ const static std::string TOOL_PATH_TOPIC = "tool_path"; namespace opp_gui { - -ToolPathEditorWidget::ToolPathEditorWidget( - QWidget* parent, - const ros::NodeHandle& nh, - const std::string& marker_frame, - const std::string& selection_world_frame, - const std::string& selection_sensor_frame -) - : ListEditorWidgetBase(parent) - , nh_(nh) - , marker_frame_(marker_frame) +ToolPathEditorWidget::ToolPathEditorWidget(QWidget* parent, + const ros::NodeHandle& nh, + const std::string& marker_frame, + const std::string& selection_world_frame, + const std::string& selection_sensor_frame) + : ListEditorWidgetBase(parent), nh_(nh), marker_frame_(marker_frame) { surface_selector_ = new SurfaceSelectionComboWidget(nh_, selection_world_frame, selection_sensor_frame, this); editor_ = new ToolPathParametersEditorWidget(nh_, this); @@ -52,8 +47,10 @@ ToolPathEditorWidget::ToolPathEditorWidget( connect(editor_, &ToolPathParametersEditorWidget::dataChanged, this, &ToolPathEditorWidget::onDataChanged); - connect(surface_selector_, &SurfaceSelectionComboWidget::newTargetMesh, - this, &ToolPathEditorWidget::newTargetMeshSelected); + connect(surface_selector_, + &SurfaceSelectionComboWidget::newTargetMesh, + this, + &ToolPathEditorWidget::newTargetMeshSelected); // Create a publisher for the tool path marker pub_ = nh_.advertise(TOOL_PATH_TOPIC, 1, true); @@ -102,7 +99,8 @@ void ToolPathEditorWidget::addToolPathData(const std::vector while (data_.find(key) != data_.end()) { ++i; - if (i > 2000) break; // can't be too careful + if (i > 2000) + break; // can't be too careful key = key_base + std::to_string(i); } data_.emplace(key, val); @@ -123,7 +121,7 @@ void ToolPathEditorWidget::onAddPressed() { bool ok; QString key = QInputDialog::getText(this, "Add New", "Name", QLineEdit::Normal, "", &ok); - if(ok && !key.isEmpty()) + if (ok && !key.isEmpty()) { opp_msgs::ToolPath val; val.params.config.line_spacing = 0.2; @@ -137,7 +135,7 @@ void ToolPathEditorWidget::onAddPressed() val.process_type.val = opp_msgs::ProcessType::NONE; // Make sure the key doesn't already exist in the map - if(data_.find(key.toStdString()) == data_.end()) + if (data_.find(key.toStdString()) == data_.end()) { // Add the entry to the map of points data_.emplace(key.toStdString(), val); @@ -153,11 +151,11 @@ void ToolPathEditorWidget::onRemovePressed() // Get the selection from the list widget QList current_items = ui_->list_widget->selectedItems(); - for(QListWidgetItem* item : current_items) + for (QListWidgetItem* item : current_items) { std::string key = item->text().toStdString(); auto it = data_.find(key); - if(it != data_.end()) + if (it != data_.end()) { data_.erase(it); } @@ -172,14 +170,13 @@ void ToolPathEditorWidget::onRemovePressed() } // Disable the parameters frame if there are no tool paths left - if(data_.empty()) + if (data_.empty()) { ui_->frame_parameters->setEnabled(false); } } -void ToolPathEditorWidget::onListSelectionChanged(QListWidgetItem* current, - QListWidgetItem* previous) +void ToolPathEditorWidget::onListSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) { if (previous) { @@ -246,7 +243,7 @@ void ToolPathEditorWidget::publishToolPathDisplay(const opp_msgs::ToolPath& tool { // Concatenate all of the tool path pose arrays (representing tool path segments) into one array for display geometry_msgs::PoseArray tool_path_display; - for(const geometry_msgs::PoseArray& pose_arr : tool_path.paths) + for (const geometry_msgs::PoseArray& pose_arr : tool_path.paths) { tool_path_display.poses.insert(tool_path_display.poses.end(), pose_arr.poses.begin(), pose_arr.poses.end()); } @@ -260,19 +257,18 @@ void ToolPathEditorWidget::publishToolPathDisplay(const opp_msgs::ToolPath& tool void ToolPathEditorWidget::onDataChanged() { - QListWidgetItem* current = ui_->list_widget->currentItem(); - if(current) + if (current) { std::string key = current->text().toStdString(); - if(!key.empty()) + if (!key.empty()) { auto it = data_.find(key); - if(it != data_.end()) + if (it != data_.end()) { opp_msgs::ToolPath tool_path; - if(editor_->getToolPath(tool_path)) + if (editor_->getToolPath(tool_path)) { if (surface_selector_ == nullptr) { @@ -295,4 +291,4 @@ void ToolPathEditorWidget::onDataChanged() } } -} // namespace opp_gui +} // namespace opp_gui diff --git a/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp b/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp index 539c984..b71c6fb 100644 --- a/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp @@ -31,13 +31,8 @@ const static std::string GENERATE_TOOLPATHS_ACTION = "generate_tool_paths"; namespace opp_gui { - -ToolPathParametersEditorWidget::ToolPathParametersEditorWidget( - ros::NodeHandle& nh, - QWidget* parent -) - : QWidget(parent) - , client_(GENERATE_TOOLPATHS_ACTION, false) +ToolPathParametersEditorWidget::ToolPathParametersEditorWidget(ros::NodeHandle& nh, QWidget* parent) + : QWidget(parent), client_(GENERATE_TOOLPATHS_ACTION, false) { ui_ = new Ui::ToolPathParametersEditor(); ui_->setupUi(this); @@ -59,14 +54,17 @@ ToolPathParametersEditorWidget::ToolPathParametersEditorWidget( // Connect the button press to the function that calls the toolpath generation action connect(ui_->push_button_generate, &QPushButton::clicked, this, &ToolPathParametersEditorWidget::generateToolPath); - connect(ui_->combo_box_process_type, &QComboBox::currentTextChanged, this, &ToolPathParametersEditorWidget::updateProcessType); - connect(ui_->spin_box_dwell_time, static_cast(&QSpinBox::valueChanged), this, &ToolPathParametersEditorWidget::updateDwellTime); + connect(ui_->combo_box_process_type, + &QComboBox::currentTextChanged, + this, + &ToolPathParametersEditorWidget::updateProcessType); + connect(ui_->spin_box_dwell_time, + static_cast(&QSpinBox::valueChanged), + this, + &ToolPathParametersEditorWidget::updateDwellTime); } -void ToolPathParametersEditorWidget::init(const shape_msgs::Mesh& mesh) -{ - mesh_.reset(new shape_msgs::Mesh(mesh)); -} +void ToolPathParametersEditorWidget::init(const shape_msgs::Mesh& mesh) { mesh_.reset(new shape_msgs::Mesh(mesh)); } void ToolPathParametersEditorWidget::setToolPathConfig(const noether_msgs::ToolPathConfig& config) { @@ -98,7 +96,7 @@ noether_msgs::ToolPathConfig ToolPathParametersEditorWidget::getToolPathConfig() void ToolPathParametersEditorWidget::setToolPath(const opp_msgs::ToolPath& tool_path) { - if(!tool_path_) + if (!tool_path_) { tool_path_.reset(new opp_msgs::ToolPath(tool_path)); } @@ -112,7 +110,7 @@ void ToolPathParametersEditorWidget::setToolPath(const opp_msgs::ToolPath& tool_ bool ToolPathParametersEditorWidget::getToolPath(opp_msgs::ToolPath& tool_path) const { - if(tool_path_) + if (tool_path_) { tool_path = *tool_path_; return true; @@ -122,14 +120,14 @@ bool ToolPathParametersEditorWidget::getToolPath(opp_msgs::ToolPath& tool_path) void ToolPathParametersEditorWidget::generateToolPath() { - if(!client_.isServerConnected()) + if (!client_.isServerConnected()) { std::string message = "Action server on '" + GENERATE_TOOLPATHS_ACTION + "' is not connected"; QMessageBox::warning(this, "ROS Communication Error", QString(message.c_str())); return; } - if(!mesh_) + if (!mesh_) { QMessageBox::warning(this, "Input Error", "Mesh has not yet been specified"); return; @@ -153,24 +151,25 @@ void ToolPathParametersEditorWidget::generateToolPath() progress_dialog_->show(); } -void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(const actionlib::SimpleClientGoalState& state, - const noether_msgs::GenerateToolPathsResultConstPtr& res) +void ToolPathParametersEditorWidget::onGenerateToolPathsComplete( + const actionlib::SimpleClientGoalState& state, + const noether_msgs::GenerateToolPathsResultConstPtr& res) { - for(int i = progress_dialog_->minimum(); i < progress_dialog_->maximum(); ++i) + for (int i = progress_dialog_->minimum(); i < progress_dialog_->maximum(); ++i) { progress_dialog_->setValue(i); ros::Duration(0.01).sleep(); } progress_dialog_->hide(); - if(state != actionlib::SimpleClientGoalState::SUCCEEDED) + if (state != actionlib::SimpleClientGoalState::SUCCEEDED) { std::string message = "Action '" + GENERATE_TOOLPATHS_ACTION + "' failed to succeed"; QMessageBox::warning(this, "Tool Path Planning Error", QString(message.c_str())); } else { - if(!res->success || !res->tool_path_validities[0]) + if (!res->success || !res->tool_path_validities[0]) { QMessageBox::warning(this, "Tool Path Planning Error", "Tool path generation failed"); } @@ -188,15 +187,16 @@ void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(const actionlib tp.params.config.raster_angle = ui_->double_spin_box_raster_angle->value() * M_PI / 180.0; tp.params.config.min_hole_size = ui_->double_spin_box_min_hole_size->value(); tp.params.config.min_segment_size = ui_->double_spin_box_min_segment_length->value(); - tp.params.config.generate_extra_rasters = false; // No option to set this from GUI at present. - tp.params.config.raster_wrt_global_axes = false; // No option to set this from GUI at present. + tp.params.config.generate_extra_rasters = false; // No option to set this from GUI at present. + tp.params.config.raster_wrt_global_axes = false; // No option to set this from GUI at present. tp.params.config.intersecting_plane_height = ui_->double_spin_box_intersecting_plane_height->value(); // Create the tool path offset transform // 1. Add z offset // 2. Rotate 180 degrees about X Eigen::Isometry3d tool_offset = Eigen::Isometry3d::Identity(); - tool_offset.rotate(Eigen::AngleAxisd(ui_->double_spin_box_tool_pitch->value()*M_PI/180.0, Eigen::Vector3d::UnitY())); + tool_offset.rotate( + Eigen::AngleAxisd(ui_->double_spin_box_tool_pitch->value() * M_PI / 180.0, Eigen::Vector3d::UnitY())); tool_offset.translate(Eigen::Vector3d(0.0, 0.0, ui_->double_spin_box_tool_z_offset->value())); tool_offset.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); tf::poseEigenToMsg(tool_offset, tp.tool_offset); @@ -211,18 +211,19 @@ void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(const actionlib void ToolPathParametersEditorWidget::updateProcessType(const QString&) { - if(tool_path_) + if (tool_path_) { - tool_path_->process_type.val = qvariant_cast(ui_->combo_box_process_type->currentData()); + tool_path_->process_type.val = + qvariant_cast(ui_->combo_box_process_type->currentData()); } } void ToolPathParametersEditorWidget::updateDwellTime(const int value) { - if(tool_path_) + if (tool_path_) { tool_path_->dwell_time = static_cast(value * 60); } } -} // namespace opp_gui +} // namespace opp_gui diff --git a/opp_gui/src/widgets/tool_path_planner_widget.cpp b/opp_gui/src/widgets/tool_path_planner_widget.cpp index 31bb048..2233ffa 100644 --- a/opp_gui/src/widgets/tool_path_planner_widget.cpp +++ b/opp_gui/src/widgets/tool_path_planner_widget.cpp @@ -39,12 +39,10 @@ const static std::string MESH_MARKER_TOPIC = "mesh_marker"; namespace opp_gui { - ToolPathPlannerWidget::ToolPathPlannerWidget(QWidget* parent, const ros::NodeHandle& nh, const std::vector& frames) - : QWidget(parent) - , nh_(nh) + : QWidget(parent), nh_(nh) { ui_ = new Ui::ToolPathPlanner(); ui_->setupUi(this); @@ -95,15 +93,22 @@ ToolPathPlannerWidget::ToolPathPlannerWidget(QWidget* parent, // Connect the signals and slots connect(ui_->push_button_find_model_file, &QPushButton::clicked, this, &ToolPathPlannerWidget::browseForMeshResource); - connect(ui_->push_button_load_parts_from_database, &QPushButton::clicked, this, &ToolPathPlannerWidget::loadModelsFromDatabase); + connect(ui_->push_button_load_parts_from_database, + &QPushButton::clicked, + this, + &ToolPathPlannerWidget::loadModelsFromDatabase); loadModelsFromDatabase(); - connect(ui_->list_widget_parts, &QListWidget::currentItemChanged, this, &ToolPathPlannerWidget::onModelSelectionChanged); + connect( + ui_->list_widget_parts, &QListWidget::currentItemChanged, this, &ToolPathPlannerWidget::onModelSelectionChanged); connect(ui_->push_button_load_selected_part, &QPushButton::clicked, this, &ToolPathPlannerWidget::loadSelectedModel); connect(ui_->push_button_save_entry, &QPushButton::clicked, this, &ToolPathPlannerWidget::saveModel); // Signals & slots for the buttons on job definition page connect(ui_->push_button_new_job, &QPushButton::clicked, this, &ToolPathPlannerWidget::newJob); - connect(ui_->push_button_load_jobs_from_database, &QPushButton::clicked, this, &ToolPathPlannerWidget::loadJobsFromDatabase); + connect(ui_->push_button_load_jobs_from_database, + &QPushButton::clicked, + this, + &ToolPathPlannerWidget::loadJobsFromDatabase); connect(ui_->list_widget_jobs, &QListWidget::currentItemChanged, this, &ToolPathPlannerWidget::onJobSelectionChanged); connect(ui_->push_button_load_selected_job, &QPushButton::clicked, this, &ToolPathPlannerWidget::loadSelectedJob); connect(ui_->push_button_save_job, &QPushButton::clicked, this, &ToolPathPlannerWidget::saveJob); @@ -125,19 +130,19 @@ ToolPathPlannerWidget::ToolPathPlannerWidget(QWidget* parent, model_parts_->setEditStrategy(QSqlTableModel::OnManualSubmit); model_parts_->select(); model_parts_->setFilter(QString::fromStdString(query)); - model_parts_->removeColumn(model_parts_->columnCount() - 1); // don't show the 'suppressed' column + model_parts_->removeColumn(model_parts_->columnCount() - 1); // don't show the 'suppressed' column model_jobs_ = new QSqlTableModel(this, database_.getDatabase()); model_jobs_->setTable(QString::fromStdString(opp_db::JOBS_TABLE_NAME)); model_jobs_->setEditStrategy(QSqlTableModel::OnManualSubmit); model_jobs_->select(); model_jobs_->setFilter(QString::fromStdString(query)); - model_jobs_->removeColumn(model_jobs_->columnCount() - 1); // don't show the 'suppressed' column + model_jobs_->removeColumn(model_jobs_->columnCount() - 1); // don't show the 'suppressed' column // Attach them to the views ui_->table_view_parts->setModel(model_parts_); - ui_->table_view_parts->setEditTriggers(QTableView::NoEditTriggers); // Make read only + ui_->table_view_parts->setEditTriggers(QTableView::NoEditTriggers); // Make read only ui_->table_view_parts->show(); ui_->table_view_jobs->setModel(model_jobs_); - ui_->table_view_jobs->setEditTriggers(QTableView::NoEditTriggers); // Make read only + ui_->table_view_jobs->setEditTriggers(QTableView::NoEditTriggers); // Make read only ui_->table_view_jobs->show(); } @@ -153,7 +158,7 @@ void ToolPathPlannerWidget::setVisualizationFrame(const QString& text) void ToolPathPlannerWidget::browseForMeshResource() { QString filename = QFileDialog::getOpenFileName(this, "Load Model", "", "Mesh Files (*.stl *.ply *.obj)"); - if(filename.isEmpty()) + if (filename.isEmpty()) { ROS_WARN_STREAM(__func__ << ": Empty filename"); return; @@ -168,31 +173,31 @@ void ToolPathPlannerWidget::loadMeshFromResource() { // Get the filename and package of the model std::string filename = ui_->line_edit_model_filename->text().toStdString(); - if(filename.empty()) + if (filename.empty()) { QMessageBox::warning(this, "Input Error", "Model filename or package name not specified"); return; } // Construct the mesh resource name using the package and filename - std::vector file_extensions = {".stl", ".ply", ".obj"}; + std::vector file_extensions = { ".stl", ".ply", ".obj" }; mesh_resource_.clear(); - for(const std::string& ext : file_extensions) + for (const std::string& ext : file_extensions) { std::regex rgx(".*" + ext + "$"); std::smatch match; - if(std::regex_search(filename, match, rgx)) + if (std::regex_search(filename, match, rgx)) { mesh_resource_ = "file://" + filename; break; } } - if(mesh_resource_.empty()) + if (mesh_resource_.empty()) { std::string message = "Invalid mesh resource file extension. Acceptable inputs are: "; - for(const std::string& ext : file_extensions) + for (const std::string& ext : file_extensions) message += ext + " "; QMessageBox::warning(this, "Input Error", QString(message.c_str())); @@ -200,7 +205,8 @@ void ToolPathPlannerWidget::loadMeshFromResource() } ROS_INFO_STREAM("Attempting to load mesh from resource: '" << mesh_resource_ << "'"); - if(!loadMesh()) return; + if (!loadMesh()) + return; } void ToolPathPlannerWidget::loadModelsFromDatabase() @@ -237,11 +243,10 @@ void ToolPathPlannerWidget::loadModelsFromDatabase() return; } -void ToolPathPlannerWidget::onModelSelectionChanged(QListWidgetItem* current, - QListWidgetItem*) +void ToolPathPlannerWidget::onModelSelectionChanged(QListWidgetItem* current, QListWidgetItem*) { // Change the description display - if(current != nullptr) + if (current != nullptr) { ui_->text_edit_part_description->setText(current->data(Qt::ItemDataRole::UserRole).toString()); } @@ -250,14 +255,15 @@ void ToolPathPlannerWidget::onModelSelectionChanged(QListWidgetItem* current, void ToolPathPlannerWidget::loadSelectedModel() { int row = ui_->list_widget_parts->currentRow(); - if(row >= 0 && row < static_cast(existing_parts_.size())) + if (row >= 0 && row < static_cast(existing_parts_.size())) { // Get the selected part information const opp_msgs::Part& part = existing_parts_[static_cast(row)]; generated_model_id_ = part.id; mesh_resource_ = part.mesh_resource; - if(!loadMesh()) return; + if (!loadMesh()) + return; // Update the UI and widgets with all part information ui_->line_edit_model_name->setText(QString::fromStdString(part.name)); @@ -278,13 +284,15 @@ void ToolPathPlannerWidget::loadSelectedModel() void ToolPathPlannerWidget::saveModel() { // Verify that the user intended to save the part - QMessageBox::StandardButton button = QMessageBox::question(this, "Save Part to Database", "Proceed with adding the defined part to the database?"); - if(button == QMessageBox::StandardButton::No) return; + QMessageBox::StandardButton button = + QMessageBox::question(this, "Save Part to Database", "Proceed with adding the defined part to the database?"); + if (button == QMessageBox::StandardButton::No) + return; // Get the relevant model information to save and verify it exists std::string model_name = ui_->line_edit_model_name->text().toStdString(); std::string model_description = ui_->plain_text_edit_model_description->toPlainText().toStdString(); - if(model_name.empty() || model_description.empty()) + if (model_name.empty() || model_description.empty()) { QMessageBox::warning(this, "Input Error", "Model ID or description field(s) is empty"); return; @@ -295,9 +303,10 @@ void ToolPathPlannerWidget::saveModel() using TouchPointMap = std::map; TouchPointMap touch_points = touch_point_editor_->getPoints(); TouchPointMap verification_points = verification_point_editor_->getPoints(); - if(touch_points.size() < 3 || verification_points.size() < 3) + if (touch_points.size() < 3 || verification_points.size() < 3) { - QMessageBox::warning(this, "Invalid Model Definition", "Ensure at least 3 touch points and 3 verification points have been defined"); + QMessageBox::warning( + this, "Invalid Model Definition", "Ensure at least 3 touch points and 3 verification points have been defined"); return; } @@ -309,14 +318,14 @@ void ToolPathPlannerWidget::saveModel() // Copy the touch points part.touch_points.reserve(touch_points.size()); - for(const std::pair& pair : touch_points) + for (const std::pair& pair : touch_points) { part.touch_points.push_back(pair.second); } // Copy the verification points part.verification_points.reserve(verification_points.size()); - for(const std::pair& pair : verification_points) + for (const std::pair& pair : verification_points) { part.verification_points.push_back(pair.second); } @@ -379,10 +388,10 @@ void ToolPathPlannerWidget::loadJobsFromDatabase() void ToolPathPlannerWidget::onJobSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) { - (void) previous; + (void)previous; // Change the description display - if(current != nullptr) + if (current != nullptr) { ui_->text_edit_jobs->setText(current->data(Qt::ItemDataRole::UserRole).toString()); } @@ -396,7 +405,7 @@ void ToolPathPlannerWidget::onJobSelectionChanged(QListWidgetItem* current, QLis void ToolPathPlannerWidget::loadSelectedJob() { int row = ui_->list_widget_jobs->currentRow(); - if(row >= 0 && row < static_cast(existing_jobs_.size())) + if (row >= 0 && row < static_cast(existing_jobs_.size())) { // Get the selected part information const opp_msgs::Job& job = existing_jobs_[static_cast(row)]; @@ -420,8 +429,9 @@ void ToolPathPlannerWidget::loadSelectedJob() void ToolPathPlannerWidget::saveJob() { // Verify that the user intended to press this button - QMessageBox::StandardButton button = QMessageBox::question(this, "Save Job to Database", "Proceed with adding the defined job to the database?"); - if(button == QMessageBox::StandardButton::No) + QMessageBox::StandardButton button = + QMessageBox::question(this, "Save Job to Database", "Proceed with adding the defined job to the database?"); + if (button == QMessageBox::StandardButton::No) { return; } @@ -429,7 +439,7 @@ void ToolPathPlannerWidget::saveJob() // Get the relevant job information to save, and ensure it is non-empty std::string job_name = ui_->line_edit_job_name->text().toStdString(); std::string job_description = ui_->plain_text_edit_job_description->toPlainText().toStdString(); - if(job_name.empty() || job_description.empty()) + if (job_name.empty() || job_description.empty()) { QMessageBox::warning(this, "Input Error", "Job ID or description is invalid"); return; @@ -445,7 +455,7 @@ void ToolPathPlannerWidget::saveJob() // Get the tool paths and add them to the job ToolPathDataMap tool_paths = tool_path_editor_->getToolPathData(); job.paths.reserve(tool_paths.size()); - for(const std::pair& pair : tool_paths) + for (const std::pair& pair : tool_paths) { job.paths.push_back(pair.second); } @@ -492,7 +502,8 @@ void ToolPathPlannerWidget::showPartFromDatabase() generated_model_id_ = part.id; mesh_resource_ = part.mesh_resource; - if(!loadMesh()) return; + if (!loadMesh()) + return; // Update the UI and widgets with all part information ui_->line_edit_model_name->setText(QString::fromStdString(part.name)); @@ -587,7 +598,7 @@ bool ToolPathPlannerWidget::loadMesh() { // Attempt to load this file into a shape_msgs/Mesh type shape_msgs::Mesh mesh; - if(!utils::getMeshMsgFromResource(mesh_resource_, mesh)) + if (!utils::getMeshMsgFromResource(mesh_resource_, mesh)) { std::string message = "Failed to load mesh from resource: '" + mesh_resource_ + "'"; QMessageBox::warning(this, "Input Error", message.c_str()); @@ -606,8 +617,7 @@ bool ToolPathPlannerWidget::loadMesh() // Publish the mesh marker visualization_msgs::Marker mesh_marker = - utils::createMeshMarker(0, "mesh", Eigen::Isometry3d::Identity(), - marker_frame_, mesh_resource_); + utils::createMeshMarker(0, "mesh", Eigen::Isometry3d::Identity(), marker_frame_, mesh_resource_); pub_.publish(mesh_marker); @@ -616,7 +626,7 @@ bool ToolPathPlannerWidget::loadMesh() void ToolPathPlannerWidget::setModelTabsEnabled(bool enabled) { - for(int i = 1; i < ui_->tool_box_model_editor->count(); ++i) + for (int i = 1; i < ui_->tool_box_model_editor->count(); ++i) { ui_->tool_box_model_editor->setItemEnabled(i, enabled); } @@ -627,7 +637,7 @@ void ToolPathPlannerWidget::setModelTabsEnabled(bool enabled) void ToolPathPlannerWidget::setJobTabsEnabled(bool enabled, bool first_enabled) { - for(int i = 1; i < ui_->tool_box_job_editor->count(); ++i) + for (int i = 1; i < ui_->tool_box_job_editor->count(); ++i) { ui_->tool_box_job_editor->setItemEnabled(i, enabled); } @@ -641,4 +651,4 @@ void ToolPathPlannerWidget::setJobTabsEnabled(bool enabled, bool first_enabled) ui_->frame_define_toolpaths->setEnabled(enabled); } -} // namespace opp_gui +} // namespace opp_gui diff --git a/opp_gui/src/widgets/touch_point_editor_widget.cpp b/opp_gui/src/widgets/touch_point_editor_widget.cpp index 1354749..8266731 100644 --- a/opp_gui/src/widgets/touch_point_editor_widget.cpp +++ b/opp_gui/src/widgets/touch_point_editor_widget.cpp @@ -26,14 +26,10 @@ namespace opp_gui { - TouchPointEditorWidget::TouchPointEditorWidget(QWidget* parent, const ros::NodeHandle& nh, const std::string& marker_frame) - : ListEditorWidgetBase(parent) - , marker_frame_(marker_frame) - , marker_color_(utils::createColor(0.0, 0.0, 1.0)) - , nh_(nh) + : ListEditorWidgetBase(parent), marker_frame_(marker_frame), marker_color_(utils::createColor(0.0, 0.0, 1.0)), nh_(nh) { point_editor_ = new TouchPointParametersEditorWidget(this); @@ -63,11 +59,10 @@ void TouchPointEditorWidget::onAddPressed() { bool ok; QString key = QInputDialog::getText(this, "Add New", "Name", QLineEdit::Normal, "", &ok); - if(ok && !key.isEmpty()) + if (ok && !key.isEmpty()) { - // Make sure the key doesn't already exist in the map - if(data_.find(key.toStdString()) == data_.end()) + if (data_.find(key.toStdString()) == data_.end()) { // Set the default value of the touch point opp_msgs::TouchPoint tp; @@ -82,8 +77,7 @@ void TouchPointEditorWidget::onAddPressed() // Add and publish an arrow marker for this point visualization_msgs::Marker marker = - utils::createArrowMarker(0, key.toStdString(), Eigen::Isometry3d::Identity(), - marker_frame_, marker_color_); + utils::createArrowMarker(0, key.toStdString(), Eigen::Isometry3d::Identity(), marker_frame_, marker_color_); marker_pub_.publish(marker); } } @@ -94,11 +88,11 @@ void TouchPointEditorWidget::onRemovePressed() // Get the selection from the list widget QList current_items = ui_->list_widget->selectedItems(); - for(QListWidgetItem* item : current_items) + for (QListWidgetItem* item : current_items) { std::string key = item->text().toStdString(); auto it = data_.find(key); - if(it != data_.end()) + if (it != data_.end()) { data_.erase(it); } @@ -112,29 +106,26 @@ void TouchPointEditorWidget::onRemovePressed() delete item; // Remove the marker from the visualization - visualization_msgs::Marker marker = - utils::createArrowMarker(0, key, Eigen::Isometry3d::Identity(), - marker_frame_, marker_color_, - visualization_msgs::Marker::DELETE); + visualization_msgs::Marker marker = utils::createArrowMarker( + 0, key, Eigen::Isometry3d::Identity(), marker_frame_, marker_color_, visualization_msgs::Marker::DELETE); marker_pub_.publish(marker); } // Disable the parameters frame if there are no touch points left - if(data_.empty()) + if (data_.empty()) { ui_->frame_parameters->setEnabled(false); } } -void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, - QListWidgetItem* previous) +void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) { - if(previous) + if (previous) { std::string prev_name = previous->text().toStdString(); auto prev_it = data_.find(prev_name); - if(prev_it != data_.end()) + if (prev_it != data_.end()) { // Get the current data from the point editor widget and save it for the "previous" point opp_msgs::TouchPoint tp = point_editor_->getTouchPoint(); @@ -150,7 +141,7 @@ void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, } } - if(current) + if (current) { // Enable the parameters frame if the selection is valid ui_->frame_parameters->setEnabled(true); @@ -158,7 +149,7 @@ void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, // Load the data for the "current" point into the point editor widget std::string curr_name = current->text().toStdString(); auto curr_it = data_.find(curr_name); - if(curr_it != data_.end()) + if (curr_it != data_.end()) { point_editor_->setTouchPoint(curr_it->second); } @@ -172,14 +163,14 @@ void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, void TouchPointEditorWidget::onDataChanged() { QListWidgetItem* item = ui_->list_widget->currentItem(); - if(item) + if (item) { std::string key = item->text().toStdString(); - if(!key.empty()) + if (!key.empty()) { // Save the updated information auto it = data_.find(key); - if(it != data_.end()) + if (it != data_.end()) { // Get the current point from the editor opp_msgs::TouchPoint tp = point_editor_->getTouchPoint(); @@ -194,9 +185,7 @@ void TouchPointEditorWidget::onDataChanged() tf::poseMsgToEigen(tp.transform.pose, pose); visualization_msgs::Marker marker = - utils::createArrowMarker(0, key, pose, marker_frame_, - marker_color_, - visualization_msgs::Marker::MODIFY); + utils::createArrowMarker(0, key, pose, marker_frame_, marker_color_, visualization_msgs::Marker::MODIFY); marker_pub_.publish(marker); } } @@ -207,7 +196,7 @@ void TouchPointEditorWidget::setPoints(const std::vector& { clear(); - for(const opp_msgs::TouchPoint& point : points) + for (const opp_msgs::TouchPoint& point : points) { // Add the entry to the map of points data_.emplace(point.name, point); @@ -217,12 +206,9 @@ void TouchPointEditorWidget::setPoints(const std::vector& } } -void TouchPointEditorWidget::setMarkerColor(const double r, - const double g, - const double b, - const double a) +void TouchPointEditorWidget::setMarkerColor(const double r, const double g, const double b, const double a) { marker_color_ = utils::createColor(r, g, b, a); } -} // namespace opp_gui +} // namespace opp_gui diff --git a/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp b/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp index c8151c3..abc5f00 100644 --- a/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp @@ -21,15 +21,14 @@ const std::string CLICKED_POINT_TOPIC = "/selection_point"; namespace opp_gui { - TouchPointParametersEditorWidget::TouchPointParametersEditorWidget(QWidget* parent) - : QWidget(parent) - , accept_mouse_input_(false) + : QWidget(parent), accept_mouse_input_(false) { ui_ = new Ui::TouchPointParametersEditor(); ui_->setupUi(this); - sub_ = nh_.subscribe(CLICKED_POINT_TOPIC, 1, &TouchPointParametersEditorWidget::callback, this); + sub_ = nh_.subscribe( + CLICKED_POINT_TOPIC, 1, &TouchPointParametersEditorWidget::callback, this); // Make the position line edits only accept numbers ui_->double_spin_box_x->setRange(-std::numeric_limits::max(), std::numeric_limits::max()); @@ -45,13 +44,16 @@ TouchPointParametersEditorWidget::TouchPointParametersEditorWidget(QWidget* pare ui_->double_spin_box_threshold->setRange(0.0, std::numeric_limits::max()); // Connect signals and slots - connect(ui_->push_button_select_with_mouse, &QPushButton::clicked, this, &TouchPointParametersEditorWidget::onSelectWithMouse); + connect(ui_->push_button_select_with_mouse, + &QPushButton::clicked, + this, + &TouchPointParametersEditorWidget::onSelectWithMouse); connect(ui_->push_button_update, &QPushButton::clicked, this, &TouchPointParametersEditorWidget::dataChanged); } void TouchPointParametersEditorWidget::onSelectWithMouse() { - if(!accept_mouse_input_) + if (!accept_mouse_input_) { // Keep the push button flat until we have received a message back ui_->push_button_select_with_mouse->setStyleSheet("background-color: red"); @@ -72,7 +74,7 @@ void TouchPointParametersEditorWidget::onSelectWithMouse() void TouchPointParametersEditorWidget::callback(const geometry_msgs::PoseStampedConstPtr& msg) { - if(accept_mouse_input_) + if (accept_mouse_input_) { // Reset the button visualization ui_->push_button_select_with_mouse->setStyleSheet(""); @@ -134,4 +136,4 @@ opp_msgs::TouchPoint TouchPointParametersEditorWidget::getTouchPoint() const return out; } -} // namespace opp_gui +} // namespace opp_gui diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h index c9c7a55..8d2cef6 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h @@ -7,32 +7,29 @@ namespace opp_msgs_serialization { - -template -inline bool serializeToBinary(const std::string& file, - const T& message) +template +inline bool serializeToBinary(const std::string& file, const T& message) { uint32_t serial_size = ros::serialization::serializationLength(message); boost::shared_array buffer(new uint8_t[serial_size]); ros::serialization::OStream stream(buffer.get(), serial_size); ros::serialization::serialize(stream, message); - std::ofstream ofs(file, std::ios::out|std::ios::binary); - if(ofs) + std::ofstream ofs(file, std::ios::out | std::ios::binary); + if (ofs) { - ofs.write((char*) buffer.get(), serial_size); + ofs.write((char*)buffer.get(), serial_size); return ofs.good(); } return false; } -template -inline bool deserializeFromBinary(const std::string& file, - T& message) +template +inline bool deserializeFromBinary(const std::string& file, T& message) { - std::ifstream ifs(file, std::ios::in|std::ios::binary); - if(!ifs) + std::ifstream ifs(file, std::ios::in | std::ios::binary); + if (!ifs) { return false; } @@ -44,7 +41,7 @@ inline bool deserializeFromBinary(const std::string& file, uint32_t file_size = end - begin; boost::shared_array ibuffer(new uint8_t[file_size]); - ifs.read((char*) ibuffer.get(), file_size); + ifs.read((char*)ibuffer.get(), file_size); ros::serialization::IStream istream(ibuffer.get(), file_size); ros::serialization::deserialize(istream, message); @@ -52,6 +49,6 @@ inline bool deserializeFromBinary(const std::string& file, return true; } -} // namespace opp_msgs_serialization +} // namespace opp_msgs_serialization -#endif // OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H +#endif // OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h index 7758b91..7117971 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -22,8 +22,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const Eigen::Affine3d& rhs) @@ -43,7 +42,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const Eigen::Vector3d& rhs) @@ -63,6 +62,6 @@ struct convert } }; -} +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_EIGEN_YAML_H +#endif // OPP_MSGS_SERIALIZATION_EIGEN_YAML_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h index 12cd808..6f58a2a 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -24,8 +24,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const geometry_msgs::Vector3& rhs) @@ -40,7 +39,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::Vector3& rhs) { - if (node.size() != 3) return false; + if (node.size() != 3) + return false; rhs.x = node["x"].as(); rhs.y = node["y"].as(); @@ -49,7 +49,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::Point& rhs) @@ -63,18 +63,18 @@ struct convert static bool decode(const Node& node, geometry_msgs::Point& rhs) { - if (node.size() != 3) return false; + if (node.size() != 3) + return false; rhs.x = node["x"].as(); rhs.y = node["y"].as(); rhs.z = node["z"].as(); - + return true; } }; - -template<> +template <> struct convert { static Node encode(const geometry_msgs::Quaternion& rhs) @@ -90,7 +90,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::Quaternion& rhs) { - if (node.size() != 4) return false; + if (node.size() != 4) + return false; rhs.x = node["x"].as(); rhs.y = node["y"].as(); @@ -101,7 +102,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::Pose& rhs) @@ -115,7 +116,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::Pose& rhs) { - if (node.size() != 2) return false; + if (node.size() != 2) + return false; rhs.position = node["position"].as(); rhs.orientation = node["orientation"].as(); @@ -123,7 +125,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::PoseStamped& rhs) @@ -137,7 +139,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::PoseStamped& rhs) { - if (node.size() != 2) return false; + if (node.size() != 2) + return false; rhs.header = node["header"].as(); rhs.pose = node["pose"].as(); @@ -145,7 +148,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::PoseArray& rhs) @@ -159,7 +162,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::PoseArray& rhs) { - if (node.size() != 2) return false; + if (node.size() != 2) + return false; rhs.header = node["header"].as(); rhs.poses = node["poses"].as >(); @@ -167,7 +171,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::Transform& rhs) @@ -181,7 +185,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::Transform& rhs) { - if (node.size() != 2) return false; + if (node.size() != 2) + return false; rhs.rotation = node["rotation"].as(); rhs.translation = node["translation"].as(); @@ -190,7 +195,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const geometry_msgs::TransformStamped& rhs) @@ -205,7 +210,8 @@ struct convert static bool decode(const Node& node, geometry_msgs::TransformStamped& rhs) { - if (node.size() != 3) return false; + if (node.size() != 3) + return false; rhs.header = node["header"].as(); rhs.child_frame_id = node["child_frame_id"].as(); @@ -214,6 +220,6 @@ struct convert } }; -} +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h index f870ef5..3df454b 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h @@ -27,8 +27,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const noether_msgs::ToolPathConfig& rhs) @@ -48,21 +47,22 @@ struct convert static bool decode(const Node& node, noether_msgs::ToolPathConfig& rhs) { - if(node.size() != 9) return false; - rhs.pt_spacing = node["pt_spacing"].as(); - rhs.line_spacing = node["line_spacing"].as(); - rhs.tool_offset = node["tool_offset"].as(); - rhs.intersecting_plane_height = node["intersecting_plane_height"].as(); - rhs.min_hole_size = node["min_hole_size"].as(); - rhs.min_segment_size = node["min_segment_size"].as(); - rhs.raster_angle = node["raster_angle"].as(); - rhs.raster_wrt_global_axes = node["raster_wrt_global_axes"].as(); - rhs.generate_extra_rasters = node["generate_extra_rasters"].as(); + if (node.size() != 9) + return false; + rhs.pt_spacing = node["pt_spacing"].as(); + rhs.line_spacing = node["line_spacing"].as(); + rhs.tool_offset = node["tool_offset"].as(); + rhs.intersecting_plane_height = node["intersecting_plane_height"].as(); + rhs.min_hole_size = node["min_hole_size"].as(); + rhs.min_segment_size = node["min_segment_size"].as(); + rhs.raster_angle = node["raster_angle"].as(); + rhs.raster_wrt_global_axes = node["raster_wrt_global_axes"].as(); + rhs.generate_extra_rasters = node["generate_extra_rasters"].as(); return true; } }; -template<> +template <> struct convert { static Node encode(const opp_msgs::Job& rhs) @@ -79,18 +79,19 @@ struct convert static bool decode(const Node& node, opp_msgs::Job& rhs) { - if(node.size() != 6) return false; - rhs.header = node["header"].as(); - rhs.id = node["id"].as(); - rhs.name = node["name"].as(); - rhs.description = node["description"].as(); - rhs.part_id = node["part_id"].as(); - rhs.paths = node["paths"].as(); + if (node.size() != 6) + return false; + rhs.header = node["header"].as(); + rhs.id = node["id"].as(); + rhs.name = node["name"].as(); + rhs.description = node["description"].as(); + rhs.part_id = node["part_id"].as(); + rhs.paths = node["paths"].as(); return true; } }; -template<> +template <> struct convert { static Node encode(const opp_msgs::ProcessType& rhs) @@ -102,13 +103,14 @@ struct convert static bool decode(const Node& node, opp_msgs::ProcessType& rhs) { - if(node.size() != 1) return false; - rhs.val = node["val"].as(); + if (node.size() != 1) + return false; + rhs.val = node["val"].as(); return true; } }; -template<> +template <> struct convert { static Node encode(const opp_msgs::ToolPath& rhs) @@ -126,25 +128,26 @@ struct convert static bool decode(const Node& node, opp_msgs::ToolPath& rhs) { // Allow both old entries (without Noether params) and new entries (with Noether params) - if(node.size() != 5 && node.size() != 6) return false; + if (node.size() != 5 && node.size() != 6) + return false; // Get the opp_msgs::ToolPath fields - rhs.header = node["header"].as(); - rhs.process_type = node["process_type"].as(); - rhs.paths = node["paths"].as(); - rhs.dwell_time = node["dwell_time"].as(); - rhs.tool_offset = node["tool_offset"].as(); + rhs.header = node["header"].as(); + rhs.process_type = node["process_type"].as(); + rhs.paths = node["paths"].as(); + rhs.dwell_time = node["dwell_time"].as(); + rhs.tool_offset = node["tool_offset"].as(); // Get the Noether parameters, if they exist if (node["params"]) { - rhs.params = node["params"].as(); + rhs.params = node["params"].as(); } return true; } }; -template<> +template <> struct convert { static Node encode(const opp_msgs::ToolPathParams& rhs) @@ -159,15 +162,18 @@ struct convert static bool decode(const Node& node, opp_msgs::ToolPathParams& rhs) { - if (node.size() != 3) { return false; } - rhs.config = node["config"].as(); - rhs.curvature_threshold = node["curvature_threshold"].as(); - rhs.min_polygons_per_cluster = node["min_polygons_per_cluster"].as(); + if (node.size() != 3) + { + return false; + } + rhs.config = node["config"].as(); + rhs.curvature_threshold = node["curvature_threshold"].as(); + rhs.min_polygons_per_cluster = node["min_polygons_per_cluster"].as(); return true; } }; -template<> +template <> struct convert { static Node encode(const opp_msgs::TouchPoint& rhs) @@ -182,15 +188,16 @@ struct convert static bool decode(const Node& node, opp_msgs::TouchPoint& rhs) { - if(node.size() != 4) return false; - rhs.name = node["name"].as(); - rhs.description = node["description"].as(); - rhs.threshold = node["threshold"].as(); - rhs.transform = node["transform"].as(); + if (node.size() != 4) + return false; + rhs.name = node["name"].as(); + rhs.description = node["description"].as(); + rhs.threshold = node["threshold"].as(); + rhs.transform = node["transform"].as(); return true; } }; -} // namespace YAML +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_OPP_MSGS_YAML_H +#endif // OPP_MSGS_SERIALIZATION_OPP_MSGS_YAML_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h index e59611d..3532e50 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h @@ -23,8 +23,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const sensor_msgs::RegionOfInterest& rhs) @@ -42,19 +41,20 @@ struct convert static bool decode(const Node& node, sensor_msgs::RegionOfInterest& rhs) { - if (node.size() != 5) return false; + if (node.size() != 5) + return false; - rhs.x_offset = node["x_offset"].as(); - rhs.y_offset = node["y_offset"].as(); - rhs.height = node["height"].as(); - rhs.width = node["width"].as(); - rhs.do_rectify = node["do_rectify"].as() ; + rhs.x_offset = node["x_offset"].as(); + rhs.y_offset = node["y_offset"].as(); + rhs.height = node["height"].as(); + rhs.width = node["width"].as(); + rhs.do_rectify = node["do_rectify"].as(); return true; } }; -template<> +template <> struct convert { static Node encode(const sensor_msgs::CameraInfo& rhs) @@ -85,7 +85,8 @@ struct convert static bool decode(const Node& node, sensor_msgs::CameraInfo& rhs) { - if (node.size() != 11) return false; + if (node.size() != 11) + return false; rhs.header = node["header"].as(); rhs.height = node["height"].as(); @@ -110,7 +111,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const sensor_msgs::JointState& rhs) @@ -128,18 +129,19 @@ struct convert static bool decode(const Node& node, sensor_msgs::JointState& rhs) { - if (node.size() != 5) return false; + if (node.size() != 5) + return false; - rhs.header = node["header"].as(); - rhs.name = node["name"].as(); - rhs.position = node["position"].as(); - rhs.velocity = node["velocity"].as(); - rhs.effort = node["effort"].as() ; + rhs.header = node["header"].as(); + rhs.name = node["name"].as(); + rhs.position = node["position"].as(); + rhs.velocity = node["velocity"].as(); + rhs.effort = node["effort"].as(); return true; } }; -} // namespace YAML +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h index 1858232..c11d605 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -24,11 +24,10 @@ namespace opp_msgs_serialization { - template -bool serialize(const std::string &file, const T& val) +bool serialize(const std::string& file, const T& val) { - std::ofstream ofh (file); + std::ofstream ofh(file); if (!ofh) { return false; @@ -40,7 +39,7 @@ bool serialize(const std::string &file, const T& val) } template -bool deserialize(const std::string &file, T& val) +bool deserialize(const std::string& file, T& val) { try { @@ -57,6 +56,6 @@ bool deserialize(const std::string &file, T& val) } } -} // namespace opp_msgs_serialization +} // namespace opp_msgs_serialization -#endif // OPP_MSGS_SERIALIZATION_SERIALIZE_H +#endif // OPP_MSGS_SERIALIZATION_SERIALIZE_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h index d5f58d8..5ae7600 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -22,8 +22,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const ros::Time& rhs) @@ -36,7 +35,8 @@ struct convert static bool decode(const Node& node, ros::Time& rhs) { - if (node.size() != 2) return false; + if (node.size() != 2) + return false; rhs.sec = node["sec"].as(); rhs.nsec = node["nsec"].as(); @@ -45,7 +45,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const std_msgs::Header& rhs) @@ -59,7 +59,8 @@ struct convert static bool decode(const Node& node, std_msgs::Header& rhs) { - if (node.size() != 3) return false; + if (node.size() != 3) + return false; rhs.seq = node["seq"].as(); rhs.stamp = node["stamp"].as(); @@ -71,6 +72,6 @@ struct convert // TODO: Duration -} +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_STD_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_STD_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h index 406535e..01713eb 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h @@ -1,12 +1,12 @@ -/* +/* * Copyright 2018 Southwest Research Institute * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. @@ -22,8 +22,7 @@ namespace YAML { - -template<> +template <> struct convert { static Node encode(const trajectory_msgs::JointTrajectoryPoint& rhs) @@ -40,7 +39,8 @@ struct convert static bool decode(const Node& node, trajectory_msgs::JointTrajectoryPoint& rhs) { - if (node.size() != 5) return false; + if (node.size() != 5) + return false; rhs.positions = node["positions"].as >(); rhs.velocities = node["velocities"].as >(); @@ -52,7 +52,7 @@ struct convert } }; -template<> +template <> struct convert { static Node encode(const trajectory_msgs::JointTrajectory& rhs) @@ -66,16 +66,17 @@ struct convert static bool decode(const Node& node, trajectory_msgs::JointTrajectory& rhs) { - if (node.size() != 3) return false; - + if (node.size() != 3) + return false; + rhs.header = node["header"].as(); rhs.joint_names = node["joint_names"].as >(); - rhs.points = node["points"].as >(); - + rhs.points = node["points"].as >(); + return true; } }; -} +} // namespace YAML -#endif // OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML diff --git a/opp_msgs_serialization/test/serialization_test.cpp b/opp_msgs_serialization/test/serialization_test.cpp index f4131f2..cad0624 100644 --- a/opp_msgs_serialization/test/serialization_test.cpp +++ b/opp_msgs_serialization/test/serialization_test.cpp @@ -74,20 +74,26 @@ TEST(opp_serialization, tool_path) orig_msg.process_type.val = opp_msgs::ProcessType::PROCESS_DEPAINT; - for(unsigned segment_idx = 0; segment_idx < 3; ++segment_idx) + for (unsigned segment_idx = 0; segment_idx < 3; ++segment_idx) { geometry_msgs::PoseArray segment; - for(unsigned point_idx = 0; point_idx < 10; ++point_idx) + for (unsigned point_idx = 0; point_idx < 10; ++point_idx) { geometry_msgs::Pose pose; pose.position.x = static_cast(point_idx); - pose.position.y = static_cast(point_idx);; - pose.position.z = static_cast(point_idx);; - pose.orientation.w = static_cast(point_idx);; - pose.orientation.x = static_cast(point_idx);; - pose.orientation.y = static_cast(point_idx);; - pose.orientation.z = static_cast(point_idx);; + pose.position.y = static_cast(point_idx); + ; + pose.position.z = static_cast(point_idx); + ; + pose.orientation.w = static_cast(point_idx); + ; + pose.orientation.x = static_cast(point_idx); + ; + pose.orientation.y = static_cast(point_idx); + ; + pose.orientation.z = static_cast(point_idx); + ; segment.poses.push_back(std::move(pose)); } @@ -113,13 +119,13 @@ TEST(opp_serialization, tool_path) EXPECT_EQ(new_msg.tool_offset.orientation.y, orig_msg.tool_offset.orientation.y); EXPECT_EQ(new_msg.tool_offset.orientation.z, orig_msg.tool_offset.orientation.z); - for(unsigned segment_idx = 0; segment_idx < new_msg.paths.size(); ++segment_idx) + for (unsigned segment_idx = 0; segment_idx < new_msg.paths.size(); ++segment_idx) { const geometry_msgs::PoseArray& new_segment = new_msg.paths[segment_idx]; const geometry_msgs::PoseArray& orig_segment = orig_msg.paths[segment_idx]; EXPECT_EQ(new_segment.poses.size(), orig_segment.poses.size()); - for(unsigned point_idx = 0; point_idx < new_segment.poses.size(); ++point_idx) + for (unsigned point_idx = 0; point_idx < new_segment.poses.size(); ++point_idx) { const geometry_msgs::Pose& new_pose = new_msg.paths[segment_idx].poses[point_idx]; const geometry_msgs::Pose& orig_pose = orig_msg.paths[segment_idx].poses[point_idx]; @@ -135,7 +141,7 @@ TEST(opp_serialization, tool_path) } } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);