From f9d111f3311924ff544c0b0a7a1b637f1854e502 Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:04:05 -0600 Subject: [PATCH 01/12] Added Message Package --- opp_msgs/CMakeLists.txt | 50 ++++++++++++++++++++++++++++++++ opp_msgs/msg/ErrorType.msg | 23 +++++++++++++++ opp_msgs/msg/Job.msg | 17 +++++++++++ opp_msgs/msg/Part.msg | 15 ++++++++++ opp_msgs/msg/ProcessType.msg | 13 +++++++++ opp_msgs/msg/ToolPath.msg | 6 ++++ opp_msgs/msg/ToolPathParams.msg | 13 +++++++++ opp_msgs/msg/TouchPoint.msg | 4 +++ opp_msgs/package.xml | 18 ++++++++++++ opp_msgs/srv/GetROISelection.srv | 12 ++++++++ 10 files changed, 171 insertions(+) create mode 100644 opp_msgs/CMakeLists.txt create mode 100644 opp_msgs/msg/ErrorType.msg create mode 100644 opp_msgs/msg/Job.msg create mode 100644 opp_msgs/msg/Part.msg create mode 100644 opp_msgs/msg/ProcessType.msg create mode 100644 opp_msgs/msg/ToolPath.msg create mode 100644 opp_msgs/msg/ToolPathParams.msg create mode 100644 opp_msgs/msg/TouchPoint.msg create mode 100644 opp_msgs/package.xml create mode 100644 opp_msgs/srv/GetROISelection.srv diff --git a/opp_msgs/CMakeLists.txt b/opp_msgs/CMakeLists.txt new file mode 100644 index 0000000..6f82175 --- /dev/null +++ b/opp_msgs/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_msgs) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + message_generation + noether_msgs + sensor_msgs + std_msgs +) + +# Prevents certain runtime errors in Ubuntu 18.04 +add_definitions(${PCL_DEFINITIONS}) + +add_message_files( + DIRECTORY msg + FILES + ErrorType.msg + Job.msg + Part.msg + ProcessType.msg + ToolPath.msg + ToolPathParams.msg + TouchPoint.msg +) + +add_service_files( + DIRECTORY srv + FILES + GetROISelection.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs + noether_msgs + sensor_msgs + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + geometry_msgs + message_runtime + noether_msgs + sensor_msgs + std_msgs +) diff --git a/opp_msgs/msg/ErrorType.msg b/opp_msgs/msg/ErrorType.msg new file mode 100644 index 0000000..24e36d9 --- /dev/null +++ b/opp_msgs/msg/ErrorType.msg @@ -0,0 +1,23 @@ +# This message type includes an enumeration containing common error codes. +# The errors have been grouped + +uint8 NONE = 0 +uint8 DATABASE_ERROR = 10 +uint8 ICP_ERROR = 20 +uint8 TOUCHOFF_ERROR = 30 +uint8 TOUCHOFF_PREEMPTED = 31 +uint8 TOUCHOFF_ABORTED = 32 +uint8 VERIFICATION_ERROR = 40 +uint8 VERIFICATION_PREEMPTED = 41 +uint8 VERIFICATION_ABORTED = 42 +uint8 FREESPACE_PLANNING_ERROR = 50 +uint8 PROCESS_PLANNING_ERROR = 60 +uint8 MOTION_EXECUTION_ERROR = 70 +uint8 MOTION_EXECUTION_PRE_MOVEMENT = 71 +uint8 MOTION_EXECUTION_DURING_MOVEMENT = 72 +uint8 SEGMENTATION_ERROR = 90 +uint8 AREA_SELECTION_ERROR = 100 +uint8 PLC_COMMUNICATION_ERROR = 110 + +uint8 val +string msg diff --git a/opp_msgs/msg/Job.msg b/opp_msgs/msg/Job.msg new file mode 100644 index 0000000..4266b2e --- /dev/null +++ b/opp_msgs/msg/Job.msg @@ -0,0 +1,17 @@ +# header - ROS header containing timestamp information. +Header header + +# id - unique integer ID number used as a key to find this job in the database (auto-generated by the database) +uint32 id + +# name - name of the job that is meaningful to the user +string name + +# description - description of the job +string description + +# part_id - integer ID used to identify the part associated with this job +uint32 part_id + +# tasks - Array of tasks associated with this job +opp_msgs/ToolPath[] paths diff --git a/opp_msgs/msg/Part.msg b/opp_msgs/msg/Part.msg new file mode 100644 index 0000000..1feffb5 --- /dev/null +++ b/opp_msgs/msg/Part.msg @@ -0,0 +1,15 @@ +# Unique ID (to be auto-generated by the database) +uint32 id + +# Name that is meaningful to the user +string name + +# Short description of the part +string description + +# The file path of the mesh resource in the format "package:///.stl" or "file://///.stl" +string mesh_resource + +# Localization information +opp_msgs/TouchPoint[] touch_points +opp_msgs/TouchPoint[] verification_points diff --git a/opp_msgs/msg/ProcessType.msg b/opp_msgs/msg/ProcessType.msg new file mode 100644 index 0000000..e761877 --- /dev/null +++ b/opp_msgs/msg/ProcessType.msg @@ -0,0 +1,13 @@ +# No process +uint8 NONE = 0 + +# Chemical Depaint Processes - use the Chem Depaint process planner(s) +uint8 PROCESS_PAINT = 1 +uint8 PROCESS_DEPAINT = 2 + +# val - which process has actually been selected +uint8 val + +# If you add a new process to this file and it needs to be available in +# offline planner, remember to add it to the drop-down menu in the gui: +# opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp diff --git a/opp_msgs/msg/ToolPath.msg b/opp_msgs/msg/ToolPath.msg new file mode 100644 index 0000000..8d5b92b --- /dev/null +++ b/opp_msgs/msg/ToolPath.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +opp_msgs/ProcessType process_type +geometry_msgs/PoseArray[] paths +uint32 dwell_time # Time (in seconds) to dwell after the tool path has been completed +geometry_msgs/Pose tool_offset # 6-DOF tool_offset +opp_msgs/ToolPathParams params # Numerical parameters used for tool path generation diff --git a/opp_msgs/msg/ToolPathParams.msg b/opp_msgs/msg/ToolPathParams.msg new file mode 100644 index 0000000..784ef40 --- /dev/null +++ b/opp_msgs/msg/ToolPathParams.msg @@ -0,0 +1,13 @@ +# ToolPathParams.msg +# This message is meant to facilitate a backwards-compatible change to +# record the tool path parameters used in the creation of a particular +# tool path. It gathers up various numerical information specified when +# creating the path and saves it in a single structure, insulating the +# ToolPath message from being affected by changes in logged data. + +# The general Noether configuration parameters +noether_msgs/ToolPathConfig config + +# The segmentation parameters +float64 curvature_threshold +int32 min_polygons_per_cluster diff --git a/opp_msgs/msg/TouchPoint.msg b/opp_msgs/msg/TouchPoint.msg new file mode 100644 index 0000000..363e9e0 --- /dev/null +++ b/opp_msgs/msg/TouchPoint.msg @@ -0,0 +1,4 @@ +string name +string description +float64 threshold +geometry_msgs/PoseStamped transform diff --git a/opp_msgs/package.xml b/opp_msgs/package.xml new file mode 100644 index 0000000..3c0f5aa --- /dev/null +++ b/opp_msgs/package.xml @@ -0,0 +1,18 @@ + + + opp_msgs + 0.0.0 + The opp_msgs package + David Merz, Jr. + Apache 2.0 + David Merz, Jr. + + catkin + message_generation + message_runtime + + geometry_msgs + noether_msgs + sensor_msgs + std_msgs + diff --git a/opp_msgs/srv/GetROISelection.srv b/opp_msgs/srv/GetROISelection.srv new file mode 100644 index 0000000..0d9d2fb --- /dev/null +++ b/opp_msgs/srv/GetROISelection.srv @@ -0,0 +1,12 @@ +# Request + +# The set of points from which the ROI +# will be selected +sensor_msgs/PointCloud2 input_cloud + +--- + +# Response +bool success +string message +int32[] cloud_indices From 7c47a559206ab212fadc6a0a51036566bad5973f Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:04:32 -0600 Subject: [PATCH 02/12] Added YAML Serialization package --- opp_msgs_serialization/CMakeLists.txt | 68 ++++++ .../binary_serialization.h | 56 +++++ .../opp_msgs_serialization/eigen_yaml.h | 67 ++++++ .../geometry_msgs_yaml.h | 218 ++++++++++++++++++ .../opp_msgs_serialization/opp_msgs_yaml.h | 195 ++++++++++++++++ .../opp_msgs_serialization/sensor_msgs_yaml.h | 144 ++++++++++++ .../opp_msgs_serialization/serialize.h | 60 +++++ .../opp_msgs_serialization/std_msgs_yaml.h | 75 ++++++ .../trajectory_msgs_yaml.h | 80 +++++++ opp_msgs_serialization/package.xml | 23 ++ .../test/serialization_test.cpp | 145 ++++++++++++ 11 files changed, 1131 insertions(+) create mode 100644 opp_msgs_serialization/CMakeLists.txt create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/serialize.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h create mode 100644 opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h create mode 100644 opp_msgs_serialization/package.xml create mode 100644 opp_msgs_serialization/test/serialization_test.cpp diff --git a/opp_msgs_serialization/CMakeLists.txt b/opp_msgs_serialization/CMakeLists.txt new file mode 100644 index 0000000..78d47b5 --- /dev/null +++ b/opp_msgs_serialization/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_msgs_serialization) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + noether_msgs + roscpp + opp_msgs + eigen_conversions + geometry_msgs + sensor_msgs + std_msgs + roscpp_serialization +) + +find_package(yaml-cpp REQUIRED) + +# Prevents certain runtime errors in Ubuntu 18.04 +add_definitions(${PCL_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + include + CATKIN_DEPENDS + eigen_conversions + geometry_msgs + sensor_msgs + std_msgs + roscpp_serialization + noether_msgs + roscpp + opp_msgs + DEPENDS + YAML_CPP +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + catkin_add_gtest(${PROJECT_NAME}_test + test/serialization_test.cpp + ) + target_link_libraries(${PROJECT_NAME}_test + ${catkin_LIBRARIES} + ) +endif() diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h new file mode 100644 index 0000000..b09640e --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h @@ -0,0 +1,56 @@ +#ifndef MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H +#define MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H + +#include +#include + +namespace opp_msgs_serialization +{ + +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) + { + ofs.write((char*) buffer.get(), serial_size); + return ofs.good(); + } + + return false; +} + +template +inline bool deserializeFromBinary(const std::string& file, + T& message) +{ + std::ifstream ifs(file, std::ios::in|std::ios::binary); + if(!ifs) + { + return false; + } + + ifs.seekg(0, std::ios::end); + std::streampos end = ifs.tellg(); + ifs.seekg(0, std::ios::beg); + std::streampos begin = ifs.tellg(); + uint32_t file_size = end - begin; + + boost::shared_array ibuffer(new uint8_t[file_size]); + ifs.read((char*) ibuffer.get(), file_size); + ros::serialization::IStream istream(ibuffer.get(), file_size); + ros::serialization::deserialize(istream, message); + + ifs.close(); + return true; +} + +} // namespace amsted_opp_msgs_serialization + +#endif // AMSTED_MESSAGE_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 new file mode 100644 index 0000000..16da41f --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h @@ -0,0 +1,67 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_EIGEN_YAML_H +#define MESSAGE_SERIALIZATION_EIGEN_YAML_H + +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const Eigen::Affine3d& rhs) + { + geometry_msgs::Pose msg; + tf::poseEigenToMsg(rhs, msg); + Node node = Node(msg); + return node; + } + + static bool decode(const Node& node, Eigen::Affine3d& rhs) + { + geometry_msgs::Pose msg; + msg = node.as(); + tf::poseMsgToEigen(msg, rhs); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const Eigen::Vector3d& rhs) + { + geometry_msgs::Point msg; + tf::pointEigenToMsg(rhs, msg); + Node node = Node(msg); + return node; + } + + static bool decode(const Node& node, Eigen::Vector3d& rhs) + { + geometry_msgs::Point msg; + msg = node.as(); + tf::pointMsgToEigen(msg, rhs); + return true; + } +}; + +} + +#endif // MESSAGE_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 new file mode 100644 index 0000000..ed5ef44 --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h @@ -0,0 +1,218 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML +#define MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML + +#include +#include +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const geometry_msgs::Vector3& rhs) + { + Node node; + node["x"] = rhs.x; + node["y"] = rhs.y; + node["z"] = rhs.z; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::Vector3& rhs) + { + if (node.size() != 3) return false; + + rhs.x = node["x"].as(); + rhs.y = node["y"].as(); + rhs.z = node["z"].as(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::Point& rhs) + { + Node node; + node["x"] = rhs.x; + node["y"] = rhs.y; + node["z"] = rhs.z; + return node; + } + + static bool decode(const Node& node, geometry_msgs::Point& rhs) + { + if (node.size() != 3) return false; + + rhs.x = node["x"].as(); + rhs.y = node["y"].as(); + rhs.z = node["z"].as(); + + return true; + } +}; + + +template<> +struct convert +{ + static Node encode(const geometry_msgs::Quaternion& rhs) + { + Node node; + node["x"] = rhs.x; + node["y"] = rhs.y; + node["z"] = rhs.z; + node["w"] = rhs.w; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::Quaternion& rhs) + { + if (node.size() != 4) return false; + + rhs.x = node["x"].as(); + rhs.y = node["y"].as(); + rhs.z = node["z"].as(); + rhs.w = node["w"].as(); + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::Pose& rhs) + { + Node node; + node["position"] = rhs.position; + node["orientation"] = rhs.orientation; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::Pose& rhs) + { + if (node.size() != 2) return false; + + rhs.position = node["position"].as(); + rhs.orientation = node["orientation"].as(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::PoseStamped& rhs) + { + Node node; + node["header"] = rhs.header; + node["pose"] = rhs.pose; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::PoseStamped& rhs) + { + if (node.size() != 2) return false; + + rhs.header = node["header"].as(); + rhs.pose = node["pose"].as(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::PoseArray& rhs) + { + Node node; + node["header"] = rhs.header; + node["poses"] = rhs.poses; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::PoseArray& rhs) + { + if (node.size() != 2) return false; + + rhs.header = node["header"].as(); + rhs.poses = node["poses"].as >(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::Transform& rhs) + { + Node node; + node["rotation"] = rhs.rotation; + node["translation"] = rhs.translation; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::Transform& rhs) + { + if (node.size() != 2) return false; + + rhs.rotation = node["rotation"].as(); + rhs.translation = node["translation"].as(); + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const geometry_msgs::TransformStamped& rhs) + { + Node node; + node["header"] = rhs.header; + node["child_frame_id"] = rhs.child_frame_id; + node["transform"] = rhs.transform; + + return node; + } + + static bool decode(const Node& node, geometry_msgs::TransformStamped& rhs) + { + if (node.size() != 3) return false; + + rhs.header = node["header"].as(); + rhs.child_frame_id = node["child_frame_id"].as(); + rhs.transform = node["transform"].as(); + return true; + } +}; + +} + +#endif // MESSAGE_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 new file mode 100644 index 0000000..674cc48 --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h @@ -0,0 +1,195 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_MSGS_SERIALIZATION_OPP_MSGS_YAML_H +#define OPP_MSGS_SERIALIZATION_OPP_MSGS_YAML_H + +#include + +#include +#include +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const noether_msgs::ToolPathConfig& rhs) + { + Node node; + node["pt_spacing"] = rhs.pt_spacing; + node["line_spacing"] = rhs.line_spacing; + node["tool_offset"] = rhs.tool_offset; + node["intersecting_plane_height"] = rhs.intersecting_plane_height; + node["min_hole_size"] = rhs.min_hole_size; + node["min_segment_size"] = rhs.min_segment_size; + node["raster_angle"] = rhs.raster_angle; + node["raster_wrt_global_axes"] = rhs.raster_wrt_global_axes; + node["generate_extra_rasters"] = rhs.generate_extra_rasters; + return node; + } + + 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(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const opp_msgs::Job& rhs) + { + Node node; + node["header"] = rhs.header; + node["id"] = rhs.id; + node["name"] = rhs.name; + node["description"] = rhs.description; + node["part_id"] = rhs.part_id; + node["paths"] = rhs.paths; + return node; + } + + 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(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const opp_msgs::ProcessType& rhs) + { + Node node; + node["val"] = rhs.val; + return node; + } + + static bool decode(const Node& node, opp_msgs::ProcessType& rhs) + { + if(node.size() != 1) return false; + rhs.val = node["val"].as(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const opp_msgs::ToolPath& rhs) + { + Node node; + node["header"] = rhs.header; + node["process_type"] = rhs.process_type; + node["paths"] = rhs.paths; + node["dwell_time"] = rhs.dwell_time; + node["tool_offset"] = rhs.tool_offset; + node["params"] = rhs.params; + return node; + } + + 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; + + // 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(); + + // Get the Noether parameters, if they exist + if (node["params"]) + { + rhs.params = node["params"].as(); + } + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const opp_msgs::ToolPathParams& rhs) + { + Node node; + // Do the things inside the noether_msgs::ToolPathConfig + node["config"] = rhs.config; + node["curvature_threshold"] = rhs.curvature_threshold; + node["min_polygons_per_cluster"] = rhs.min_polygons_per_cluster; + return node; + } + + 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(); + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const opp_msgs::TouchPoint& rhs) + { + Node node; + node["name"] = rhs.name; + node["description"] = rhs.description; + node["threshold"] = rhs.threshold; + node["transform"] = rhs.transform; + return node; + } + + 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(); + return true; + } +}; + +} // namespace YAML + +#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 new file mode 100644 index 0000000..13b33f6 --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h @@ -0,0 +1,144 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML +#define MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML + +#include +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const sensor_msgs::RegionOfInterest& rhs) + { + Node node; + + node["x_offset"] = rhs.x_offset; + node["y_offset"] = rhs.y_offset; + node["height"] = rhs.height; + node["width"] = rhs.width; + node["do_rectify"] = rhs.do_rectify; + + return node; + } + + static bool decode(const Node& node, sensor_msgs::RegionOfInterest& rhs) + { + 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() ; + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const sensor_msgs::CameraInfo& rhs) + { + Node node; + + node["header"] = rhs.header; + node["height"] = rhs.height; + node["width"] = rhs.width; + node["distortion_model"] = rhs.distortion_model; + + std::vector K_vec, R_vec, P_vec; + + std::copy_n(rhs.K.begin(), rhs.K.size(), std::back_inserter(K_vec)); + std::copy_n(rhs.R.begin(), rhs.R.size(), std::back_inserter(R_vec)); + std::copy_n(rhs.P.begin(), rhs.P.size(), std::back_inserter(P_vec)); + + node["D"] = rhs.D; + node["K"] = K_vec; + node["R"] = R_vec; + node["P"] = P_vec; + node["binning_x"] = rhs.binning_x; + node["binning_y"] = rhs.binning_y; + node["roi"] = rhs.roi; + + return node; + } + + static bool decode(const Node& node, sensor_msgs::CameraInfo& rhs) + { + if (node.size() != 11) return false; + + rhs.header = node["header"].as(); + rhs.height = node["height"].as(); + rhs.width = node["width"].as(); + rhs.distortion_model = node["distortion_model"].as(); + rhs.D = node["D"].as(); + + std::vector K_vec, R_vec, P_vec; + K_vec = node["K"].as(); + R_vec = node["R"].as(); + P_vec = node["P"].as(); + + std::copy_n(K_vec.begin(), K_vec.size(), rhs.K.begin()); + std::copy_n(R_vec.begin(), R_vec.size(), rhs.R.begin()); + std::copy_n(P_vec.begin(), P_vec.size(), rhs.P.begin()); + + rhs.binning_x = node["binning_x"].as(); + rhs.binning_y = node["binning_y"].as(); + rhs.roi = node["roi"].as(); + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const sensor_msgs::JointState& rhs) + { + Node node; + + node["header"] = rhs.header; + node["name"] = rhs.name; + node["position"] = rhs.position; + node["velocity"] = rhs.velocity; + node["effort"] = rhs.effort; + + return node; + } + + static bool decode(const Node& node, sensor_msgs::JointState& rhs) + { + 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() ; + + return true; + } +}; + +} // namespace YAML + +#endif // MESSAGE_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 new file mode 100644 index 0000000..698005d --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h @@ -0,0 +1,60 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_SERIALIZE_H +#define MESSAGE_SERIALIZATION_SERIALIZE_H + +#include +#include +#include + +namespace opp_msgs_serialization +{ + +template +bool serialize(const std::string &file, const T& val) +{ + std::ofstream ofh (file); + if (!ofh) + { + return false; + } + + YAML::Node n = YAML::Node(val); + ofh << n; + return true; +} + +template +bool deserialize(const std::string &file, T& val) +{ + try + { + YAML::Node node; + node = YAML::LoadFile(file); + val = node.as(); + return true; + } + catch (const YAML::Exception& ex) + { + ROS_ERROR_STREAM("An exception was thrown while processing file: " << file); + ROS_ERROR_STREAM(ex.what()); + return false; + } +} + +} // namespace opp_msgs_serialization + +#endif // MESSAGE_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 new file mode 100644 index 0000000..f8526ac --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h @@ -0,0 +1,75 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_STD_MSGS_YAML +#define MESSAGE_SERIALIZATION_STD_MSGS_YAML + +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const ros::Time& rhs) + { + Node node; + node["sec"] = rhs.sec; + node["nsec"] = rhs.nsec; + return node; + } + + static bool decode(const Node& node, ros::Time& rhs) + { + if (node.size() != 2) return false; + + rhs.sec = node["sec"].as(); + rhs.nsec = node["nsec"].as(); + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const std_msgs::Header& rhs) + { + Node node; + node["seq"] = rhs.seq; + node["stamp"] = rhs.stamp; + node["frame_id"] = rhs.frame_id; + return node; + } + + static bool decode(const Node& node, std_msgs::Header& rhs) + { + if (node.size() != 3) return false; + + rhs.seq = node["seq"].as(); + rhs.stamp = node["stamp"].as(); + rhs.frame_id = node["frame_id"].as(); + + return true; + } +}; + +// TODO: Duration + +} + +#endif // MESSAGE_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 new file mode 100644 index 0000000..1ef37c0 --- /dev/null +++ b/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h @@ -0,0 +1,80 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML +#define MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML + +#include +#include + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const trajectory_msgs::JointTrajectoryPoint& rhs) + { + Node node; + node["positions"] = rhs.positions; + node["velocities"] = rhs.velocities; + node["accelerations"] = rhs.accelerations; + node["effort"] = rhs.effort; + node["time_from_start"] = rhs.time_from_start.toSec(); + + return node; + } + + static bool decode(const Node& node, trajectory_msgs::JointTrajectoryPoint& rhs) + { + if (node.size() != 5) return false; + + rhs.positions = node["positions"].as >(); + rhs.velocities = node["velocities"].as >(); + rhs.accelerations = node["accelerations"].as >(); + rhs.effort = node["effort"].as >(); + rhs.time_from_start = ros::Duration(node["time_from_start"].as()); + + return true; + } +}; + +template<> +struct convert +{ + static Node encode(const trajectory_msgs::JointTrajectory& rhs) + { + Node node; + node["header"] = rhs.header; + node["joint_names"] = rhs.joint_names; + node["points"] = rhs.points; + return node; + } + + static bool decode(const Node& node, trajectory_msgs::JointTrajectory& rhs) + { + if (node.size() != 3) return false; + + rhs.header = node["header"].as(); + rhs.joint_names = node["joint_names"].as >(); + rhs.points = node["points"].as >(); + + return true; + } +}; + +} + +#endif // MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML diff --git a/opp_msgs_serialization/package.xml b/opp_msgs_serialization/package.xml new file mode 100644 index 0000000..68f628e --- /dev/null +++ b/opp_msgs_serialization/package.xml @@ -0,0 +1,23 @@ + + + opp_msgs_serialization + 0.0.0 + The opp_msgs_serialization package + + mripperger + mripperger + + Apache 2.0 + + catkin + noether_msgs + roscpp + opp_msgs + eigen_conversions + geometry_msgs + sensor_msgs + std_msgs + roscpp_serialization + + rostest + diff --git a/opp_msgs_serialization/test/serialization_test.cpp b/opp_msgs_serialization/test/serialization_test.cpp new file mode 100644 index 0000000..5ea22ee --- /dev/null +++ b/opp_msgs_serialization/test/serialization_test.cpp @@ -0,0 +1,145 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +TEST(opp_serialization, process_type) +{ + std::string file = "process_type.yaml"; + opp_msgs::ProcessType orig_msg; + orig_msg.val = opp_msgs::ProcessType::PROCESS_PAINT; + + bool result = opp_msgs_serialization::serialize(file, orig_msg); + EXPECT_TRUE(result); + + opp_msgs::ProcessType new_msg; + bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + EXPECT_TRUE(load_result); + + bool eq = (orig_msg.val == new_msg.val); + EXPECT_TRUE(eq); +} + +TEST(opp_serialization, touch_point) +{ + std::string file = "touch_point.yaml"; + opp_msgs::TouchPoint orig_msg; + orig_msg.name = "test"; + orig_msg.description = "test"; + orig_msg.transform.pose.position.x = 1.0; + orig_msg.transform.pose.position.y = 1.0; + orig_msg.transform.pose.position.z = 1.0; + orig_msg.transform.pose.orientation.w = 1.0; + orig_msg.transform.pose.orientation.x = 1.0; + orig_msg.transform.pose.orientation.y = 1.0; + orig_msg.transform.pose.orientation.z = 1.0; + + bool save_result = opp_msgs_serialization::serialize(file, orig_msg); + EXPECT_TRUE(save_result); + + opp_msgs::TouchPoint new_msg; + bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + EXPECT_TRUE(load_result); + + EXPECT_EQ(new_msg.name, orig_msg.name); + EXPECT_EQ(new_msg.description, orig_msg.description); + EXPECT_EQ(new_msg.transform.pose.position.x, orig_msg.transform.pose.position.x); + EXPECT_EQ(new_msg.transform.pose.position.y, orig_msg.transform.pose.position.y); + EXPECT_EQ(new_msg.transform.pose.position.z, orig_msg.transform.pose.position.z); + EXPECT_EQ(new_msg.transform.pose.orientation.w, orig_msg.transform.pose.orientation.w); + EXPECT_EQ(new_msg.transform.pose.orientation.x, orig_msg.transform.pose.orientation.x); + EXPECT_EQ(new_msg.transform.pose.orientation.y, orig_msg.transform.pose.orientation.y); + EXPECT_EQ(new_msg.transform.pose.orientation.z, orig_msg.transform.pose.orientation.z); +} + +TEST(opp_serialization, tool_path) +{ + std::string file = "tool_path.yaml"; + opp_msgs::ToolPath orig_msg; + + orig_msg.process_type.val = opp_msgs::ProcessType::SAND_STRIP; + + for(unsigned segment_idx = 0; segment_idx < 3; ++segment_idx) + { + geometry_msgs::PoseArray segment; + + 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);; + + segment.poses.push_back(std::move(pose)); + } + + orig_msg.paths.push_back(std::move(segment)); + } + + bool result = opp_msgs_serialization::serialize(file, orig_msg); + EXPECT_TRUE(result); + + opp_msgs::ToolPath new_msg; + bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + EXPECT_TRUE(load_result); + + EXPECT_EQ(new_msg.process_type.val, orig_msg.process_type.val); + EXPECT_EQ(new_msg.paths.size(), orig_msg.paths.size()); + + EXPECT_EQ(new_msg.tool_offset.position.x, orig_msg.tool_offset.position.x); + EXPECT_EQ(new_msg.tool_offset.position.y, orig_msg.tool_offset.position.y); + EXPECT_EQ(new_msg.tool_offset.position.z, orig_msg.tool_offset.position.z); + EXPECT_EQ(new_msg.tool_offset.orientation.w, orig_msg.tool_offset.orientation.w); + EXPECT_EQ(new_msg.tool_offset.orientation.x, orig_msg.tool_offset.orientation.x); + 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) + { + 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) + { + 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]; + + EXPECT_EQ(new_pose.position.x, orig_pose.position.x); + EXPECT_EQ(new_pose.position.y, orig_pose.position.y); + EXPECT_EQ(new_pose.position.z, orig_pose.position.z); + EXPECT_EQ(new_pose.orientation.w, orig_pose.orientation.w); + EXPECT_EQ(new_pose.orientation.x, orig_pose.orientation.x); + EXPECT_EQ(new_pose.orientation.y, orig_pose.orientation.y); + EXPECT_EQ(new_pose.orientation.z, orig_pose.orientation.z); + } + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + int ret = RUN_ALL_TESTS(); + + return ret; +} From 4b460d508c3f0aed7d7072af38526388ad873512 Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:05:13 -0600 Subject: [PATCH 03/12] Added Area Selection Tool package --- opp_area_selection/CMakeLists.txt | 97 ++++ .../config/area_selection_parameters.yaml | 8 + .../opp_area_selection/area_selector.h | 84 ++++ .../area_selector_parameters.h | 42 ++ .../include/opp_area_selection/filter.h | 47 ++ .../include/opp_area_selection/filter_impl.h | 75 +++ .../opp_area_selection/selection_artist.h | 107 +++++ opp_area_selection/package.xml | 30 ++ .../src/area_selection_node.cpp | 67 +++ opp_area_selection/src/area_selector.cpp | 288 ++++++++++++ opp_area_selection/src/selection_artist.cpp | 437 ++++++++++++++++++ 11 files changed, 1282 insertions(+) create mode 100644 opp_area_selection/CMakeLists.txt create mode 100644 opp_area_selection/config/area_selection_parameters.yaml create mode 100644 opp_area_selection/include/opp_area_selection/area_selector.h create mode 100644 opp_area_selection/include/opp_area_selection/area_selector_parameters.h create mode 100644 opp_area_selection/include/opp_area_selection/filter.h create mode 100644 opp_area_selection/include/opp_area_selection/filter_impl.h create mode 100644 opp_area_selection/include/opp_area_selection/selection_artist.h create mode 100644 opp_area_selection/package.xml create mode 100644 opp_area_selection/src/area_selection_node.cpp create mode 100644 opp_area_selection/src/area_selector.cpp create mode 100644 opp_area_selection/src/selection_artist.cpp diff --git a/opp_area_selection/CMakeLists.txt b/opp_area_selection/CMakeLists.txt new file mode 100644 index 0000000..ea82d7b --- /dev/null +++ b/opp_area_selection/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_area_selection) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + eigen_conversions + geometry_msgs + opp_msgs_serialization + roscpp + roslib + opp_msgs + sensor_msgs + std_srvs + tf + tf_conversions + visualization_msgs +) + +find_package(Eigen3 REQUIRED) + +find_package(PCL 1.9 REQUIRED) +add_definitions(${PCL_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + area_selection + CATKIN_DEPENDS + eigen_conversions + geometry_msgs + opp_msgs_serialization + roscpp + roslib + opp_msgs + sensor_msgs + std_srvs + tf + tf_conversions + visualization_msgs + DEPENDS + EIGEN3 + PCL +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +# Library containing tools for specifying regions +# and determining inlying points +add_library(area_selection + src/area_selector.cpp + src/selection_artist.cpp +) +target_link_libraries(area_selection + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) +add_dependencies(area_selection + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +# Node that runs area selection service +add_executable(area_selection_node + src/area_selection_node.cpp +) +target_link_libraries(area_selection_node + area_selection + ${catkin_LIBRARIES} +) +add_dependencies(area_selection_node + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +# Install built targets +install(TARGETS + area_selection + area_selection_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES config/area_selection_parameters.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) diff --git a/opp_area_selection/config/area_selection_parameters.yaml b/opp_area_selection/config/area_selection_parameters.yaml new file mode 100644 index 0000000..400a812 --- /dev/null +++ b/opp_area_selection/config/area_selection_parameters.yaml @@ -0,0 +1,8 @@ +cluster_tolerance: 0.25 +min_cluster_size: 100 +max_cluster_size: 500000 +plane_distance_threshold: 1.0 +normal_est_radius: 0.17 +region_growing_nneighbors: 10 +region_growing_smoothness: 5.0 +region_growing_curvature: 1.0 diff --git a/opp_area_selection/include/opp_area_selection/area_selector.h b/opp_area_selection/include/opp_area_selection/area_selector.h new file mode 100644 index 0000000..d25f77b --- /dev/null +++ b/opp_area_selection/include/opp_area_selection/area_selector.h @@ -0,0 +1,84 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_AREA_SELECTION_AREA_SELECTOR_H +#define OPP_AREA_SELECTION_AREA_SELECTOR_H + +#include +#include + +#include +#include +#include +#include + +#include + +namespace opp_area_selection +{ + +struct FittedPlane +{ + Eigen::Vector3d normal = Eigen::Vector3d::UnitZ(); + Eigen::Vector3d origin = Eigen::Vector3d::Zero(); +}; + +/** + * @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 +{ +public: + /** + * @brief AreaSelector class constructor + */ + 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. + */ + 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); + + 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); +}; + +} // namespace opp_area_selection + +#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 new file mode 100644 index 0000000..f25eebe --- /dev/null +++ b/opp_area_selection/include/opp_area_selection/area_selector_parameters.h @@ -0,0 +1,42 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H +#define OPP_AREA_SELECTION_AREA_SELECTOR_PARAMETERS_H + +#include + +namespace opp_area_selection +{ + +struct AreaSelectorParameters +{ + double cluster_tolerance = 0.25; + int min_cluster_size = 100; + int max_cluster_size = std::numeric_limits::max(); + double plane_distance_threshold = 1.0; + + double normal_est_radius = 0.17; + 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 + diff --git a/opp_area_selection/include/opp_area_selection/filter.h b/opp_area_selection/include/opp_area_selection/filter.h new file mode 100644 index 0000000..1277a5f --- /dev/null +++ b/opp_area_selection/include/opp_area_selection/filter.h @@ -0,0 +1,47 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef FILTER_H +#define FILTER_H + +#include + +namespace opp_area_selection +{ + +namespace data_filtering +{ + +template +using Cloud = typename pcl::PointCloud; + +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); + +} // end namespace data_filtering + +} // end namespace opp_area_selection + +#include + +#endif // 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 new file mode 100644 index 0000000..e5f4940 --- /dev/null +++ b/opp_area_selection/include/opp_area_selection/filter_impl.h @@ -0,0 +1,75 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef AREA_SELECTOR_IMPL_H +#define AREA_SELECTOR_IMPL_H + +#include + +#include +#include +#include +#include + +#include + +namespace opp_area_selection +{ + +namespace data_filtering +{ + +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::SACSegmentation seg; + + // Set segmentation parameters + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(threshold); + seg.setInputCloud(input_cloud); + + // Extract best fit cylinder inliers and calculate cylinder coefficients + seg.segment(*plane_inliers, *plane_coefficients); + + 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()); + + // Create new point cloud from + extract.setInputCloud(input_cloud); + extract.setIndices(plane_inliers); + extract.setNegative(false); + extract.filter(output_cloud); + + return true; +} + +} // end namespace data_filtering + +} // end namespace opp_area_selection + +#endif // AREA_SELECTOR_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 new file mode 100644 index 0000000..355c23c --- /dev/null +++ b/opp_area_selection/include/opp_area_selection/selection_artist.h @@ -0,0 +1,107 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H +#define OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +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. + */ +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) + * @param nh + * @param world_frame + * @param 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 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 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); + + ros::NodeHandle nh_; + + std::string world_frame_; + + std::string sensor_frame_; + + ros::Subscriber drawn_points_sub_; + + ros::Publisher marker_pub_; + + ros::ServiceServer clear_roi_points_srv_; + + ros::ServiceServer collect_roi_points_srv_; + + std::shared_ptr listener_; + + visualization_msgs::MarkerArray marker_array_; + +}; + +} // namespace opp_area_selection + +#endif // OPP_AREA_SELECTION_SELECTION_AREA_ARTIST_H diff --git a/opp_area_selection/package.xml b/opp_area_selection/package.xml new file mode 100644 index 0000000..9231419 --- /dev/null +++ b/opp_area_selection/package.xml @@ -0,0 +1,30 @@ + + opp_area_selection + 0.0.0 + + + The opp_area_selection package contains a number of tools to aid + selection of submeshes in RViz using the publish point tool. Much of + this code is based on the area selection module from the A5 project. + + + David Merz, Jr. + David Merz, Jr. + Apache 2.0 + + catkin + + eigen_conversions + geometry_msgs + opp_msgs_serialization + pcl_ros + roscpp + roslib + opp_msgs + sensor_msgs + std_srvs + tf + tf_conversions + visualization_msgs + + diff --git a/opp_area_selection/src/area_selection_node.cpp b/opp_area_selection/src/area_selection_node.cpp new file mode 100644 index 0000000..03d00ac --- /dev/null +++ b/opp_area_selection/src/area_selection_node.cpp @@ -0,0 +1,67 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include + +static const float TIMEOUT = 15.0; + +int main(int argc, char **argv) +{ + // Set up ROS. + ros::init(argc, argv, "roi_selection_node"); + + // Set up ROS node handle + ros::NodeHandle pnh("~"); + ros::NodeHandle nh; + + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Get ROS parameters + std::string 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)) + { + ROS_FATAL("'sensor_data_frame' parameter must be set"); + return 1; + } + + // Wait for tf to initialize + tf::TransformListener listener; + 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); + + ros::waitForShutdown(); + spinner.stop(); + + return 0; +} diff --git a/opp_area_selection/src/area_selector.cpp b/opp_area_selection/src/area_selector.cpp new file mode 100644 index 0000000..fb310da --- /dev/null +++ b/opp_area_selection/src/area_selector.cpp @@ -0,0 +1,288 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace +{ + +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)); + return d; +} + +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; + cluster_indices.push_back(a); + cluster_indices.push_back(b); + + // 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) + { + // Get the centroid of the cluster + pcl::CentroidPoint centroid; + for(auto it = cluster_it->indices.begin(); it != cluster_it->indices.end(); ++it) + { + centroid.add(cloud->points[static_cast(*it)]); + } + pcl::PointXYZ centroid_pt; + centroid.get(centroid_pt); + dist.push_back(sqrDistance(centroid_pt, origin)); + } + + return dist.front() < dist.back(); +} + +} // namespace anonymous + +namespace opp_area_selection +{ + +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::CentroidPoint centroid; + for(auto it = points.begin(); it != points.end(); ++it) + { + pcl::PointXYZ pt; + pt.x = static_cast((*it)(0)); + pt.y = static_cast((*it)(1)); + pt.z = static_cast((*it)(2)); + + input_cloud->points.push_back(pt); + centroid.add(pt); + } + + 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)) + { + ROS_ERROR("Unable to fit points to plane model"); + return {}; + } + + FittedPlane plane; + plane.normal(0) = static_cast(coefficients->values[0]); + plane.normal(1) = static_cast(coefficients->values[1]); + plane.normal(2) = static_cast(coefficients->values[2]); + plane.normal.normalize(); + + plane.origin(0) = static_cast(origin.x); + plane.origin(1) = static_cast(origin.y); + plane.origin(2) = static_cast(origin.z); + + return boost::make_optional(plane); +} + +pcl::PointCloud::Ptr AreaSelector::projectPointsOntoPlane( + const std::vector& points, + const FittedPlane& plane) +{ + pcl::PointCloud projected_points; + for(const Eigen::Vector3d& current_pt : points) + { + // Create a vector from plane origin to point + 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 + 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; + + // Convert each point to PCL format + pcl::PointXYZ pt; + pt.x = static_cast(proj_pt(0)); + pt.y = static_cast(proj_pt(1)); + pt.z = static_cast(proj_pt(2)); + + projected_points.points.push_back(pt); + } + + 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) +{ + // Calculate the diagonal of search_points_ cloud + pcl::PointXYZ min_pt, max_pt; + pcl::getMinMax3D(*cloud, min_pt, max_pt); + double half_dist = std::sqrt(static_cast(sqrDistance(min_pt, max_pt))) / 2.0; + + // Extrude the convex hull by half the max distance + pcl::ExtractPolygonalPrismData prism; + 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()) + { + 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::ExtractIndices extract; + extract.setInputCloud(cloud); + extract.setIndices(selection_indices); + extract.setNegative(false); + extract.filter(*prism_cloud); + + // 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::NormalEstimation normal_estimator; + 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)); + + std::vector int_indices; + reg.extract(int_indices); + + // We need to translate the indices into the prism cloud into indices into the original cloud (via selection indices) + std::vector cluster_indices; + cluster_indices.reserve(int_indices.size()); + for (const auto& index_set : int_indices) + { + pcl::PointIndices output_set; + output_set.header = index_set.header; + for (const auto i : index_set.indices) + { + output_set.indices.push_back(selection_indices->indices[static_cast(i)]); + } + cluster_indices.push_back(output_set); + } + + + if(cluster_indices.size() > 1) + { + 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; + polygon_centroid.x = static_cast(plane.origin(0)); + polygon_centroid.y = static_cast(plane.origin(1)); + polygon_centroid.z = static_cast(plane.origin(2)); + + // Find the closest cluster + auto closest_cluster = std::min_element(cluster_indices.begin(), + cluster_indices.end(), + boost::bind(&clusterComparator, _1, _2, cloud, polygon_centroid)); + return closest_cluster->indices; + } + else + { + // Either one or zero clusters were found, so just return all of the points in the extruded hull + return selection_indices->indices; + } +} + +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) + { + 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) + { + ROS_ERROR("No points to search for inside the selection polygon"); + return {}; + } + + // Take selection points (points_) and fit a plane to them using RANSAC + boost::optional plane = fitPlaneToPoints(points, params); + if(!plane) + { + ROS_ERROR("Failed to fit plane to selection points"); + return {}; + } + + // Project the selection points onto the fitted plane + pcl::PointCloud::Ptr proj_sel_points = projectPointsOntoPlane(points, plane.get()); + + /* 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()) + { + ROS_ERROR("Unable to identify points in the region of interest"); + return {}; + } + + return roi_cloud_indices; +} + +} // namespace opp_area_selection + diff --git a/opp_area_selection/src/selection_artist.cpp b/opp_area_selection/src/selection_artist.cpp new file mode 100644 index 0000000..e552afb --- /dev/null +++ b/opp_area_selection/src/selection_artist.cpp @@ -0,0 +1,437 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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"; + +} // end namespace opp_area_selection. It appears twice - once for constants and once for actual code. + +namespace +{ +std::vector makeVisual(const std::string& frame_id) +{ + visualization_msgs::Marker points, lines; + points.header.frame_id = lines.header.frame_id = frame_id; + points.ns = lines.ns = "roi_selection"; + points.action = lines.action = visualization_msgs::Marker::ADD; + + // Point specific properties + points.id = 0; + points.type = visualization_msgs::Marker::SPHERE_LIST; + points.scale.x = points.scale.y = 0.1; + points.color.r = points.color.a = 1.0; + points.pose.orientation.w = 1.0; + + // Line specific properties + lines.id = 1; + lines.type = visualization_msgs::Marker::LINE_STRIP; + lines.scale.x = lines.scale.y = 0.05; + lines.color.r = lines.color.a = 1.0; + lines.pose.orientation.w = 1.0; + + std::vector visuals; + visuals.push_back(points); + visuals.push_back(lines); + return visuals; +} + +bool pclToShapeMsg(const pcl::PolygonMesh& pcl_mesh, shape_msgs::Mesh& mesh_msg) +{ + // Make sure that there are at least three points and at least one polygon + if (pcl_mesh.cloud.height * pcl_mesh.cloud.width < 3 || pcl_mesh.polygons.size() < 1) + { + return false; + } + + // Prepare the message's vectors to receive data + // One resize now saves time later + mesh_msg.vertices.resize(pcl_mesh.cloud.height * pcl_mesh.cloud.width); + mesh_msg.triangles.resize(pcl_mesh.polygons.size()); + + // Get the points from the pcl mesh + pcl::PointCloud vertices; + pcl::fromPCLPointCloud2(pcl_mesh.cloud, vertices); + + // Copy the coordinates inside the vertices into the new mesh + // TODO: Maybe check for nan? + for (std::size_t i = 0; i < vertices.size(); ++i) + { + mesh_msg.vertices[i].x = static_cast(vertices[i]._PointXYZ::x); + mesh_msg.vertices[i].y = static_cast(vertices[i]._PointXYZ::y); + mesh_msg.vertices[i].z = static_cast(vertices[i]._PointXYZ::z); + } + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < pcl_mesh.polygons.size(); ++i) + { + // If a 'polygon' in the old mesh did not have 3 or more vertices, + // throw an error. (If the polygon has 4 or more, just use the first + // 3 to make a triangle.) + // TODO: It is possible to decompose any polygon into multiple + // triangles, and that would prevent loss of data here. + if (pcl_mesh.polygons[i].vertices.size() < 3) + { + return false; + } + mesh_msg.triangles[i].vertex_indices[0] = pcl_mesh.polygons[i].vertices[0]; + mesh_msg.triangles[i].vertex_indices[1] = pcl_mesh.polygons[i].vertices[1]; + mesh_msg.triangles[i].vertex_indices[2] = pcl_mesh.polygons[i].vertices[2]; + } + + return true; +} + +bool pclFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl::PolygonMesh& pcl_mesh) +{ + // Make sure that there are at least three points and at least one polygon + if (mesh_msg.vertices.size() < 3 || mesh_msg.triangles.size() < 1) + { + return false; + } + + // Prepare PCL structures to receive data + // Resizing once now saves time later + pcl::PointCloud vertices; + vertices.resize(mesh_msg.vertices.size()); + pcl_mesh.polygons.resize(mesh_msg.triangles.size()); + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < mesh_msg.vertices.size(); ++i) + { + vertices[i]._PointXYZ::x = static_cast(mesh_msg.vertices[i].x); + vertices[i]._PointXYZ::y = static_cast(mesh_msg.vertices[i].y); + vertices[i]._PointXYZ::z = static_cast(mesh_msg.vertices[i].z); + } + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < mesh_msg.triangles.size(); ++i) + { + pcl_mesh.polygons[i].vertices.resize(3); + pcl_mesh.polygons[i].vertices[0] = mesh_msg.triangles[i].vertex_indices[0]; + pcl_mesh.polygons[i].vertices[1] = mesh_msg.triangles[i].vertex_indices[1]; + pcl_mesh.polygons[i].vertices[2] = mesh_msg.triangles[i].vertex_indices[2]; + } + + // Use the filled pointcloud to populate the pcl mesh + pcl::toPCLPointCloud2(vertices, pcl_mesh.cloud); + + return true; +} + +} // end anonymous namespace, used for utility functions + +namespace YAML +{ + +template<> +struct convert +{ + static Node encode(const opp_area_selection::AreaSelectorParameters& rhs) + { + Node node; + node["cluster_tolerance"] = rhs.cluster_tolerance; + node["min_cluster_size"] = rhs.min_cluster_size; + node["max_cluster_size"] = rhs.max_cluster_size; + node["plane_distance_threshold"] = rhs.plane_distance_threshold; + node["normal_est_radius"] = rhs.normal_est_radius; + node["region_growing_nneighbors"] = rhs.region_growing_nneighbors; + node["region_growing_smoothness"] = rhs.region_growing_smoothness; + node["region_growing_curvature"] = rhs.region_growing_curvature; + return node; + } + + static bool decode(const Node& node, opp_area_selection::AreaSelectorParameters& rhs) + { + if (node.size() != 8) + { + 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(); + return true; + } +}; + +} // end namespace YAML, used to deserialize config file + +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) +{ + marker_pub_ = nh_.advertise(MARKER_ARRAY_TOPIC, 1, false); + listener_.reset(new tf::TransformListener (nh_)); + + 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); + + // Initialize subscribers and callbacks + 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) +{ + (void)req; + for(auto it = marker_array_.markers.begin(); it != marker_array_.markers.end(); ++it) + { + it->points.clear(); + } + + marker_pub_.publish(marker_array_); + res.success = true; + res.message = "Selection cleared"; + + return true; +} + +bool SelectionArtist::collectROIMesh( + const shape_msgs::Mesh& mesh_msg, + shape_msgs::Mesh& submesh_msg, + std::string& message) +{ + pcl::PolygonMesh mesh; + pclFromShapeMsg(mesh_msg, mesh); + pcl::PointCloud mesh_cloud; + pcl::fromPCLPointCloud2(mesh.cloud, mesh_cloud); + opp_msgs::GetROISelection srv; + pcl::toROSMsg(mesh_cloud, srv.request.input_cloud); + + bool success = collectROIPointsCb(srv.request, srv.response); + if (!success || !srv.response.success) + { + submesh_msg = mesh_msg; + message = srv.response.message; + return false; + } + + pcl::PolygonMesh submesh; + filterMesh(mesh, srv.response.cloud_indices, submesh); + pclToShapeMsg(submesh, submesh_msg); + + return true; +} + +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;}); + + // Convert the selection points to Eigen vectors + std::vector 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::fromROSMsg(req.input_cloud, *cloud); + + AreaSelectorParameters params; + bool success = opp_msgs_serialization::deserialize(area_selection_config_file, params); + if (!success) + { + ROS_ERROR_STREAM("Could not load area selection config from: " << area_selection_config_file); + return false; + } + AreaSelector sel; + res.cloud_indices = sel.getRegionOfInterest(cloud, points, params); + + if(!res.cloud_indices.empty()) + { + res.success = true; + res.message = "Selection complete"; + } + else + { + res.success = false; + res.message = "Unable to identify points within selection boundary"; + } + + return true; +} + +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 + tf::StampedTransform frame; + try + { + listener_->lookupTransform(sensor_frame_, world_frame_, ros::Time(0), frame); + } + catch(tf::TransformException ex) + { + ROS_ERROR("%s", ex.what()); + return false; + } + + Eigen::Affine3d transform; + tf::transformTFToEigen(frame, transform); + + // 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); + pt_vec_sensor = transform * pt_vec_ff; + + transformed_pt.x = pt_vec_sensor(0); + transformed_pt.y = pt_vec_sensor(1); + transformed_pt.z = pt_vec_sensor(2); + + return true; +} + +void SelectionArtist::addSelectionPoint(const geometry_msgs::PointStampedConstPtr pt_stamped) +{ + geometry_msgs::Point pt; + if(!transformPoint(pt_stamped, pt)) + { + return; + } + + // Get the iterator to the points and lines markers in the interactive marker + 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;}); + + 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;}); + + // Add new point to the points marker + if(points_it == marker_array_.markers.end() || lines_it == marker_array_.markers.end()) + { + ROS_ERROR("Unable to find line or point marker"); + return; + } + else + { + points_it->points.push_back(pt); + + // 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()) + { + lines_it->points.push_back(pt); + lines_it->points.push_back(pt); + } + // Insert the new point in the second to last position if points already exist in the array + else + { + const auto it = lines_it->points.end() - 1; + lines_it->points.insert(it, pt); + } + } + + marker_pub_.publish(marker_array_); +} + +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); + for (std::size_t i = 0; i < inlying_indices.size(); ++i) + { + whitelist[ static_cast(inlying_indices[i]) ] = true; + } + + // All points marked 'false' in the whitelist will be removed. If + // all of the polygon's points are marked 'true', that polygon + // should be included. + pcl::PolygonMesh intermediate_mesh; + 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] ]) + { + intermediate_mesh.polygons.push_back( input_mesh.polygons[i] ); + } + } + + // Remove unused points and save the result to the output mesh + pcl::surface::SimplificationRemoveUnusedVertices simplifier; + simplifier.simplify(intermediate_mesh, output_mesh); + + return; +} + +} // namespace opp_area_selection From 5c1b4f92c40fa39c4b7383f5ae41b23cb80c029d Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:05:33 -0600 Subject: [PATCH 04/12] Added Database Interface package --- opp_database/CMakeLists.txt | 77 +++ opp_database/README.md | 16 + .../opp_database/opp_database_interface_cpp.h | 91 ++++ opp_database/package.xml | 28 ++ .../src/opp_database_interface_cpp.cpp | 459 ++++++++++++++++++ .../test/database_interface_cpp_test.cpp | 53 ++ 6 files changed, 724 insertions(+) create mode 100644 opp_database/CMakeLists.txt create mode 100644 opp_database/README.md create mode 100644 opp_database/include/opp_database/opp_database_interface_cpp.h create mode 100644 opp_database/package.xml create mode 100644 opp_database/src/opp_database_interface_cpp.cpp create mode 100644 opp_database/test/database_interface_cpp_test.cpp diff --git a/opp_database/CMakeLists.txt b/opp_database/CMakeLists.txt new file mode 100644 index 0000000..c9de48b --- /dev/null +++ b/opp_database/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_database) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + opp_msgs + opp_msgs_serialization +) + +find_package(Qt5 REQUIRED COMPONENTS Sql) +find_package(Boost REQUIRED) + +# Prevents certain runtime errors in Ubuntu 18.04 +add_definitions(${PCL_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME}_cpp + CATKIN_DEPENDS + roscpp + roslib + opp_msgs + opp_msgs_serialization + DEPENDS + Qt5 + Boost +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME}_cpp + src/${PROJECT_NAME}_interface_cpp.cpp +) +target_link_libraries(${PROJECT_NAME}_cpp + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + Qt5::Sql +) + + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME}_cpp + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + catkin_add_gtest(database_interface_cpp_test test/database_interface_cpp_test.cpp) + target_link_libraries(database_interface_cpp_test + ${catkin_LIBRARIES} + ${PROJECT_NAME}_cpp + ${Boost_LIBRARIES} + Qt5::Sql + ) +endif() diff --git a/opp_database/README.md b/opp_database/README.md new file mode 100644 index 0000000..fbc4e77 --- /dev/null +++ b/opp_database/README.md @@ -0,0 +1,16 @@ +# opp_database + +## Database Setup + +1. Open the mysql client and create a database with the correct name: + ``` + mysql -u root -p + mysql> CREATE DATABASE opp; + mysql> exit + ``` + ``` + +2. You can test that the package & database have been set up correctly: + ``` + catkin run_tests opp_database + ``` diff --git a/opp_database/include/opp_database/opp_database_interface_cpp.h b/opp_database/include/opp_database/opp_database_interface_cpp.h new file mode 100644 index 0000000..4f429a0 --- /dev/null +++ b/opp_database/include/opp_database/opp_database_interface_cpp.h @@ -0,0 +1,91 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_DATABASE_INTERAFACE_CPP_H +#define OPP_DATABASE_INTERAFACE_CPP_H + +#include +#include + +#include +#include + +#include +#include + +namespace opp_db +{ + +const static std::string JOBS_TABLE_NAME = "jobs"; +const static std::string PARTS_TABLE_NAME = "parts"; + +class ROSDatabaseInterface +{ +public: + ROSDatabaseInterface(); + virtual ~ROSDatabaseInterface(); + + QSqlDatabase& getDatabase(); + bool isConnected() const; + + long int addJobToDatabase(const opp_msgs::Job& job, + 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 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 getAllPartsFromDatabase(std::string& message, + std::map& parts); + + bool suppressJob(const unsigned int job_id, + std::string& message); + + bool suppressPart(const unsigned int part_id, + std::string& message); + + long int getLastEntryId(const std::string& table_name); + + std::string getErrorString(QSqlQuery& query) const; + +protected: + 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); + + bool createJobsTable(); + + bool createPartsTable(); + + bool createTableHelper(const QString &table_name, const QString& script); + +}; + +} +#endif // OPP_DATABASE_INTERAFACE_CPP_H diff --git a/opp_database/package.xml b/opp_database/package.xml new file mode 100644 index 0000000..bd7ad8e --- /dev/null +++ b/opp_database/package.xml @@ -0,0 +1,28 @@ + + + opp_database + 0.0.0 + + + The opp_database package contains two database interfaces which + used in different places in the project. The first accesses a MySQL + database through a python connector, which communicates with the + system via ROS services. The second is a C++ library utilizing QSQL + to more flexibly and directly interface with the MySQL database. + + + David Merz, Jr. + David Merz, Jr. + + Apache 2.0 + + catkin + roscpp + roslib + opp_msgs + opp_msgs_serialization + libqt5sql5-mysql + libmysqlclient-dev + libssl-dev + + diff --git a/opp_database/src/opp_database_interface_cpp.cpp b/opp_database/src/opp_database_interface_cpp.cpp new file mode 100644 index 0000000..73d1f1c --- /dev/null +++ b/opp_database/src/opp_database_interface_cpp.cpp @@ -0,0 +1,459 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace opp_db +{ + +ROSDatabaseInterface::ROSDatabaseInterface() +{ + database_ = QSqlDatabase::addDatabase("QMYSQL"); + database_.setHostName("localhost"); + database_.setDatabaseName("opp"); + database_.setUserName("ros-client"); + database_.setPassword("0000"); + if (!database_.open()) + { + ROS_ERROR("Failed to connect to database"); + } + else + { + createJobsTable(); + createPartsTable(); + } + save_dir_ = std::string (std::getenv("HOME")) + "/.local/share/offline_generated_paths"; +} + +ROSDatabaseInterface::~ROSDatabaseInterface() +{ + database_.close(); +} + +QSqlDatabase& ROSDatabaseInterface::getDatabase() +{ + return database_; +} + +bool ROSDatabaseInterface::isConnected() const +{ + return database_.isOpen(); +} + +long int ROSDatabaseInterface::addJobToDatabase(const opp_msgs::Job& job, + std::string& message) +{ + 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"; + bool success = opp_msgs_serialization::serialize(filepath, job.paths); + if (!success) + { + message = "Failed to save tool paths to file at '" + filepath + "'"; + return -1; + } + + values.push_back(boost::lexical_cast(filepath)); + + return insert(JOBS_TABLE_NAME, columns, values, 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 values; + values.push_back(boost::lexical_cast(part.name)); + values.push_back(boost::lexical_cast(part.description)); + values.push_back(boost::lexical_cast(part.mesh_resource)); + + const std::string time = boost::posix_time::to_iso_string(ros::Time::now().toBoost()); + + // touchoff_points + std::string filepath = save_dir_ + "/touch_pts_" + part.name + "_" + time + ".yaml"; + bool success = opp_msgs_serialization::serialize(filepath, part.touch_points); + if (!success) + { + message = "Failed to save touchoff points to file '" + filepath + "'"; + return -1; + } + + values.push_back(boost::lexical_cast(filepath)); + + // verification_points + filepath = save_dir_ + "/verification_points_" + part.name + "_" + time + ".yaml"; + success = opp_msgs_serialization::serialize(filepath, part.verification_points); + if (!success) + { + message = "Failed to save verification points to file '" + filepath + "'"; + return -1; + } + + values.push_back(boost::lexical_cast(filepath)); + + return insert(PARTS_TABLE_NAME, columns, values, message); +} + +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); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + if (result.size() < 1) + { + message = "ID is not available"; + return false; + } + else if (result.size() > 1) + { + message = "Malformed table; multiple jobs share auto-generated ID"; + return false; + } + + result.next(); + job.id = result.value("id").toUInt(); + job.name = result.value("name").toString().toStdString(); + job.description = result.value("description").toString().toStdString(); + job.part_id = result.value("part_id").toUInt(); + + const std::string file_path = result.value("paths").toString().toStdString(); + bool success = opp_msgs_serialization::deserialize(file_path, job.paths); + if (!success) + { + message = "Failed to load tool paths file from '" + file_path + "'"; + return false; + } + + message = "Successfully retrieved job from database"; + return success; +} + +bool ROSDatabaseInterface::getAllJobsFromDatabase(const unsigned int part_id, + std::string& message, + 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); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + if (result.size() < 1) + { + message = "No Jobs in the database for part id: " + std::to_string(part_id); + return true; + } + + jobs.clear(); + while(result.next()) + { + opp_msgs::Job j; + j.id = result.value("id").toUInt(); + j.name = result.value("name").toString().toStdString(); + j.description = result.value("description").toString().toStdString(); + j.part_id = result.value("part_id").toUInt(); + + const std::string filepath = result.value("paths").toString().toStdString(); + bool success = opp_msgs_serialization::deserialize(filepath, j.paths); + if (!success) + { + message = "Failed to load tool path file from '" + filepath + "'"; + return false; + } + + jobs.insert(std::pair(j.id, j)); + } + + message = "Successfully retrieved full jobs for part id '" + std::to_string(part_id) + "' from database"; + return true; +} + +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); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + if (result.size() < 1) + { + message = "ID is not available"; + return false; + } + else if (result.size() > 1) + { + message = "Malformed table; multiple parts share auto-generated ID"; + return false; + } + + result.next(); + part.id = result.value("part_id").toUInt(); + part.name = result.value("name").toString().toStdString(); + part.description = result.value("description").toString().toStdString(); + part.mesh_resource = result.value("mesh_resource").toString().toStdString(); + + const std::string touchoff_pts_filename = result.value("touchoff_points").toString().toStdString(); + bool success = opp_msgs_serialization::deserialize(touchoff_pts_filename, part.touch_points); + if (!success) + { + message = "Failed to load touchoff points from '" + touchoff_pts_filename + "'"; + return false; + } + + const std::string verification_pts_filename = result.value("verification_points").toString().toStdString(); + success = opp_msgs_serialization::deserialize(verification_pts_filename, part.verification_points); + if (!success) + { + message = "Failed to load verification points from '" + verification_pts_filename + "'"; + return false; + } + + message = "Successfully retrieved part from database"; + return success; +} + +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)); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + if (result.size() < 1) + { + message = "No Parts in the database"; + return true; + } + + parts.clear(); + while(result.next()) + { + opp_msgs::Part p; + p.id = result.value("part_id").toUInt(); + p.name = result.value("name").toString().toStdString(); + p.description = result.value("description").toString().toStdString(); + p.mesh_resource = result.value("mesh_resource").toString().toStdString(); + + const std::string touch_pts_filename = result.value("touchoff_points").toString().toStdString(); + bool success = opp_msgs_serialization::deserialize(touch_pts_filename, p.touch_points); + if (!success) + { + message = "Failed to load touchoff points from '" + touch_pts_filename + "'"; + return false; + } + + const std::string verification_pts_filename = result.value("verification_points").toString().toStdString(); + success = opp_msgs_serialization::deserialize(verification_pts_filename, p.verification_points); + if (!success) + { + message = "Failed to load verification points from '" + verification_pts_filename + "'"; + return false; + } + parts.insert(std::pair(p.id, p)); + } + + message = "Successfully retrieved full parts from database"; + return true; +} + +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); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + result.next(); + + message = "Successfully suppressed part in database"; + return true; +} + +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); + QSqlQuery result = database_.exec(script); + + if (!result.isActive()) + { + message = "Database query failed: " + getErrorString(result);; + return false; + } + + result.next(); + + message = "Successfully suppressed job in database"; + return true; +} + +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) + columns_string += QString("`%1`,").arg(QString::fromStdString(column)); + + columns_string.chop(1); + + for (const auto& value : values) + values_string += QString("\"%1\",").arg(QString::fromStdString(value)); + + values_string.chop(1); + + 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()) + { + message = "Succesfully added item to table: " + table_name; + return getLastEntryId(table_name); + } + else + { + message = "Failed to added item to table '" + table_name + "' " + getErrorString(result); + return -1; + } +} + +bool ROSDatabaseInterface::createJobsTable() +{ + QString table_name = QString::fromStdString(JOBS_TABLE_NAME); + QString script = QString("CREATE TABLE `%1` (\ + `id` int(10) unsigned NOT NULL AUTO_INCREMENT,\ + `name` varchar(100) NOT NULL,\ + `description` text NOT NULL,\ + `part_id` int(10) unsigned NOT NULL,\ + `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); + + return createTableHelper(table_name, script); +} + +bool ROSDatabaseInterface::createPartsTable() +{ + QString table_name = QString::fromStdString(PARTS_TABLE_NAME); + QString script = QString("CREATE TABLE `%1` (\ + `part_id` int(10) unsigned NOT NULL AUTO_INCREMENT,\ + `name` varchar(100) NOT NULL,\ + `description` text NOT NULL,\ + `mesh_resource` varchar(200) DEFAULT NULL,\ + `touchoff_points` varchar(200) NOT NULL,\ + `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); + + return createTableHelper(table_name, script); +} + +bool ROSDatabaseInterface::createTableHelper(const QString& table_name, const QString& script) +{ + QStringList tables = database_.tables(); + + // If the table does not exist it is created + if (!tables.contains(table_name)) + { + QSqlQuery result = database_.exec(script); + + if (result.isActive()) + { + return true; + } + else + { + ROS_ERROR_STREAM("Failed to create table: '" + table_name.toStdString() + "' " + getErrorString(result)); + return false; + } + } + + return true; +} + +std::string ROSDatabaseInterface::getErrorString(QSqlQuery& query) const +{ + return "DB Error (" + std::to_string(query.lastError().number()) + "): " + query.lastError().text().toStdString(); +} + +long int ROSDatabaseInterface::getLastEntryId(const std::string& table_name) +{ + QString id_field = "id"; + if (table_name == PARTS_TABLE_NAME) + id_field = "part_id"; + + QString script = QString("SELECT MAX(%1) FROM %2;").arg(id_field).arg(QString::fromStdString(table_name)); + + QSqlQuery result = database_.exec(script); + + if (result.isActive() && result.size() > 0) + { + result.next(); + return result.value(0).toInt(); + } + + return -1; +} + +} diff --git a/opp_database/test/database_interface_cpp_test.cpp b/opp_database/test/database_interface_cpp_test.cpp new file mode 100644 index 0000000..f2111fb --- /dev/null +++ b/opp_database/test/database_interface_cpp_test.cpp @@ -0,0 +1,53 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +TEST(ROSDatabaseInterfaceCppUnit, GetTableLastID) +{ + opp_db::ROSDatabaseInterface database; + long int id = database.getLastEntryId(opp_db::LOC_LOG_TABLE_NAME); + EXPECT_TRUE(id == 7); +} + +TEST(ROSDatabaseInterfaceCppUnit, GetAllJobs) +{ + opp_db::ROSDatabaseInterface database; + const unsigned int part_id = 9999; + std::string message; + std::map jobs; + + EXPECT_TRUE(database.getAllJobsFromDatabase(part_id, message, jobs)); + EXPECT_TRUE(jobs.size() == 2); +} + +TEST(ROSDatabaseInterfaceCppUnit, GetAllParts) +{ + opp_db::ROSDatabaseInterface database; + std::string message; + std::map parts; + + EXPECT_TRUE(database.getAllPartsFromDatabase(message, parts)); + EXPECT_TRUE(parts.size() > 0); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From 20c6b7bd8b8da8b999cfd9f50f5ed0bbfc647d89 Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:06:08 -0600 Subject: [PATCH 05/12] Added GUI package --- opp_gui/CMakeLists.txt | 227 ++++++ opp_gui/README.md | 55 ++ .../opp_gui/register_ros_msgs_for_qt.h | 29 + .../include/opp_gui/tool_path_planner_panel.h | 62 ++ opp_gui/include/opp_gui/utils.h | 70 ++ .../opp_gui/widgets/database_browser_widget.h | 79 +++ .../opp_gui/widgets/database_log_widget.h | 66 ++ .../widgets/database_selection_widget.h | 128 ++++ .../opp_gui/widgets/list_editor_widget_base.h | 74 ++ .../widgets/polygon_area_selection_widget.h | 70 ++ .../segmentation_parameters_editor_widget.h | 101 +++ .../widgets/surface_selection_combo_widget.h | 90 +++ .../opp_gui/widgets/tool_path_editor_widget.h | 109 +++ .../tool_path_parameters_editor_widget.h | 99 +++ .../widgets/tool_path_planner_widget.h | 122 ++++ .../widgets/touch_point_editor_widget.h | 102 +++ .../touch_point_parameters_editor_widget.h | 84 +++ opp_gui/package.xml | 42 ++ opp_gui/plugin_description.xml | 16 + opp_gui/src/nodes/segmentation_demo_app.cpp | 82 +++ .../src/nodes/tool_path_editor_demo_app.cpp | 76 +++ .../src/nodes/tool_path_planner_demo_app.cpp | 45 ++ .../src/nodes/touch_point_editor_demo_app.cpp | 49 ++ opp_gui/src/tool_path_planner_panel.cpp | 52 ++ opp_gui/src/utils.cpp | 241 +++++++ .../src/widgets/database_browser_widget.cpp | 71 ++ opp_gui/src/widgets/database_log_widget.cpp | 69 ++ .../src/widgets/database_selection_widget.cpp | 389 +++++++++++ .../widgets/polygon_area_selection_widget.cpp | 105 +++ .../segmentation_parameters_editor_widget.cpp | 188 +++++ .../surface_selection_combo_widget.cpp | 230 +++++++ .../src/widgets/tool_path_editor_widget.cpp | 298 ++++++++ .../tool_path_parameters_editor_widget.cpp | 228 +++++++ .../src/widgets/tool_path_planner_widget.cpp | 644 ++++++++++++++++++ .../src/widgets/touch_point_editor_widget.cpp | 225 ++++++ .../touch_point_parameters_editor_widget.cpp | 137 ++++ opp_gui/ui/database_browser.ui | 113 +++ opp_gui/ui/database_log.ui | 38 ++ opp_gui/ui/list_editor.ui | 81 +++ opp_gui/ui/polygon_area_selection_widget.ui | 43 ++ opp_gui/ui/segmentation_parameters_editor.ui | 97 +++ opp_gui/ui/surface_selection_combo_widget.ui | 30 + opp_gui/ui/tool_path_parameters_editor.ui | 245 +++++++ opp_gui/ui/tool_path_planner.ui | 568 +++++++++++++++ opp_gui/ui/touch_point_parameters_editor.ui | 253 +++++++ 45 files changed, 6222 insertions(+) create mode 100644 opp_gui/CMakeLists.txt create mode 100644 opp_gui/README.md create mode 100644 opp_gui/include/opp_gui/register_ros_msgs_for_qt.h create mode 100644 opp_gui/include/opp_gui/tool_path_planner_panel.h create mode 100644 opp_gui/include/opp_gui/utils.h create mode 100644 opp_gui/include/opp_gui/widgets/database_browser_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/database_log_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/database_selection_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/list_editor_widget_base.h create mode 100644 opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h create mode 100644 opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h create mode 100644 opp_gui/package.xml create mode 100644 opp_gui/plugin_description.xml create mode 100644 opp_gui/src/nodes/segmentation_demo_app.cpp create mode 100644 opp_gui/src/nodes/tool_path_editor_demo_app.cpp create mode 100644 opp_gui/src/nodes/tool_path_planner_demo_app.cpp create mode 100644 opp_gui/src/nodes/touch_point_editor_demo_app.cpp create mode 100644 opp_gui/src/tool_path_planner_panel.cpp create mode 100644 opp_gui/src/utils.cpp create mode 100644 opp_gui/src/widgets/database_browser_widget.cpp create mode 100644 opp_gui/src/widgets/database_log_widget.cpp create mode 100644 opp_gui/src/widgets/database_selection_widget.cpp create mode 100644 opp_gui/src/widgets/polygon_area_selection_widget.cpp create mode 100644 opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp create mode 100644 opp_gui/src/widgets/surface_selection_combo_widget.cpp create mode 100644 opp_gui/src/widgets/tool_path_editor_widget.cpp create mode 100644 opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp create mode 100644 opp_gui/src/widgets/tool_path_planner_widget.cpp create mode 100644 opp_gui/src/widgets/touch_point_editor_widget.cpp create mode 100644 opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp create mode 100644 opp_gui/ui/database_browser.ui create mode 100644 opp_gui/ui/database_log.ui create mode 100644 opp_gui/ui/list_editor.ui create mode 100644 opp_gui/ui/polygon_area_selection_widget.ui create mode 100644 opp_gui/ui/segmentation_parameters_editor.ui create mode 100644 opp_gui/ui/surface_selection_combo_widget.ui create mode 100644 opp_gui/ui/tool_path_parameters_editor.ui create mode 100644 opp_gui/ui/tool_path_planner.ui create mode 100644 opp_gui/ui/touch_point_parameters_editor.ui diff --git a/opp_gui/CMakeLists.txt b/opp_gui/CMakeLists.txt new file mode 100644 index 0000000..0788bca --- /dev/null +++ b/opp_gui/CMakeLists.txt @@ -0,0 +1,227 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_gui) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + actionlib + eigen_conversions + eigen_stl_containers + geometric_shapes + geometry_msgs + noether_msgs + pluginlib + roscpp + opp_area_selection + opp_msgs + opp_database + rviz + shape_msgs + std_srvs + visualization_msgs +) + +find_package(Qt5 REQUIRED COMPONENTS + Core + Widgets + Sql +) + +find_package(PCL 1.9 REQUIRED) +add_definitions(${PCL_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME}_panels + ${PROJECT_NAME}_utils + ${PROJECT_NAME}_widgets + CATKIN_DEPENDS + actionlib + eigen_conversions + eigen_stl_containers + geometric_shapes + geometry_msgs + noether_msgs + pluginlib + roscpp + opp_area_selection + opp_msgs + opp_database + rviz + shape_msgs + std_srvs + visualization_msgs + DEPENDS + Qt5 + PCL +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${CMAKE_CURRENT_BINARY_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(rpg_gui_UIS + ui/database_browser.ui + ui/list_editor.ui + ui/polygon_area_selection_widget.ui + ui/segmentation_parameters_editor.ui + ui/surface_selection_combo_widget.ui + ui/tool_path_parameters_editor.ui + ui/tool_path_planner.ui + ui/touch_point_parameters_editor.ui + ui/database_log.ui +) + +set(rpg_gui_widget_HEADERS + include/${PROJECT_NAME}/widgets/list_editor_widget_base.h + include/${PROJECT_NAME}/widgets/polygon_area_selection_widget.h + include/${PROJECT_NAME}/widgets/segmentation_parameters_editor_widget.h + include/${PROJECT_NAME}/widgets/surface_selection_combo_widget.h + include/${PROJECT_NAME}/widgets/tool_path_editor_widget.h + include/${PROJECT_NAME}/widgets/tool_path_parameters_editor_widget.h + include/${PROJECT_NAME}/widgets/tool_path_planner_widget.h + include/${PROJECT_NAME}/widgets/touch_point_editor_widget.h + include/${PROJECT_NAME}/widgets/touch_point_parameters_editor_widget.h +) + +set(rpg_gui_widget_SOURCES + src/widgets/polygon_area_selection_widget.cpp + src/widgets/segmentation_parameters_editor_widget.cpp + src/widgets/surface_selection_combo_widget.cpp + src/widgets/tool_path_editor_widget.cpp + src/widgets/tool_path_parameters_editor_widget.cpp + src/widgets/tool_path_planner_widget.cpp + src/widgets/touch_point_editor_widget.cpp + src/widgets/touch_point_parameters_editor_widget.cpp +) + +set(rpg_gui_panel_HEADERS + include/${PROJECT_NAME}/tool_path_planner_panel.h +) + +set(rpg_gui_panel_SOURCES + src/tool_path_planner_panel.cpp +) + +# Run the MOC on the Qt-related UIs and headers +qt5_wrap_ui(rpg_gui_UIS_H + ${rpg_gui_UIS} +) +qt5_wrap_cpp(rpg_gui_widget_MOCS + ${rpg_gui_widget_HEADERS} +) +qt5_wrap_cpp(rpg_gui_panel_MOCS + ${rpg_gui_panel_HEADERS} +) + +# Utilities library +add_library(${PROJECT_NAME}_utils + src/utils.cpp +) +target_link_libraries(${PROJECT_NAME}_utils + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + Qt5::Core + Qt5::Sql +) + +# Qt Widgets +add_library(${PROJECT_NAME}_widgets + ${rpg_gui_UIS_H} + ${rpg_gui_widget_MOCS} + ${rpg_gui_widget_SOURCES} +) +target_link_libraries(${PROJECT_NAME}_widgets + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${PROJECT_NAME}_utils + Qt5::Widgets +) + +# Rviz Panel +add_library(${PROJECT_NAME}_panels + ${rpg_gui_panel_MOCS} + ${rpg_gui_panel_SOURCES} +) +target_link_libraries(${PROJECT_NAME}_panels + ${PROJECT_NAME}_widgets +) + +###################### +## Demo Executables ## +###################### + +# Segmentation +add_executable(segmentation_demo_app + src/nodes/segmentation_demo_app.cpp +) +target_link_libraries(segmentation_demo_app + ${PROJECT_NAME}_widgets +) + +# Tool Path Editor +add_executable(tool_path_editor_demo_app + src/nodes/tool_path_editor_demo_app.cpp +) +target_link_libraries(tool_path_editor_demo_app + ${PROJECT_NAME}_widgets +) + +# Tool Path Planner +add_executable(tool_path_planner_demo_app + src/nodes/tool_path_planner_demo_app.cpp +) +target_link_libraries(tool_path_planner_demo_app + ${PROJECT_NAME}_widgets +) + +# Touch Point Editor +add_executable(touch_point_editor_demo_app + src/nodes/touch_point_editor_demo_app.cpp +) +target_link_libraries(touch_point_editor_demo_app + ${PROJECT_NAME}_widgets +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS + ${PROJECT_NAME}_panels + ${PROJECT_NAME}_utils + ${PROJECT_NAME}_widgets + segmentation_demo_app + tool_path_editor_demo_app + tool_path_planner_demo_app + touch_point_editor_demo_app + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY ui/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ui +) +install( + FILES + plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/opp_gui/README.md b/opp_gui/README.md new file mode 100644 index 0000000..0638318 --- /dev/null +++ b/opp_gui/README.md @@ -0,0 +1,55 @@ +# opp_gui + +This package provides GUI implementations for various applications + +## Tool Path Planner + +The tool path planner GUI is an RViz panel designed to facilitate the setup of new models and tool path +planning on those models. The GUI also hosts an interface to the database for saving this information + +### Usage + +1. Build and source the package +1. Run the application + ``` + roslaunch opp_startup planner_application.launch + ``` +1. Follow the order of operations of the GUI + +## Widget Demos + +Each stand-alone widget has a node for demonstrating its functionality + +### Touch point editor +``` +rosrun opp_gui touch_point_editor_demo_app +rosrun rviz rviz +``` + +To add a point using the mouse follow these steps: + + 1. Add the `Circle Tool Cursor` tool to RViz (Should already be included.) + 2. Click the `Select with Mouse` button in the widget + 3. Activate the `Circle Tool Cursor` tool + 4. Select a location on a mesh in the Rviz render panel + +### Tool path editor + +``` +rosrun noether surface_raster_planner_server +rosrun opp_gui tool_path_editor_demo_app +rosrun rviz rviz +``` + +### Tool path planner +``` +rosrun noether surface_raster_planner_server +rosrun opp_gui tool_path_planner_demo_app +rosrun rviz rviz +``` + +### Database Browser Widget +``` +roslaunch opp_startup opp.launch +rosrun opp_gui database_browser_demo_app +``` 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 new file mode 100644 index 0000000..b3992ed --- /dev/null +++ b/opp_gui/include/opp_gui/register_ros_msgs_for_qt.h @@ -0,0 +1,29 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef REGISTER_ROS_MSGS_FOR_QT_H +#define REGISTER_ROS_MSGS_FOR_QT_H + +#include + +#include + +Q_DECLARE_METATYPE(shape_msgs::Mesh); +Q_DECLARE_METATYPE(shape_msgs::Mesh::Ptr); +Q_DECLARE_METATYPE(std::vector); +Q_DECLARE_METATYPE(std::vector); + +#endif // 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 new file mode 100644 index 0000000..8cf0d82 --- /dev/null +++ b/opp_gui/include/opp_gui/tool_path_planner_panel.h @@ -0,0 +1,62 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_TOOL_PATH_PLANNER_PANEL_H +#define OPP_GUI_TOOL_PATH_PLANNER_PANEL_H + +#include +#include + +namespace opp_gui +{ + +class ToolPathPlannerWidget; + +/** + * @brief Simple RViz panel that wraps the tool path planner widget such that can easily + * be used within the context of RViz + */ +class ToolPathPlannerPanel : public rviz::Panel +{ +Q_OBJECT +public: + + /** + * @brief ToolPathPlannerPanel + * + * Note: the visualization manager object, inherited from `rviz::Panel`, cannot be used in + * the constructor. Even though it is a valid, non-null object, it will not be initialized + * until the constructor has finished. + * @param parent + */ + ToolPathPlannerPanel(QWidget* parent = nullptr); + + /** + * @brief Initializes the TPP widget and uses the visualization manager to gather + * information about the available frames (used to visualize markers). + */ + virtual void onInitialize() override; + +private: + + ToolPathPlannerWidget* tpp_widget_; + + ros::NodeHandle nh_; +}; + +} // namespace opp_gui + +#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 new file mode 100644 index 0000000..ddd1fa2 --- /dev/null +++ b/opp_gui/include/opp_gui/utils.h @@ -0,0 +1,70 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_UTILS_H +#define OPP_GUI_UTILS_H + +#include +#include +#include +#include +#include + +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 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); + +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); + +bool pclMsgToShapeMsg(const pcl_msgs::PolygonMesh& pcl_mesh, shape_msgs::Mesh& mesh_msg); + +bool pclMsgFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl_msgs::PolygonMesh& pcl_mesh); + +} // namespace utils +} // namespace opp_gui + +#endif // OPP_GUI_UTILS_H diff --git a/opp_gui/include/opp_gui/widgets/database_browser_widget.h b/opp_gui/include/opp_gui/widgets/database_browser_widget.h new file mode 100644 index 0000000..b423cbb --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/database_browser_widget.h @@ -0,0 +1,79 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef DATABASE_BROWSER_H +#define DATABASE_BROWSER_H + +#include +#include +#include +#include +#include + +class QTreeWidgetItem; + +namespace Ui +{ +class DatabaseBrowser; +} + +namespace opp_gui +{ + +/** + * @brief Widget that displays all parts and jobs known to the database in a tree hierarchy form + */ +class DatabaseBrowserWidget : public QWidget +{ +Q_OBJECT +public: + + DatabaseBrowserWidget(QWidget* parent = nullptr); + + virtual ~DatabaseBrowserWidget() = default; + +Q_SIGNALS: + + /** + * @brief Signal indicating whether or not the selection in the tree (part or job) has been set + * active in the application. This signal can be used by dependent widgets (namely a higher level + * application GUI) to determine whether they can proceed in their process + * @param status + */ + void okToProceed(bool status); + +protected Q_SLOTS: + + void onTreeItemSelected(QTreeWidgetItem* item); + +protected: + + Ui::DatabaseBrowser* ui_; + + ros::NodeHandle nh_; + ros::ServiceClient get_all_parts_client_; + ros::ServiceClient get_all_jobs_client_; + + std::map part_map_; + std::map job_map_; + + const static int PART_ROLE = Qt::ItemDataRole::UserRole + 1; + const static int JOB_ROLE = Qt::ItemDataRole::UserRole + 2; +}; + +} // namespace opp_gui + +#endif // DATABASE_BROWSER_H diff --git a/opp_gui/include/opp_gui/widgets/database_log_widget.h b/opp_gui/include/opp_gui/widgets/database_log_widget.h new file mode 100644 index 0000000..837971b --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/database_log_widget.h @@ -0,0 +1,66 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef DATABASE_LOG_WIDGET_H +#define DATABASE_LOG_WIDGET_H + +#include +#include +#include +#include +#include +#include +#include // TODO add the Log service to msgs, and change this to Log.h + +namespace Ui { + class DatabaseLog; +} + +class QSqlTableModel; + +namespace opp_gui +{ + +/** + * @brief Widget that displays all recent jobs performed + */ +class DatabaseLogWidget : public QWidget +{ + Q_OBJECT + + public: + + DatabaseLogWidget(opp_application::ApplicationContextBasePtr app, + QWidget *parent = nullptr); + + ~DatabaseLogWidget(); + +protected Q_SLOTS: + void refresh(); + void filter(const QString& text); + +protected: + + Ui::DatabaseLog* ui_; + ros::NodeHandle nh_; + QTimer refresh_timer_; + QSqlTableModel *model_; + + opp_application::ApplicationContextBasePtr app_; +}; +} // end of namespace opp_gui + +#endif // DATABASE_LOG_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/database_selection_widget.h b/opp_gui/include/opp_gui/widgets/database_selection_widget.h new file mode 100644 index 0000000..44f9b7a --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/database_selection_widget.h @@ -0,0 +1,128 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_H +#define OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_H + +#include + +#include + +namespace opp_application +{ +class ApplicationContextBase; +typedef typename std::shared_ptr ApplicationContextBasePtr; +typedef typename boost::function NotifyCbType; +} + +namespace opp_gui +{ + +/** + * @brief Inherits the DatabaseBrowserWidget class and uses its functionality to set the active part + * in the application from the parts available in the database + */ +class PartSelectorWidget : public DatabaseBrowserWidget +{ +Q_OBJECT +public: + + /** + * @brief PartSelectorWidget - Constructor that sets the initial part + * transform to identity + * @param app - input - the application context object being used to + * connect to the underlying ROS system + * @param parent - input - the QT object which is this widget's parent + */ + PartSelectorWidget(opp_application::ApplicationContextBasePtr app, + QWidget* parent = nullptr); + + /** + * @brief PartSelectorWidget - Constructor that sets the initial part + * transform to a calling-code-defined transform + * @param app - input - the application context object being used to + * connect to the underlying ROS system + * @param initial_part_location - input - the position at which parts + * should first appear when added + * @param parent - input - the QT object which is this widget's parent + */ + PartSelectorWidget(opp_application::ApplicationContextBasePtr app, + const geometry_msgs::PoseStamped initial_part_location, + QWidget* parent = nullptr); + + public: + /** &brief the currently selected part's work control document number **/ + std::string selected_part_wcd_; + +protected Q_SLOTS: + + /** @brief a function that is called whenever the 'select part' button + * in this widget is clicked. It adds the part to the list of parts in + * the application context object. + */ + bool onSelectPart(); + /** @brief a function that is called whenever the WID is entered + * It enables the "Select" button so a part is added + */ + void onEnterWID(); + /** @brief loads all parts/jobs into the part selector widget + */ + void onLoadDatabase(); + +private: + + /** @brief the application context ojbect used to connect to the system */ + opp_application::ApplicationContextBasePtr app_; + + /** @brief the position at which parts should first appear */ + geometry_msgs::PoseStamped initial_part_location_; + +}; + +/** + * @brief Inherits the DatabaseBrowserWidget class and uses its functionality to set the active job + * in the application from the jobs available in the database + */ +class JobSelectorWidget : public DatabaseBrowserWidget +{ +Q_OBJECT +public: + + JobSelectorWidget(opp_application::ApplicationContextBasePtr app, + QWidget* parent = nullptr); + /** @brief a function that is called when the active part is chagned + * displays the active parts jobs and hides the jobs for all other parts + */ + void onPartSelect(); +protected Q_SLOTS: + + /** @brief a function that is called whenever the select job button is pressed + */ + void onSelectJob(); + /** @brief loads all parts/jobs into the part selector widget. + * Only shows the entry for the active job + */ + void onLoadDatabase(); + + +private: + + opp_application::ApplicationContextBasePtr app_; +}; + +} // namespace opp_gui + +#endif // OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_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 new file mode 100644 index 0000000..a68d6e4 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/list_editor_widget_base.h @@ -0,0 +1,74 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_LIST_EDITOR_WIDGET_BASE_H +#define OPP_GUI_WIDGETS_LIST_EDITOR_WIDGET_BASE_H + +#include +#include +#include + +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 + * on the right in which to load a widget to display the parameters of the selection + */ +class ListEditorWidgetBase : public QWidget +{ +Q_OBJECT +public: + + ListEditorWidgetBase(QWidget* parent = nullptr) + : QWidget(parent) + { + ui_ = new Ui::ListEditor(); + ui_->setupUi(this); + } + + virtual ~ListEditorWidgetBase() + { + + } + + /** + * @brief Clears the entries in the `QListWidget` display + */ + virtual void clear() + { + ui_->list_widget->clear(); + } + +protected Q_SLOTS: + + virtual void onAddPressed() = 0; + + virtual void onRemovePressed() = 0; + + virtual void onListSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) = 0; + + virtual void onDataChanged() = 0; + +protected: + + Ui::ListEditor* ui_; +}; + +} // namespace opp_gui + +#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 new file mode 100644 index 0000000..d39459f --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/polygon_area_selection_widget.h @@ -0,0 +1,70 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef POLYGON_AREA_SELECTION_WIDGET_H +#define POLYGON_AREA_SELECTION_WIDGET_H + +#include + +#include +#include + +#include +#include + +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); + ~PolygonAreaSelectionWidget(); + +public Q_SLOTS: + void init(const shape_msgs::Mesh& mesh); + +Q_SIGNALS: + void selectedSubmesh(const shape_msgs::Mesh::Ptr& selected_submesh); + +private Q_SLOTS: + void clearROISelection(); + + void applySelection(); + +private: + Ui::PolygonAreaSelectionWidget *ui_; + + shape_msgs::Mesh::Ptr mesh_; + + shape_msgs::Mesh::Ptr submesh_; + + opp_area_selection::SelectionArtist selector_; +}; // end class PolygonAreaSelectionWidget + +} // end namespace opp_gui + +#endif // POLYGON_AREA_SELECTION_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h b/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h new file mode 100644 index 0000000..d2b5aa0 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h @@ -0,0 +1,101 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_SEGMENTATION_PARAMETERS_EDITOR_WIDGET_H +#define OPP_GUI_WIDGETS_SEGMENTATION_PARAMETERS_EDITOR_WIDGET_H + +#include + +#include +#include +#include + +#include + +namespace Ui +{ +class SegmentationParametersEditor; +} + +class QProgressDialog; + +namespace opp_gui +{ +/** + * @brief A widget for segmenting a mesh into its constituent surfaces based on curvature + */ +class SegmentationParametersEditorWidget : public QWidget +{ + Q_OBJECT +public: + SegmentationParametersEditorWidget(QWidget* parent = nullptr); + + /** @brief initialize the widget with the input mesh */ + void init(const pcl_msgs::PolygonMesh& mesh); + + /** @brief initialize the widget with the input mesh */ + void init(const shape_msgs::Mesh& mesh); + + /** @brief Populates the spin boxes based on the segmentation config*/ + void setSegmentationConfig(const noether_msgs::SegmentationConfig& config); + + /** @brief Populates the spin boxes based on the filtering config*/ + void setFilteringConfig(const noether_msgs::FilteringConfig& config); + + /** @brief Create a segmentation config based on the spin boxes */ + noether_msgs::SegmentationConfig getSegmentationConfig() const; + + /** @brief Create a Filtering config based on the spin boxes */ + noether_msgs::FilteringConfig getFilteringConfig() const; + + /** @brief Returns the segmentation results as a vector*/ + std::vector getSegments() const { return segments_; } + + /** @brief Return the segmentation edges. These are the mesh trianges that were not classified into another segment */ + shape_msgs::Mesh::Ptr getEdges() const { return edges_; } + +Q_SIGNALS: + + /** + * @brief Signal emitted when segmentation is finished containing vector of the segments and the edges + */ + void segmentationFinished(const std::vector&, const shape_msgs::Mesh::Ptr&); + +private Q_SLOTS: + + /** @brief Performs segmentation by calling the segmentation action */ + void segmentMesh(); + +private: + void onSegmentMeshComplete(const actionlib::SimpleClientGoalState& state, + const noether_msgs::SegmentResultConstPtr& res); + + actionlib::SimpleActionClient client_; + + Ui::SegmentationParametersEditor* ui_; + + shape_msgs::Mesh::Ptr input_mesh_; + + std::vector segments_; + + shape_msgs::Mesh::Ptr edges_; + + QProgressDialog* progress_dialog_; +}; + +} // namespace opp_gui + +#endif // OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_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 new file mode 100644 index 0000000..563b8be --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/surface_selection_combo_widget.h @@ -0,0 +1,90 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SURFACE_SELECTION_COMBO_WIDGET_H +#define SURFACE_SELECTION_COMBO_WIDGET_H + +#include + +#include + +#include + +#include +#include +#include + +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); + ~SurfaceSelectionComboWidget(); + + void init(const shape_msgs::Mesh& mesh); + + /** @brief Create a segmentation config based on the spin boxes */ + noether_msgs::SegmentationConfig getSegmentationConfig(); + + /** @brief Populates the spin boxes based on the segmentation config*/ + void setSegmentationConfig(const noether_msgs::SegmentationConfig& config); + +Q_SIGNALS: + void newTargetMesh(const shape_msgs::Mesh::Ptr& target_mesh); + +private Q_SLOTS: + void newSegmentList(const std::vector&, const shape_msgs::Mesh::Ptr&); + + void newSelectedSegment(); + + void newSelectedSubmesh(const shape_msgs::Mesh::Ptr& selected_submesh); + +private: + + void publishTargetMesh(); + + Ui::SurfaceSelectionComboWidget* ui_; + + SegmentationParametersEditorWidget* segmenter_; + + PolygonAreaSelectionWidget* area_selector_; + + ros::Publisher selected_area_marker_publisher_; + + shape_msgs::Mesh::Ptr mesh_; + + std::vector segment_list_; + + shape_msgs::Mesh::Ptr selected_area_; + +}; + +} // end namespace opp_gui + +#endif // 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 new file mode 100644 index 0000000..5166a08 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/tool_path_editor_widget.h @@ -0,0 +1,109 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_TOOLPATH_EDITOR_WIDGET_H +#define OPP_GUI_WIDGETS_TOOLPATH_EDITOR_WIDGET_H + +#include +#include // pair + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace opp_gui +{ + +typedef typename std::map ToolPathDataMap; + +class ToolPathParametersEditorWidget; + +/** + * @brief A widget, based on the list editor base class widget, for editing the parameters of + * and generating multiple tool paths + */ +class ToolPathEditorWidget : public ListEditorWidgetBase +{ +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" + ); + + inline virtual void clear() override + { + ListEditorWidgetBase::clear(); + + data_.clear(); + } + + void init(const shape_msgs::Mesh& mesh); + + inline ToolPathDataMap getToolPathData() const + { + return data_; + } + + void addToolPathData(const std::vector& tool_path_list); + + inline void setMarkerFrame(const std::string& frame) + { + marker_frame_ = frame; + } + +protected Q_SLOTS: + + void newTargetMeshSelected(const shape_msgs::Mesh::Ptr& target_mesh); + + virtual void onAddPressed() override; + + virtual void onRemovePressed() override; + + virtual void onListSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) override; + + virtual void onDataChanged() override; + +private: + + void publishToolPathDisplay(const opp_msgs::ToolPath& tool_path); + + SurfaceSelectionComboWidget* surface_selector_; + + ToolPathParametersEditorWidget* editor_; + + ToolPathDataMap data_; + + ros::NodeHandle nh_; + + ros::Publisher pub_; + + std::string marker_frame_; +}; + +} // namespace opp_gui + +#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 new file mode 100644 index 0000000..d714fe9 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h @@ -0,0 +1,99 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H +#define OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H + +#include +#include +#include +#include +#include + +namespace Ui +{ +class ToolPathParametersEditor; +} + +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 +public: + + ToolPathParametersEditorWidget( + ros::NodeHandle& nh, + QWidget* parent = nullptr + ); + + /** + * @brief Sets the internal mesh to be used for path planning and a parameter (not intended to + * be user-facing) that defines the tool path generation process + * @param mesh + * @param intersecting_plane_height + */ + void init(const shape_msgs::Mesh& mesh); + + void setToolPath(const opp_msgs::ToolPath& tool_path); + + bool getToolPath(opp_msgs::ToolPath& tool_path) const; + + void setToolPathConfig(const noether_msgs::ToolPathConfig& config); + + noether_msgs::ToolPathConfig getToolPathConfig() const; + +Q_SIGNALS: + + /** + * @brief Signal emitted when data has changed in one of the editable UI fields + */ + void dataChanged(); + +private Q_SLOTS: + + void updateProcessType(const QString&); + + void updateDwellTime(int value); + + void generateToolPath(); + +private: + + void onGenerateToolPathsComplete(const actionlib::SimpleClientGoalState& state, + const noether_msgs::GenerateToolPathsResultConstPtr& res); + + actionlib::SimpleActionClient client_; + + Ui::ToolPathParametersEditor* ui_; + + opp_msgs::ToolPath::Ptr tool_path_; + + shape_msgs::Mesh::Ptr mesh_; + + QProgressDialog* progress_dialog_; +}; + +} // opp_gui + +#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 new file mode 100644 index 0000000..97719dc --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h @@ -0,0 +1,122 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TOOL_PATH_PLANNER_WIDGET_H +#define TOOL_PATH_PLANNER_WIDGET_H + +#include +#include + +#include +#include +#include + +#include +#include + +class QListWidgetItem; + +namespace Ui +{ +class ToolPathPlanner; +} + +namespace opp_gui +{ + +class TouchPointEditorWidget; +class ToolPathEditorWidget; + +/** + * @brief A widget for facilitating the definition of a new part (including specification of + * geometry, touch points, and verification points) and the generation of tool paths + * associated with that part. This widget interfaces with the application database to save + * the part information for future use. + */ +class ToolPathPlannerWidget : public QWidget +{ +Q_OBJECT +public: + + ToolPathPlannerWidget(QWidget* parent = nullptr, + const ros::NodeHandle& nh = ros::NodeHandle("~"), + const std::vector& frames = {"map"}); + +protected Q_SLOTS: + + void setVisualizationFrame(const QString& text); + + // Parts Page + void browseForMeshResource(); + void loadMeshFromResource(); + + void loadModelsFromDatabase(); + void onModelSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous); + void loadSelectedModel(); + void saveModel(); + + // Jobs Page + void newJob(); + void loadJobsFromDatabase(); + void onJobSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous); + void loadSelectedJob(); + void saveJob(); + + // Database Management Page + void showPartFromDatabase(); + void deletePart(); + void deleteJob(); + void refresh(); + +private: + + void clear(); + + bool loadMesh(); + + void setModelTabsEnabled(bool enabled); + + void setJobTabsEnabled(bool enabled, bool first_enabled = true); + + Ui::ToolPathPlanner* ui_; + + TouchPointEditorWidget* touch_point_editor_; + TouchPointEditorWidget* verification_point_editor_; + ToolPathEditorWidget* tool_path_editor_; + + ros::NodeHandle nh_; + ros::Publisher pub_; + ros::ServiceClient save_model_client_; + ros::ServiceClient save_job_client_; + ros::ServiceClient get_all_models_client_; + + std::string marker_frame_; + + std::vector existing_parts_; + std::vector existing_jobs_; + + std::string mesh_resource_; + uint32_t generated_model_id_; + + opp_db::ROSDatabaseInterface database_; + QSqlTableModel *model_parts_; + QSqlTableModel *model_jobs_; +}; + +} // namespace opp_gui + + +#endif // 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 new file mode 100644 index 0000000..a871b85 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/touch_point_editor_widget.h @@ -0,0 +1,102 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H +#define OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H + +#include +#include +#include +#include +#include +#include +#include + +namespace Ui +{ +class TouchPointParametersEditor; +} + +namespace opp_gui +{ + +class TouchPointParametersEditorWidget; + +/** + * @brief A widget, based on the list editor widget, for editing the definition of multiple + * touch points + */ +class TouchPointEditorWidget : public ListEditorWidgetBase +{ +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 void setMarkerFrame(const std::string& frame) + { + marker_frame_ = frame; + } + + inline virtual void clear() override + { + ListEditorWidgetBase::clear(); + + data_.clear(); + } + + void setMarkerColor(const double r, + const double g, + const double b, + const double a = 1.0); + +protected Q_SLOT: + + virtual void onAddPressed() override; + + virtual void onRemovePressed() override; + + virtual void onListSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) override; + + virtual void onDataChanged() override; + +private: + + TouchPointParametersEditorWidget* point_editor_; + + std::map data_; + + std::string marker_frame_; + + std_msgs::ColorRGBA marker_color_; + + ros::NodeHandle nh_; + + ros::Publisher marker_pub_; +}; + +} // namespace opp_gui + +#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 new file mode 100644 index 0000000..53ba6f5 --- /dev/null +++ b/opp_gui/include/opp_gui/widgets/touch_point_parameters_editor_widget.h @@ -0,0 +1,84 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H +#define OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H + +#include +#include +#include +#include +#include +#include +#include + +namespace Ui +{ +class TouchPointParametersEditor; +} + +namespace opp_gui +{ + +/** + * @brief Widget for editing the parameters associated with a touch-off point + */ +class TouchPointParametersEditorWidget : public QWidget +{ +Q_OBJECT +public: + + TouchPointParametersEditorWidget(QWidget* parent = nullptr); + + opp_msgs::TouchPoint getTouchPoint() const; + + void setTouchPoint(const opp_msgs::TouchPoint& tp); + +Q_SIGNALS: + + /** + * @brief Signal emitted when data has changed in one of the editable UI fields + */ + void dataChanged(); + +protected Q_SLOTS: + + /** + * @brief Callback that allows the widget to listen for pose data from an external topic + * (i.e. from RViz) with which to set the touch point position and orientation fields + */ + void onSelectWithMouse(); + +private: + + void setPose(const geometry_msgs::Pose& pose); + + geometry_msgs::Pose getPose() const; + + void callback(const geometry_msgs::PoseStampedConstPtr& msg); + + Ui::TouchPointParametersEditor* ui_; + + ros::NodeHandle nh_; + + ros::Subscriber sub_; + + std::atomic accept_mouse_input_; +}; + +} // namespace opp_gui + +#endif // OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H diff --git a/opp_gui/package.xml b/opp_gui/package.xml new file mode 100644 index 0000000..859cf71 --- /dev/null +++ b/opp_gui/package.xml @@ -0,0 +1,42 @@ + + + opp_gui + 0.0.0 + + The opp_gui package contains the various files for the on- and + off-line guis. The .ui files contain the visual layout, and are used + to generate basic QT objects. The .h and .cpp files specify how those + basic QT objects interact with each other and the rest of the system. + + Most of the direct interaction with the ROS system is actually handled + by the classes in the opp_application package. The GUI objects + own a copy of an application object, and pass user input down into the + ROS system. + + + mripperger + mripperger + + Apache 2.0 + + catkin + actionlib + eigen_conversions + eigen_stl_containers + geometric_shapes + geometry_msgs + noether_msgs + pluginlib + roscpp + opp_area_selection + opp_msgs + opp_database + rviz + shape_msgs + std_srvs + visualization_msgs + + + + + diff --git a/opp_gui/plugin_description.xml b/opp_gui/plugin_description.xml new file mode 100644 index 0000000..dc69c60 --- /dev/null +++ b/opp_gui/plugin_description.xml @@ -0,0 +1,16 @@ + + + + A panel for creating registration and tool path data for new models + + + + + A panel for operating the chemical depaint process + + + diff --git a/opp_gui/src/nodes/segmentation_demo_app.cpp b/opp_gui/src/nodes/segmentation_demo_app.cpp new file mode 100644 index 0000000..5ce691d --- /dev/null +++ b/opp_gui/src/nodes/segmentation_demo_app.cpp @@ -0,0 +1,82 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include + +int main(int argc, char** argv) +{ + // Setup ROS Node + ros::init(argc, argv, "segmentation_demo_app"); + ros::NodeHandle nh, pnh("~"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Import the mesh + std::string filename; + bool save_outputs; + pnh.param("filename", filename, ""); + pnh.param("save_outputs", save_outputs, false); + pcl::PolygonMesh pcl_mesh; + pcl::io::loadPolygonFile(filename, pcl_mesh); + ROS_INFO_STREAM("Imported as PCL mesh of size " << pcl_mesh.cloud.data.size() << '\n'); + + // Convert to ROS message + pcl_msgs::PolygonMesh pcl_mesh_msg; + pcl_conversions::fromPCL(pcl_mesh, pcl_mesh_msg); + + // Create and start the Qt application + QApplication app(argc, argv); + + // Start the widget + opp_gui::SegmentationParametersEditorWidget* segmentation_widget = new opp_gui::SegmentationParametersEditorWidget(nullptr); + segmentation_widget->show(); + segmentation_widget->init(pcl_mesh_msg); + + app.exec(); + + // Save the meshes + auto segmented_meshes = segmentation_widget->getSegments(); + if (save_outputs) + { + for (int ind = 0; ind < segmented_meshes.size(); ind++) + { + ROS_INFO_STREAM("Saving: " << ind << "\n"); + + std::ostringstream ss; + ss << ind; + 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); + pcl::PolygonMesh pcl_mesh; + pcl_conversions::toPCL(pcl_mesh_msg, pcl_mesh); + pcl::io::savePolygonFile(filename, pcl_mesh); + } + } + + ros::waitForShutdown(); + + delete segmentation_widget; + + return 0; +} diff --git a/opp_gui/src/nodes/tool_path_editor_demo_app.cpp b/opp_gui/src/nodes/tool_path_editor_demo_app.cpp new file mode 100644 index 0000000..9140904 --- /dev/null +++ b/opp_gui/src/nodes/tool_path_editor_demo_app.cpp @@ -0,0 +1,76 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +template +bool get(const ros::NodeHandle& nh, + const std::string& key, + T& val) +{ + if(!nh.getParam(key, val)) + { + ROS_ERROR_STREAM("Failed to get '" << key << "' parameter"); + return false; + } + return true; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "touch_point_editor_demo_app"); + ros::NodeHandle nh, pnh("~"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::string mesh_resource; + if(!get(pnh, "mesh_resource", mesh_resource)) return -1; + + double intersecting_plane_height; + pnh.param("intersecting_plane_height", intersecting_plane_height, 0.1); + + std::string fixed_frame; + pnh.param("fixed_frame", fixed_frame, "map"); + ROS_INFO_STREAM("Using fixed frame '" << fixed_frame << "' for displays"); + + // Create the mesh message from the input resource + shape_msgs::Mesh mesh_msg; + 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); + tool_path_editor_widget->init(mesh_msg); + tool_path_editor_widget->show(); + + app.exec(); + + // 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) + { + ROS_INFO_STREAM("Tool Path: " << pair.first << "\nTool Path (including config):\n" << pair.second); + } + + delete tool_path_editor_widget; + + return 0; +} diff --git a/opp_gui/src/nodes/tool_path_planner_demo_app.cpp b/opp_gui/src/nodes/tool_path_planner_demo_app.cpp new file mode 100644 index 0000000..00f6ca5 --- /dev/null +++ b/opp_gui/src/nodes/tool_path_planner_demo_app.cpp @@ -0,0 +1,45 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "tool_path_planner_demo_app"); + ros::NodeHandle nh, pnh("~"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::string fixed_frame; + pnh.param("fixed_frame", fixed_frame, "map"); + ROS_INFO_STREAM("Using fixed frame '" << fixed_frame << "' for displays"); + + // Create and start the Qt application + QApplication app(argc, argv); + + opp_gui::ToolPathPlannerWidget* tpp_widget = new opp_gui::ToolPathPlannerWidget(nullptr, nh, {fixed_frame}); + tpp_widget->show(); + + app.exec(); + + delete tpp_widget; + + return 0; +} diff --git a/opp_gui/src/nodes/touch_point_editor_demo_app.cpp b/opp_gui/src/nodes/touch_point_editor_demo_app.cpp new file mode 100644 index 0000000..f2ca2af --- /dev/null +++ b/opp_gui/src/nodes/touch_point_editor_demo_app.cpp @@ -0,0 +1,49 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "touch_point_editor_demo_app"); + ros::NodeHandle nh, pnh("~"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::string fixed_frame; + pnh.param("fixed_frame", fixed_frame, "map"); + ROS_INFO_STREAM("Using fixed frame '" << fixed_frame << "' for displays"); + + QApplication app(argc, argv); + + 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) + { + ROS_INFO_STREAM("Touch Point: " << pair.first << "\nInfo:\n" << pair.second); + } + + delete touch_point_editor_widget; + + return 0; +} diff --git a/opp_gui/src/tool_path_planner_panel.cpp b/opp_gui/src/tool_path_planner_panel.cpp new file mode 100644 index 0000000..b111626 --- /dev/null +++ b/opp_gui/src/tool_path_planner_panel.cpp @@ -0,0 +1,52 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include +#include +#include + +namespace opp_gui +{ + +ToolPathPlannerPanel::ToolPathPlannerPanel(QWidget* parent) + : rviz::Panel(parent) +{ + +} + +void ToolPathPlannerPanel::onInitialize() +{ + // Add the current fixed frame to the list if it doesn't exist in the frames that TF knows about + std::vector frames; + std::string fixed_frame = vis_manager_->getFixedFrame().toStdString(); + frames.insert(frames.begin(), fixed_frame); + + tpp_widget_ = new ToolPathPlannerWidget(this, nh_, frames); + + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(tpp_widget_); + + setLayout(layout); +} + +} // 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 new file mode 100644 index 0000000..04f0147 --- /dev/null +++ b/opp_gui/src/utils.cpp @@ -0,0 +1,241 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace opp_gui +{ +namespace utils +{ + +namespace vm = visualization_msgs; + +bool getMeshMsgFromResource(const std::string& resource, + shape_msgs::Mesh& mesh_msg) +{ + shapes::Mesh* mesh = shapes::createMeshFromResource(resource); + if(!mesh) + { + ROS_ERROR_STREAM("Failed to load mesh from resource: '" << resource << "'"); + return false; + } + + shapes::ShapeMsg shape_msg; + if(!shapes::constructMsgFromShape(mesh, shape_msg)) + { + ROS_ERROR_STREAM(__func__ << ": Failed to create shape message from mesh"); + return false; + } + + mesh_msg = boost::get(shape_msg); + + return true; +} + +std_msgs::ColorRGBA createColor(const float r, + const float g, + const float b, + const float a) +{ + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +vm::Marker createArrowMarker(const int id, + const std::string& ns, + const Eigen::Isometry3d& pose, + const std::string& frame, + const std_msgs::ColorRGBA& color, + const vm::Marker::_action_type& action) +{ + vm::Marker marker; + + marker.header.frame_id = frame; + marker.id = id; + marker.ns = ns; + marker.action = action; + marker.color = color; + + marker.type = vm::Marker::ARROW; + double length = 0.25; + + geometry_msgs::Point head; + tf::pointEigenToMsg(pose.matrix().col(3).head<3>(), head); + + geometry_msgs::Point tail; + tf::pointEigenToMsg(pose * Eigen::Vector3d(0.0, 0.0, -length), tail); + + marker.points.reserve(2); + marker.points.push_back(tail); + marker.points.push_back(head); + + marker.scale.x = length / 10.0; + marker.scale.y = length * 2.0 / 10.0; + marker.scale.z = length * 2.0 / 10.0; + + return marker; +} + +vm::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, + const vm::Marker::_action_type& action) +{ + vm::Marker marker; + + marker.header.frame_id = frame; + marker.id = id; + marker.ns = ns; + marker.action = action; + + marker.type = vm::Marker::MESH_RESOURCE; + marker.mesh_resource = mesh_resource; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + + marker.color = color; + + tf::poseEigenToMsg(pose, marker.pose); + + return marker; +} + +QStringList toQStringList(const std::vector& in) +{ + QStringList out; + out.reserve(in.size()); + 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 + pcl::PolygonMesh pcl_mesh; + pcl_conversions::toPCL(pcl_mesh_msg, pcl_mesh); + + // Make sure that there are at least three points and at least one polygon + if (pcl_mesh.cloud.height * pcl_mesh.cloud.width < 3 || pcl_mesh.polygons.size() < 1) + { + return false; + } + + // Prepare the message's vectors to receive data + // One resize now saves time later + mesh_msg.vertices.resize(pcl_mesh.cloud.height * pcl_mesh.cloud.width); + mesh_msg.triangles.resize(pcl_mesh.polygons.size()); + + // Get the points from the pcl mesh + pcl::PointCloud vertices; + pcl::fromPCLPointCloud2(pcl_mesh.cloud, vertices); + + // Copy the coordinates inside the vertices into the new mesh + // TODO: Maybe check for nan? + for (std::size_t i = 0; i < vertices.size(); ++i) + { + mesh_msg.vertices[i].x = static_cast(vertices[i]._PointXYZ::x); + mesh_msg.vertices[i].y = static_cast(vertices[i]._PointXYZ::y); + mesh_msg.vertices[i].z = static_cast(vertices[i]._PointXYZ::z); + } + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < pcl_mesh.polygons.size(); ++i) + { + // If a 'polygon' in the old mesh did not have 3 or more vertices, + // throw an error. (If the polygon has 4 or more, just use the first + // 3 to make a triangle.) + // TODO: It is possible to decompose any polygon into multiple + // triangles, and that would prevent loss of data here. + if (pcl_mesh.polygons[i].vertices.size() < 3) + { + return false; + } + mesh_msg.triangles[i].vertex_indices[0] = pcl_mesh.polygons[i].vertices[0]; + mesh_msg.triangles[i].vertex_indices[1] = pcl_mesh.polygons[i].vertices[1]; + mesh_msg.triangles[i].vertex_indices[2] = pcl_mesh.polygons[i].vertices[2]; + } + + return true; +} + +bool pclMsgFromShapeMsg(const shape_msgs::Mesh& mesh_msg, pcl_msgs::PolygonMesh& pcl_mesh_msg) +{ + pcl::PolygonMesh pcl_mesh; + + // Make sure that there are at least three points and at least one polygon + if (mesh_msg.vertices.size() < 3 || mesh_msg.triangles.size() < 1) + { + return false; + } + + // Prepare PCL structures to receive data + // Resizing once now saves time later + pcl::PointCloud vertices; + vertices.resize(mesh_msg.vertices.size()); + pcl_mesh.polygons.resize(mesh_msg.triangles.size()); + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < mesh_msg.vertices.size(); ++i) + { + vertices[i]._PointXYZ::x = static_cast(mesh_msg.vertices[i].x); + vertices[i]._PointXYZ::y = static_cast(mesh_msg.vertices[i].y); + vertices[i]._PointXYZ::z = static_cast(mesh_msg.vertices[i].z); + } + + // Copy the vertex indices (which describe each polygon) from + // the old mesh to the new mesh + for (std::size_t i = 0; i < mesh_msg.triangles.size(); ++i) + { + pcl_mesh.polygons[i].vertices.resize(3); + pcl_mesh.polygons[i].vertices[0] = mesh_msg.triangles[i].vertex_indices[0]; + pcl_mesh.polygons[i].vertices[1] = mesh_msg.triangles[i].vertex_indices[1]; + pcl_mesh.polygons[i].vertices[2] = mesh_msg.triangles[i].vertex_indices[2]; + } + + // Use the filled pointcloud to populate the pcl mesh + pcl::toPCLPointCloud2(vertices, pcl_mesh.cloud); + + // Convert to message + pcl_conversions::fromPCL(pcl_mesh, pcl_mesh_msg); + + return true; +} + +} // namespace utils +} // namespace opp_gui diff --git a/opp_gui/src/widgets/database_browser_widget.cpp b/opp_gui/src/widgets/database_browser_widget.cpp new file mode 100644 index 0000000..0e8dd6a --- /dev/null +++ b/opp_gui/src/widgets/database_browser_widget.cpp @@ -0,0 +1,71 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include "ui_database_browser.h" + +const static std::string GET_ALL_PARTS_SERVICE = "get_all_parts_from_db"; +const static std::string GET_ALL_JOBS_SERVICE = "get_all_jobs_from_db"; + +namespace opp_gui +{ + +DatabaseBrowserWidget::DatabaseBrowserWidget(QWidget* parent) + : QWidget(parent) +{ + ui_ = new Ui::DatabaseBrowser(); + ui_->setupUi(this); + + // Disable the jobs pane until a part selection has been made + ui_->tree_widget_db_entries->setEnabled(false); + ui_->text_edit_description->setEnabled(false); + ui_->push_button_select->setEnabled(false); + + connect(ui_->tree_widget_db_entries, &QTreeWidget::itemClicked, this, &DatabaseBrowserWidget::onTreeItemSelected); + + get_all_parts_client_ = nh_.serviceClient(GET_ALL_PARTS_SERVICE); + get_all_jobs_client_ = nh_.serviceClient(GET_ALL_JOBS_SERVICE); +} + + +void DatabaseBrowserWidget::onTreeItemSelected(QTreeWidgetItem* item) +{ + if(item) + { + QVariant part_data = item->data(0, PART_ROLE); + auto part_it = part_map_.find(qvariant_cast(part_data)); + + if(part_it != part_map_.end()) + { + ui_->text_edit_description->setText(QString::fromStdString(part_it->second.description)); + } + + QVariant job_data = item->data(0, JOB_ROLE); + auto job_it = job_map_.find(qvariant_cast(job_data)); + + if(job_it != job_map_.end()) + { + ui_->text_edit_description->setText(QString::fromStdString(job_it->second.description)); + } + } + + emit okToProceed(false); +} + +} // namespace opp_gui diff --git a/opp_gui/src/widgets/database_log_widget.cpp b/opp_gui/src/widgets/database_log_widget.cpp new file mode 100644 index 0000000..8aa29e8 --- /dev/null +++ b/opp_gui/src/widgets/database_log_widget.cpp @@ -0,0 +1,69 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "opp_gui/widgets/database_log_widget.h" +#include "ui_database_log.h" +#include +#include +#include + +namespace opp_gui +{ + +DatabaseLogWidget::DatabaseLogWidget(opp_application::ApplicationContextBasePtr app, + QWidget *parent) : + QWidget(parent) + , app_(app) +{ + ui_ = new Ui::DatabaseLog(); + ui_->setupUi(this); + + model_ = new QSqlTableModel(this, app_->database_.getDatabase()); + model_->setTable(QString::fromStdString(opp_db::JOB_LOG_TABLE_NAME)); + model_->setEditStrategy(QSqlTableModel::OnManualSubmit); + model_->select(); + model_->removeColumn(0); // don't show the ID + + connect(&refresh_timer_, SIGNAL(timeout()), this, SLOT(refresh())); + refresh_timer_.setInterval(10000); + refresh_timer_.start(); + + connect(ui_->line_edit_wcd_entry, &QLineEdit::textChanged, this, &DatabaseLogWidget::filter); + + // Attach it to the view + ui_->log_table_view->setModel(model_); + ui_->log_table_view->setEditTriggers(QTableView::NoEditTriggers); // Make read only + ui_->log_table_view->show(); +} + +DatabaseLogWidget::~DatabaseLogWidget() +{ + delete ui_; +} + +void DatabaseLogWidget::refresh() +{ + model_->select(); + refresh_timer_.start(); +} + +void DatabaseLogWidget::filter(const QString& text) +{ + std::string query = "`work control document number` LIKE \"" + text.toStdString() + "%\""; + model_->setFilter(QString::fromStdString(query)); +} + +}// end namespace opp_gui diff --git a/opp_gui/src/widgets/database_selection_widget.cpp b/opp_gui/src/widgets/database_selection_widget.cpp new file mode 100644 index 0000000..181340a --- /dev/null +++ b/opp_gui/src/widgets/database_selection_widget.cpp @@ -0,0 +1,389 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include "ui_database_browser.h" + +namespace opp_gui +{ + +// Constructor that uses default initial-transform value +PartSelectorWidget::PartSelectorWidget(opp_application::ApplicationContextBasePtr app, + QWidget* parent) + : DatabaseBrowserWidget(parent) + , app_(app), + selected_part_wcd_("") +{ + // Initialize the initial_part_location transform to identity + initial_part_location_.pose.orientation.w = 1.0; + + // Connect the button + ui_->push_button_select->setText("Add Part"); + ui_->tree_widget_db_entries->setRootIsDecorated(false); + ui_->WorkCD_lineEdit->setReadOnly(false); + connect(ui_->push_button_update, &QPushButton::clicked, this, &PartSelectorWidget::onLoadDatabase); + connect(ui_->push_button_select, &QPushButton::clicked, this, &PartSelectorWidget::onSelectPart); + + // Connect the lineEdit + connect(ui_->WorkCD_lineEdit, &QLineEdit::editingFinished, this, &PartSelectorWidget::onEnterWID); + ui_->WCD_Label->setText("Enter WCD"); +} + +// Constructor that specifies the initial transform +PartSelectorWidget::PartSelectorWidget(opp_application::ApplicationContextBasePtr app, + const geometry_msgs::PoseStamped initial_part_location, + QWidget* parent) + : DatabaseBrowserWidget(parent) + , app_(app) + , initial_part_location_(initial_part_location), + selected_part_wcd_("") +{ + // Connect the button + ui_->push_button_select->setText("Add Part"); + ui_->tree_widget_db_entries->setRootIsDecorated(false); + connect(ui_->push_button_update, &QPushButton::clicked, this, &PartSelectorWidget::onLoadDatabase); + connect(ui_->push_button_select, &QPushButton::clicked, this, &PartSelectorWidget::onSelectPart); + + // Connect the lineEdit + connect(ui_->WorkCD_lineEdit, &QLineEdit::editingFinished, this, &PartSelectorWidget::onEnterWID); +} + +bool PartSelectorWidget::onSelectPart() +{ + selected_part_wcd_ = ui_->WorkCD_lineEdit->text().toStdString(); + if(selected_part_wcd_ == ""){ + QMessageBox MB; + MB.setText("No Work ID entered"); + MB.exec(); + return false; + } + QTreeWidgetItem* current = ui_->tree_widget_db_entries->currentItem(); + if(current) + { + // Define a lambda for attempting to set the current part + auto try_set_part = [this](QVariant& part_data)->bool + { + auto part_it = part_map_.find(qvariant_cast(part_data)); + + if(part_it != part_map_.end()) + { + // Set the active part + opp_application::TaskResponse res = app_->addPart(part_it->second, initial_part_location_, selected_part_wcd_); + if(res.success) + { + emit okToProceed(true); + ui_->WorkCD_lineEdit->setText(""); + return true; + } + else + { + QMessageBox::warning(this, "Application Error", QString::fromStdString(res.message)); + } + } + + return false; + }; + + // Get the data from the current selection + QVariant part_data = current->data(0, PART_ROLE); + + // Try to set the active part in the application + if(try_set_part(part_data)) + { + ui_->WorkCD_lineEdit->setText(""); + return true; + } + else + { + /* Maybe the user has selected a job; get the current selection's parent (which may be a part) + * and try to set it as the active part */ + QTreeWidgetItem* part = current->parent(); + + // Make sure the current selection has a parent + if(part) + { + part_data = part->data(0, PART_ROLE); + if(try_set_part(part_data)) + { + ui_->WorkCD_lineEdit->setText(""); + return true; + } + } + else + { + QMessageBox::warning(this, "Widget Error", "The current selection does not have a parent"); + } + } + } + else + { + QMessageBox::warning(this, "Input Error", "Please select a part in the tree"); + } + + emit okToProceed(false); +} + +void PartSelectorWidget::onEnterWID() +{ + // TODO add something here if necessary or delete. This function was created for debugging, but no longer used +} + +void PartSelectorWidget::onLoadDatabase() +{ + ui_->tree_widget_db_entries->clear(); + part_map_.clear(); + job_map_.clear(); + + opp_msgs::GetAllPartsFromDatabase srv; + if(!get_all_parts_client_.call(srv)) + { + std::string message = "Failed to call '" + get_all_parts_client_.getService() + "' service"; + QMessageBox::warning(this, "ROS Communication Error", QString::fromStdString(message)); + return; + } + else if(!srv.response.success) + { + QMessageBox::warning(this, "Database Error", QString::fromStdString(srv.response.message)); + return; + } + + // Error message + std::string job_load_error; + // Iterate over the parts and add the jobs associated with each one + ui_->tree_widget_db_entries->setSortingEnabled(false); + for(const opp_msgs::Part& part : srv.response.parts) + { + // Add the current part to the persistent part map + part_map_.emplace(part.id, part); + + // Add a QTreeWidgetItem for the part + QTreeWidgetItem* q_part = new QTreeWidgetItem(); + q_part->setText(0, QString::fromStdString(part.name)); + q_part->setData(0, PART_ROLE, QVariant(part.id)); + + // Call the service to get the jobs for this part from the database + opp_msgs::GetAllJobsFromDatabase srv; + srv.request.part_id = part.id; + + if(!get_all_jobs_client_.call(srv)) + { + job_load_error += part.name + ": Failed to call '" + get_all_jobs_client_.getService() + "' service\n"; + } + else if(!srv.response.success) + { + job_load_error += part.name + "\n"; + } + else + { + // Iterate over all of the jobs associated with this part and add them as children to the tree visualization + for(const opp_msgs::Job& job : srv.response.jobs) + { + job_map_.emplace(job.id, job); + + QTreeWidgetItem* q_job = new QTreeWidgetItem(); + q_job->setText(0, QString::fromStdString(job.name)); + q_job->setData(0, JOB_ROLE, QVariant(job.id)); + + q_part->addChild(q_job); + } + } + + // Add the part and its children jobs to the tree widget + ui_->tree_widget_db_entries->addTopLevelItem(q_part); + } + ui_->tree_widget_db_entries->setSortingEnabled(true); + + // Enable the panes + ui_->tree_widget_db_entries->setEnabled(true); + ui_->text_edit_description->setEnabled(true); + ui_->push_button_select->setEnabled(true); + + if(!job_load_error.empty()) + { + std::string message = "Failed to load jobs for the following parts:\n\n" + job_load_error; + QMessageBox::warning(this, "Database Error", QString::fromStdString(message)); + } + + emit okToProceed(false); +} + + +JobSelectorWidget::JobSelectorWidget(opp_application::ApplicationContextBasePtr app, + QWidget* parent) + : DatabaseBrowserWidget(parent) + , app_(app) +{ + ui_->push_button_select->setText("Select Job"); + ui_->WorkCD_lineEdit->setText(app_->active_part_->second.work_control_document.c_str()); + ui_->WCD_Label->setText("Active WCD"); + ui_->WorkCD_lineEdit->setReadOnly(true); + connect(ui_->push_button_update, &QPushButton::clicked, this, &JobSelectorWidget::onLoadDatabase); + connect(ui_->push_button_select, &QPushButton::clicked, this, &JobSelectorWidget::onSelectJob); +} + +void JobSelectorWidget::onSelectJob() +{ + QTreeWidgetItem* current = ui_->tree_widget_db_entries->currentItem(); + if(current) + { + QVariant job_data = current->data(0, JOB_ROLE); + auto job_it = job_map_.find(qvariant_cast(job_data)); + + if(job_it != job_map_.end()) + { + // Set the active job + ui_->WorkCD_lineEdit->setText(app_->active_part_->second.work_control_document.c_str()); + opp_application::TaskResponse res = app_->setActiveJob(job_it->second); + if(res.success) + { + emit okToProceed(true); + return; + } + else + { + QMessageBox::warning(this, "Application Error", QString::fromStdString(res.message)); + } + } + else + { + QMessageBox::warning(this, "Input Error", "Selection is not a known job"); + } + } + else + { + QMessageBox::warning(this, "Input Error", "Please select a job in the tree"); + } + + emit okToProceed(false); +} + +void JobSelectorWidget::onPartSelect() +{ + int topCount_ = ui_->tree_widget_db_entries->topLevelItemCount(); + for (int i = 0; i < topCount_; i++) + { + QTreeWidgetItem* current = ui_->tree_widget_db_entries->topLevelItem(i); + if(current) + { + QVariant part_data = current->data(0, PART_ROLE); + auto part_it = part_map_.find(qvariant_cast(part_data)); + + if(app_->active_part_->second.part.id == part_it->second.id) + { + current->setHidden(false); + current->setExpanded(true); + } + else + { + current->setHidden(true); + } + } + } +} + +void JobSelectorWidget::onLoadDatabase() +{ + ui_->tree_widget_db_entries->clear(); + part_map_.clear(); + job_map_.clear(); + + opp_msgs::GetAllPartsFromDatabase srv; + if(!get_all_parts_client_.call(srv)) + { + std::string message = "Failed to call '" + get_all_parts_client_.getService() + "' service"; + QMessageBox::warning(this, "ROS Communication Error", QString::fromStdString(message)); + return; + } + else if(!srv.response.success) + { + QMessageBox::warning(this, "Database Error", QString::fromStdString(srv.response.message)); + return; + } + + // Error message + std::string job_load_error; + // Iterate over the parts and add the jobs associated with each one + ui_->tree_widget_db_entries->setSortingEnabled(false); + for(const opp_msgs::Part& part : srv.response.parts) + { + // Add the current part to the persistent part map + part_map_.emplace(part.id, part); + + // Add a QTreeWidgetItem for the part + QTreeWidgetItem* q_part = new QTreeWidgetItem(); + q_part->setText(0, QString::fromStdString(part.name)); + q_part->setData(0, PART_ROLE, QVariant(part.id)); + + // Call the service to get the jobs for this part from the database + opp_msgs::GetAllJobsFromDatabase srv; + srv.request.part_id = part.id; + + if(!get_all_jobs_client_.call(srv)) + { + job_load_error += part.name + ": Failed to call '" + get_all_jobs_client_.getService() + "' service\n"; + } + else if(!srv.response.success) + { + job_load_error += part.name + "\n"; + } + else + { + // Iterate over all of the jobs associated with this part and add them as children to the tree visualization + for(const opp_msgs::Job& job : srv.response.jobs) + { + job_map_.emplace(job.id, job); + + QTreeWidgetItem* q_job = new QTreeWidgetItem(); + q_job->setText(0, QString::fromStdString(job.name)); + q_job->setData(0, JOB_ROLE, QVariant(job.id)); + + q_part->addChild(q_job); + } + } + // Add the part and its children jobs to the tree widget + ui_->tree_widget_db_entries->addTopLevelItem(q_part); + QTreeWidgetItem* current = ui_->tree_widget_db_entries->topLevelItem(ui_->tree_widget_db_entries->topLevelItemCount()-1); + if(part.id != app_->active_part_->second.part.id) + { + current->setHidden(true); + } + else + { + current->setExpanded(true); + current->setHidden(false); + } + } + ui_->tree_widget_db_entries->setSortingEnabled(true); + + // Enable the panes + ui_->tree_widget_db_entries->setEnabled(true); + ui_->text_edit_description->setEnabled(true); + ui_->push_button_select->setEnabled(true); + + if(!job_load_error.empty()) + { + std::string message = "Failed to load jobs for the following parts:\n\n" + job_load_error; + QMessageBox::warning(this, "Database Error", QString::fromStdString(message)); + } + + emit okToProceed(false); +} +} // 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 new file mode 100644 index 0000000..8b8de31 --- /dev/null +++ b/opp_gui/src/widgets/polygon_area_selection_widget.cpp @@ -0,0 +1,105 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include + +#include + +#include "ui_polygon_area_selection_widget.h" + +namespace opp_gui +{ + +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); +} + +PolygonAreaSelectionWidget::~PolygonAreaSelectionWidget() +{ + delete ui_; +} + +void PolygonAreaSelectionWidget::init(const shape_msgs::Mesh& mesh) +{ + mesh_.reset(new shape_msgs::Mesh(mesh)); + clearROISelection(); + return; +} + +void PolygonAreaSelectionWidget::clearROISelection() +{ + std_srvs::Trigger srv; + // Currently, this callback is being called directly, so it will always return true. + bool success = selector_.clearROIPointsCb(srv.request, srv.response); + if (!success) + { + ROS_ERROR("Tool Path Parameter Editor Widget: Area Selection error: could not clear polygon points"); + } + if (!srv.response.success) + { + ROS_ERROR_STREAM("Tool Path Parameter Editor Widget: Area Selection error:" << srv.response.message); + } + submesh_.reset(new shape_msgs::Mesh (*mesh_)); + + emit(selectedSubmesh(submesh_)); + return; +} + +void PolygonAreaSelectionWidget::applySelection() +{ + if (!mesh_) + { + QMessageBox::warning(this, "Tool Path Planning Error", "No mesh available to crop"); + return; + } + submesh_.reset(new shape_msgs::Mesh()); + std::string error_message; + 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); + } + if (submesh_->vertices.size() < 3 || submesh_->triangles.size() < 1) + { + submesh_.reset(new shape_msgs::Mesh (*mesh_)); + } + + emit(selectedSubmesh(submesh_)); + return; +} + +} // 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 new file mode 100644 index 0000000..61bdddb --- /dev/null +++ b/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp @@ -0,0 +1,188 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "ui_segmentation_parameters_editor.h" +#include + +const static std::string SEGMENTATION_ACTION = "/mesh_segmenter_server_node/segmenter/"; + +namespace opp_gui +{ +SegmentationParametersEditorWidget::SegmentationParametersEditorWidget(QWidget* parent) + : QWidget(parent), client_(SEGMENTATION_ACTION, true) +{ + ui_ = new Ui::SegmentationParametersEditor(); + ui_->setupUi(this); + + // Parameter boxes + ui_->spin_box_min_cluster_size->setRange(0, std::numeric_limits::max()); + ui_->double_spin_box_curvature_threshold->setRange(0.0, 1.0); + + // Connect the button press to the function that calls the toolpath generation action + connect(ui_->push_button_segment, &QPushButton::clicked, this, &SegmentationParametersEditorWidget::segmentMesh); +} + +void SegmentationParametersEditorWidget::init(const pcl_msgs::PolygonMesh& mesh) +{ + shape_msgs::Mesh input_mesh; + opp_gui::utils::pclMsgToShapeMsg(mesh, input_mesh); + input_mesh_.reset(new shape_msgs::Mesh(input_mesh)); +} + +void SegmentationParametersEditorWidget::init(const shape_msgs::Mesh& mesh) +{ + input_mesh_.reset(new shape_msgs::Mesh(mesh)); +} + +void SegmentationParametersEditorWidget::setSegmentationConfig(const noether_msgs::SegmentationConfig& config) +{ + ROS_INFO("Setting Segmentation Parameters"); + + ui_->spin_box_min_cluster_size->setValue(config.min_cluster_size); + ui_->double_spin_box_curvature_threshold->setValue(config.curvature_threshold); +} + +void SegmentationParametersEditorWidget::setFilteringConfig(const noether_msgs::FilteringConfig& config) +{ + ROS_INFO("Setting Filtering Parameters"); + // This is currently empty +} + +noether_msgs::SegmentationConfig SegmentationParametersEditorWidget::getSegmentationConfig() const +{ + noether_msgs::SegmentationConfig config; + + // Create a path configuration from the line edit fields + config.min_cluster_size = ui_->spin_box_min_cluster_size->value(); + config.curvature_threshold = ui_->double_spin_box_curvature_threshold->value(); + + // TODO: Advanced setting + config.use_mesh_normals = true; + // If false enable box +// config.neighborhood_radius; + + return config; +} + +noether_msgs::FilteringConfig SegmentationParametersEditorWidget::getFilteringConfig() const +{ + noether_msgs::FilteringConfig config; + + // Should add a dropdown for advanced settings + config.enable_filtering = true; + config.windowed_sinc_iterations = 20; + + return config; +} + +void SegmentationParametersEditorWidget::segmentMesh() +{ + ROS_INFO("Performing Segmentation"); + + if (!client_.isServerConnected()) + { + std::string message = "Action server on '" + SEGMENTATION_ACTION + "' is not connected"; + QMessageBox::warning(this, "ROS Communication Error", QString(message.c_str())); + return; + } + + if (!input_mesh_) + { + QMessageBox::warning(this, "Input Error", "Mesh has not yet been specified"); + return; + } + + // Get the correct mesh format for Noether + pcl_msgs::PolygonMesh pcl_mesh; + opp_gui::utils::pclMsgFromShapeMsg(*input_mesh_, pcl_mesh); + + // Create an action goal + noether_msgs::SegmentGoal goal; + goal.input_mesh = std::move(pcl_mesh); + goal.filtering_config = getFilteringConfig(); + goal.segmentation_config = getSegmentationConfig(); + + client_.sendGoal(goal, boost::bind(&SegmentationParametersEditorWidget::onSegmentMeshComplete, this, _1, _2)); + + progress_dialog_ = new QProgressDialog(this); + progress_dialog_->setModal(true); + progress_dialog_->setLabelText("Segmentation Progress"); + progress_dialog_->setMinimum(0); + progress_dialog_->setMaximum(100); + + progress_dialog_->setValue(progress_dialog_->minimum()); + progress_dialog_->show(); +} + +void SegmentationParametersEditorWidget::onSegmentMeshComplete(const actionlib::SimpleClientGoalState& state, + const noether_msgs::SegmentResultConstPtr& res) +{ + // TODO: Actually make this do something useful + 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) + { + std::string message = "Action '" + SEGMENTATION_ACTION + "' failed to succeed"; + QMessageBox::warning(this, "Segmentation Error", QString(message.c_str())); + } + else + { + ROS_INFO_STREAM("Successfully segmented the mesh"); + + // Store results + segments_.resize(res->output_mesh.size() - 1); + shape_msgs::Mesh mesh_msg; + for (std::size_t ind = 0; ind < res->output_mesh.size() - 1; ind++) + { + // get the pcl_msgs::PolygonMesh + res->output_mesh[ind]; + + // convert to shape_msgs::Mesh + opp_gui::utils::pclMsgToShapeMsg(res->output_mesh[ind], mesh_msg); + + // Add to array + segments_[ind].reset(new shape_msgs::Mesh(mesh_msg)); + + // convert to pcl::PolygonMesh + pcl::PolygonMesh pcl_mesh; + pcl_conversions::toPCL(res->output_mesh[ind], pcl_mesh); + + // Save to file + std::string filename = "/tmp/segment_" + boost::lexical_cast(ind) + ".stl"; + pcl::io::savePolygonFile(filename, pcl_mesh, true); + } + opp_gui::utils::pclMsgToShapeMsg(res->output_mesh.back(), mesh_msg); + edges_.reset(new shape_msgs::Mesh(mesh_msg)); + + emit segmentationFinished(segments_, edges_); + } +} + +} // namespace opp_gui diff --git a/opp_gui/src/widgets/surface_selection_combo_widget.cpp b/opp_gui/src/widgets/surface_selection_combo_widget.cpp new file mode 100644 index 0000000..37a7cc6 --- /dev/null +++ b/opp_gui/src/widgets/surface_selection_combo_widget.cpp @@ -0,0 +1,230 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include + +#include + +#include +#include +#include "ui_surface_selection_combo_widget.h" + +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 +{ + // Setup the publisher for visualizing the selected area in RViz + selected_area_marker_publisher_ = nh.advertise(MARKER_TOPIC, 1, true); + + // Register ROS-message types with QT so we can use them in signals & slots + int id = qRegisterMetaType(); + id = qRegisterMetaType(); + id = qRegisterMetaType>(); + + // Initialize the visual component + ui_->setupUi(this); + + // Create and attach the segmentation widget + segmenter_ = new opp_gui::SegmentationParametersEditorWidget(this); + ui_->layout_for_segmenter_widget->addWidget(segmenter_); + + // Set behavior on the list selection widget (which was initialized as part of ui_) + ui_->list_widget_segment_list->setSelectionBehavior(QAbstractItemView::SelectItems); + ui_->list_widget_segment_list->setSelectionMode(QAbstractItemView::SingleSelection); + 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); + 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); +} + +SurfaceSelectionComboWidget::~SurfaceSelectionComboWidget() +{ + delete ui_; + delete segmenter_; + delete area_selector_; +} + +noether_msgs::SegmentationConfig SurfaceSelectionComboWidget::getSegmentationConfig() +{ + noether_msgs::SegmentationConfig config; + if (segmenter_ == nullptr) + { + ROS_ERROR("segmenter_ not set in SurfaceSelectionComboWidget"); + } + else + { + config = segmenter_->getSegmentationConfig(); + } + return config; +} + +void SurfaceSelectionComboWidget::setSegmentationConfig(const noether_msgs::SegmentationConfig& config) +{ + if (segmenter_ == nullptr) + { + ROS_ERROR("segmenter_ not set in SurfaceSelectionComboWidget"); + } + else + { + segmenter_->setSegmentationConfig(config); + } + return; +} + +void SurfaceSelectionComboWidget::init(const shape_msgs::Mesh& mesh) +{ + // Set the mesh + mesh_.reset(new shape_msgs::Mesh(mesh)); + + // Empty the segment list, and reset the 'whole mesh' option + segment_list_.resize(1); + segment_list_[0].reset(new shape_msgs::Mesh(mesh)); + ui_->list_widget_segment_list->clear(); + ui_->list_widget_segment_list->addItem("Full Mesh"); + ui_->list_widget_segment_list->setCurrentRow(0); + + // Reset the selected area + selected_area_.reset(new shape_msgs::Mesh(mesh)); + + // Send the mesh along to the segmentation widget + // (The polygon selector will get it automatically) + segmenter_->init(mesh); + return; +} + +void SurfaceSelectionComboWidget::newSegmentList( + const std::vector& segments, + const shape_msgs::Mesh::Ptr& remnants) +{ + // Save the original mesh with the segments + segment_list_.resize(1); + segment_list_[0].reset(new shape_msgs::Mesh(*mesh_)); + for (std::size_t i = 0; i < segments.size(); ++i) + { + segment_list_.emplace_back(new shape_msgs::Mesh(*(segments[i]))); + } + + // Add labels for each segment to the list + ui_->list_widget_segment_list->clear(); + ui_->list_widget_segment_list->addItem("Full Mesh"); + std::string label; + for (std::size_t i = 0; i < segments.size(); ++i) + { + label = "segment_" + boost::lexical_cast(i); + ui_->list_widget_segment_list->addItem(label.c_str()); + } + + return; +} + +void SurfaceSelectionComboWidget::newSelectedSegment() +{ + // Get the selected item(s) from the list + QList selectedSegments = ui_->list_widget_segment_list->selectedItems(); + + // Make sure that there is a segment selected, otherwise crashes can happen + if (selectedSegments.size() == 0) + { + return; + } + + // Since the listWidget was set to have only one selection in this + // class's constructor, we just use the first item. + std::size_t selection_index = static_cast(ui_->list_widget_segment_list->currentRow()); + + // Based on the user's selection, send the appropriate segment to the + // polygon selection tool and set the selected area to the entire + // selected segment. + selected_area_.reset(new shape_msgs::Mesh(*(segment_list_[selection_index]))); + area_selector_->init(*(segment_list_[selection_index])); + + return; +} + +void SurfaceSelectionComboWidget::newSelectedSubmesh(const shape_msgs::Mesh::Ptr& selected_submesh) +{ + // Save the selected submesh, display it, then emit it as a signal + selected_area_.reset(new shape_msgs::Mesh(*selected_submesh)); + publishTargetMesh(); + emit newTargetMesh(selected_area_); + + return; +} + +void SurfaceSelectionComboWidget::publishTargetMesh() +{ + visualization_msgs::Marker target_mesh; + target_mesh.header.frame_id = "map"; + target_mesh.header.stamp = ros::Time(); + target_mesh.id = 0; + target_mesh.type = visualization_msgs::Marker::TRIANGLE_LIST; + target_mesh.action = visualization_msgs::Marker::ADD; + target_mesh.pose.position.x = 0; + target_mesh.pose.position.y = 0; + target_mesh.pose.position.z = 0; + target_mesh.pose.orientation.w = 1; + target_mesh.pose.orientation.x = 0; + target_mesh.pose.orientation.y = 0; + target_mesh.pose.orientation.z = 0; + target_mesh.scale.x = 1; + target_mesh.scale.y = 1; + target_mesh.scale.z = 1; + target_mesh.color.a = 1; + target_mesh.color.r = 1; + target_mesh.color.b = 0; + target_mesh.color.g = 1; + target_mesh.points.clear(); + for (std::size_t i = 0; i < selected_area_->triangles.size(); ++i) + { + target_mesh.points.push_back(selected_area_->vertices[selected_area_->triangles[i].vertex_indices[0]]); + target_mesh.points.push_back(selected_area_->vertices[selected_area_->triangles[i].vertex_indices[1]]); + target_mesh.points.push_back(selected_area_->vertices[selected_area_->triangles[i].vertex_indices[2]]); + } + selected_area_marker_publisher_.publish(target_mesh); + return; +} + +} // 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 new file mode 100644 index 0000000..795577d --- /dev/null +++ b/opp_gui/src/widgets/tool_path_editor_widget.cpp @@ -0,0 +1,298 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +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) +{ + surface_selector_ = new SurfaceSelectionComboWidget(nh_, selection_world_frame, selection_sensor_frame, this); + editor_ = new ToolPathParametersEditorWidget(nh_, this); + + // Add the point editor to the parameters frame + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(surface_selector_); + layout->addWidget(editor_); + ui_->frame_parameters->setLayout(layout); + + // Disable the parameters frame until a path is added + ui_->frame_parameters->setEnabled(false); + + // Connect the signals and slots + connect(ui_->push_button_add, &QPushButton::clicked, this, &ToolPathEditorWidget::onAddPressed); + connect(ui_->push_button_remove, &QPushButton::clicked, this, &ToolPathEditorWidget::onRemovePressed); + connect(ui_->list_widget, &QListWidget::currentItemChanged, this, &ToolPathEditorWidget::onListSelectionChanged); + + connect(editor_, &ToolPathParametersEditorWidget::dataChanged, this, &ToolPathEditorWidget::onDataChanged); + + connect(surface_selector_, &SurfaceSelectionComboWidget::newTargetMesh, + this, &ToolPathEditorWidget::newTargetMeshSelected); + + // Create a publisher for the tool path marker + pub_ = nh_.advertise(TOOL_PATH_TOPIC, 1, true); +} + +void ToolPathEditorWidget::init(const shape_msgs::Mesh& mesh) +{ + surface_selector_->init(mesh); + editor_->init(mesh); +} + +void ToolPathEditorWidget::addToolPathData(const std::vector& tool_path_list) +{ + // reset the visualization + opp_msgs::ToolPath empty; + publishToolPathDisplay(empty); + + // empty the paths + data_.clear(); + ui_->list_widget->clear(); + + // Add the new paths to the list + int i = 0; + for (opp_msgs::ToolPath path : tool_path_list) + { + opp_msgs::ToolPath val; + val = path; + + // If the config was empty, set default values. Floating-point + // comparison to 0.0 will turn false negatives, but should never get + // a false positive. We definitely don't want to overwrite valid + // parameters, so that seems OK for now. + if (val.params.config.line_spacing == 0.0) + { + val.params.config.line_spacing = 0.2; + val.params.config.pt_spacing = 0.1; + val.params.config.min_hole_size = 0.2; + val.params.config.min_segment_size = 0.5; + val.params.config.intersecting_plane_height = 0.05; + + val.params.curvature_threshold = 0.050; + val.params.min_polygons_per_cluster = 500; + } + std::string key_base = "previous_path_"; + std::string key = key_base + std::to_string(i); + while (data_.find(key) != data_.end()) + { + ++i; + if (i > 2000) break; // can't be too careful + key = key_base + std::to_string(i); + } + data_.emplace(key, val); + ui_->list_widget->addItem(QString::fromStdString(key)); + } +} + +void ToolPathEditorWidget::newTargetMeshSelected(const shape_msgs::Mesh::Ptr& target_mesh) +{ + if (target_mesh != nullptr) + { + editor_->init(*target_mesh); + } + return; +} + +void ToolPathEditorWidget::onAddPressed() +{ + bool ok; + QString key = QInputDialog::getText(this, "Add New", "Name", QLineEdit::Normal, "", &ok); + if(ok && !key.isEmpty()) + { + opp_msgs::ToolPath val; + val.params.config.line_spacing = 0.2; + val.params.config.pt_spacing = 0.1; + val.params.config.min_hole_size = 0.2; + val.params.config.min_segment_size = 0.5; + val.params.config.intersecting_plane_height = 0.05; + val.params.curvature_threshold = 0.050; + val.params.min_polygons_per_cluster = 500; + + 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()) + { + // Add the entry to the map of points + data_.emplace(key.toStdString(), val); + + // Add the name to the list widget + ui_->list_widget->addItem(key); + } + } +} + +void ToolPathEditorWidget::onRemovePressed() +{ + // Get the selection from the list widget + QList current_items = ui_->list_widget->selectedItems(); + + for(QListWidgetItem* item : current_items) + { + std::string key = item->text().toStdString(); + auto it = data_.find(key); + if(it != data_.end()) + { + data_.erase(it); + } + else + { + ROS_ERROR_STREAM(__func__ << "Failed to find part '" << key << "' in map"); + } + + // Remove the selections from the list widget + ui_->list_widget->removeItemWidget(item); + delete item; + } + + // Disable the parameters frame if there are no tool paths left + if(data_.empty()) + { + ui_->frame_parameters->setEnabled(false); + } +} + +void ToolPathEditorWidget::onListSelectionChanged(QListWidgetItem* current, + QListWidgetItem* previous) +{ + if (previous) + { + // Get the current data from the tool path editor widget and save it for the "previous" tool path + opp_msgs::ToolPath tool_path; + if (!editor_->getToolPath(tool_path)) + { + ROS_WARN_STREAM(__func__ << ": Toolpath has not been generated yet"); + return; + } + tool_path.params.config = editor_->getToolPathConfig(); + tool_path.params.curvature_threshold = surface_selector_->getSegmentationConfig().curvature_threshold; + tool_path.params.min_polygons_per_cluster = surface_selector_->getSegmentationConfig().min_cluster_size; + + std::string prev_name = previous->text().toStdString(); + auto prev_it = data_.find(prev_name); + if (prev_it != data_.end()) + { + prev_it->second = tool_path; + } + else + { + ROS_WARN_STREAM(__func__ << ": Previous point '" << prev_name << "' does not exist in the map"); + } + } + + if (current) + { + // Enable the parameters frame if the current selection is valid + ui_->frame_parameters->setEnabled(true); + + // Load the tool path configuration data for the "current" tool path into the point editor widget + std::string curr_name = current->text().toStdString(); + auto curr_it = data_.find(curr_name); + if (curr_it != data_.end()) + { + // Set segmentation parameters + noether_msgs::SegmentationConfig seg_config = surface_selector_->getSegmentationConfig(); + seg_config.min_cluster_size = curr_it->second.params.min_polygons_per_cluster; + seg_config.curvature_threshold = curr_it->second.params.curvature_threshold; + surface_selector_->setSegmentationConfig(seg_config); + + // Set rastering parameters + editor_->setToolPathConfig(curr_it->second.params.config); + + // Set the tool path + editor_->setToolPath(curr_it->second); + + // Display the current tool path + opp_msgs::ToolPath tool_path; + if (editor_->getToolPath(tool_path)) + { + publishToolPathDisplay(tool_path); + } + } + else + { + ROS_WARN_STREAM(__func__ << "Current point '" << curr_name << "' does not exist in the map"); + } + } +} + +void ToolPathEditorWidget::publishToolPathDisplay(const opp_msgs::ToolPath& tool_path) +{ + // 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) + { + tool_path_display.poses.insert(tool_path_display.poses.end(), pose_arr.poses.begin(), pose_arr.poses.end()); + } + + // Set the frame for this display + tool_path_display.header.frame_id = marker_frame_; + + // Publish the display + pub_.publish(tool_path_display); +} + +void ToolPathEditorWidget::onDataChanged() +{ + + QListWidgetItem* current = ui_->list_widget->currentItem(); + if(current) + { + std::string key = current->text().toStdString(); + + if(!key.empty()) + { + auto it = data_.find(key); + if(it != data_.end()) + { + opp_msgs::ToolPath tool_path; + if(editor_->getToolPath(tool_path)) + { + if (surface_selector_ == nullptr) + { + ROS_ERROR("surface_selector_ not assigned in ToolPathEditorWidget"); + } + else + { + tool_path.params.curvature_threshold = surface_selector_->getSegmentationConfig().curvature_threshold; + tool_path.params.min_polygons_per_cluster = surface_selector_->getSegmentationConfig().min_cluster_size; + } + + // Save the updated data internally + it->second = tool_path; + + // Publish an updated tool path display + publishToolPathDisplay(tool_path); + } + } + } + } +} + +} // 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 new file mode 100644 index 0000000..bd4515a --- /dev/null +++ b/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp @@ -0,0 +1,228 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include "ui_tool_path_parameters_editor.h" + +#include +#include + +#include +#include +#include + +#include +#include + +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) +{ + ui_ = new Ui::ToolPathParametersEditor(); + ui_->setupUi(this); + + ui_->double_spin_box_line_spacing->setRange(0.0, std::numeric_limits::max()); + ui_->double_spin_box_min_hole_size->setRange(0.0, std::numeric_limits::max()); + ui_->double_spin_box_point_spacing->setRange(0.0, std::numeric_limits::max()); + ui_->double_spin_box_tool_z_offset->setRange(-std::numeric_limits::max(), std::numeric_limits::max()); + ui_->double_spin_box_min_segment_length->setRange(0.0, std::numeric_limits::max()); + ui_->double_spin_box_intersecting_plane_height->setRange(0.0, std::numeric_limits::max()); + ui_->double_spin_box_raster_angle->setRange(-180., 180.); + + ui_->spin_box_dwell_time->setRange(0, std::numeric_limits::max()); + + // Add values to the process type drop-down + ui_->combo_box_process_type->addItem("None", QVariant(opp_msgs::ProcessType::NONE)); + ui_->combo_box_process_type->addItem("Process Paint", QVariant(opp_msgs::ProcessType::PROCESS_PAINT)); + ui_->combo_box_process_type->addItem("Process De-paint", QVariant(opp_msgs::ProcessType::PROCESS_DEPAINT)); + + // 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); +} + +void ToolPathParametersEditorWidget::init(const shape_msgs::Mesh& mesh) +{ + mesh_.reset(new shape_msgs::Mesh(mesh)); +} + +void ToolPathParametersEditorWidget::setToolPathConfig(const noether_msgs::ToolPathConfig& config) +{ + ui_->double_spin_box_point_spacing->setValue(config.pt_spacing); + ui_->double_spin_box_tool_z_offset->setValue(config.tool_offset); + ui_->double_spin_box_line_spacing->setValue(config.line_spacing); + ui_->double_spin_box_min_hole_size->setValue(config.min_hole_size); + ui_->double_spin_box_min_segment_length->setValue(config.min_segment_size); + ui_->double_spin_box_intersecting_plane_height->setValue(config.intersecting_plane_height); + ui_->double_spin_box_raster_angle->setValue(config.raster_angle * 180.0 / M_PI); +} + +noether_msgs::ToolPathConfig ToolPathParametersEditorWidget::getToolPathConfig() const +{ + noether_msgs::ToolPathConfig config; + + // Create a path configuration from the line edit fields + config.pt_spacing = ui_->double_spin_box_point_spacing->value(); + config.tool_offset = ui_->double_spin_box_tool_z_offset->value(); + config.line_spacing = ui_->double_spin_box_line_spacing->value(); + config.min_hole_size = ui_->double_spin_box_min_hole_size->value(); + config.min_segment_size = ui_->double_spin_box_min_segment_length->value(); + config.intersecting_plane_height = ui_->double_spin_box_intersecting_plane_height->value(); + config.raster_angle = ui_->double_spin_box_raster_angle->value() * M_PI / 180.0; + config.raster_wrt_global_axes = false; + + return config; +} + +void ToolPathParametersEditorWidget::setToolPath(const opp_msgs::ToolPath& tool_path) +{ + if(!tool_path_) + { + tool_path_.reset(new opp_msgs::ToolPath(tool_path)); + } + else + { + *tool_path_ = tool_path; + ui_->combo_box_process_type->setCurrentIndex(ui_->combo_box_process_type->findData(tool_path_->process_type.val)); + ui_->spin_box_dwell_time->setValue(static_cast(tool_path_->dwell_time)); + } +} + +bool ToolPathParametersEditorWidget::getToolPath(opp_msgs::ToolPath& tool_path) const +{ + if(tool_path_) + { + tool_path = *tool_path_; + return true; + } + return false; +} + +void ToolPathParametersEditorWidget::generateToolPath() +{ + 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_) + { + QMessageBox::warning(this, "Input Error", "Mesh has not yet been specified"); + return; + } + + // Create an action goal + noether_msgs::GenerateToolPathsGoal goal; + goal.path_configs.push_back(getToolPathConfig()); + goal.surface_meshes.push_back(*mesh_); + goal.proceed_on_failure = false; + + client_.sendGoal(goal, boost::bind(&ToolPathParametersEditorWidget::onGenerateToolPathsComplete, this, _1, _2)); + + progress_dialog_ = new QProgressDialog(this); + progress_dialog_->setModal(true); + progress_dialog_->setLabelText("Path Planning Progress"); + progress_dialog_->setMinimum(0); + progress_dialog_->setMaximum(100); + + progress_dialog_->setValue(progress_dialog_->minimum()); + progress_dialog_->show(); +} + +void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(const actionlib::SimpleClientGoalState& state, + const noether_msgs::GenerateToolPathsResultConstPtr& res) +{ + 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) + { + 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]) + { + QMessageBox::warning(this, "Tool Path Planning Error", "Tool path generation failed"); + } + else + { + ROS_INFO_STREAM("Successfully generated tool path"); + + opp_msgs::ToolPath tp; + tp.header.stamp = ros::Time::now(); + tp.process_type.val = qvariant_cast(ui_->combo_box_process_type->currentData()); + tp.paths = res->tool_raster_paths[0].paths; + tp.params.config.pt_spacing = ui_->double_spin_box_point_spacing->value(); + tp.params.config.tool_offset = ui_->double_spin_box_tool_z_offset->value(); + tp.params.config.line_spacing = ui_->double_spin_box_line_spacing->value(); + 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.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.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); + + // Save the toolpath + tool_path_.reset(new opp_msgs::ToolPath(tp)); + + emit dataChanged(); + } + } +} + +void ToolPathParametersEditorWidget::updateProcessType(const QString&) +{ + if(tool_path_) + { + tool_path_->process_type.val = qvariant_cast(ui_->combo_box_process_type->currentData()); + } +} + +void ToolPathParametersEditorWidget::updateDwellTime(const int value) +{ + if(tool_path_) + { + tool_path_->dwell_time = static_cast(value * 60); + } +} + +} // 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 new file mode 100644 index 0000000..26a7296 --- /dev/null +++ b/opp_gui/src/widgets/tool_path_planner_widget.cpp @@ -0,0 +1,644 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include "ui_tool_path_planner.h" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +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) +{ + ui_ = new Ui::ToolPathPlanner(); + ui_->setupUi(this); + + // Add the available tf frame + if (!frames.empty()) + { + marker_frame_ = frames.front(); + } + else + { + ROS_ERROR_STREAM("No visualization frames available. Using \"map\" instead."); + marker_frame_ = "map"; + } + + // Create the other widgets + touch_point_editor_ = new TouchPointEditorWidget(this, nh_, marker_frame_); + verification_point_editor_ = new TouchPointEditorWidget(this, nh_, marker_frame_); + tool_path_editor_ = new ToolPathEditorWidget(this, nh_, marker_frame_); + + // Set the color of the touch point markers + touch_point_editor_->setMarkerColor(1.0, 0.0, 0.0); + verification_point_editor_->setMarkerColor(0.0, 0.0, 1.0); + + // Add the widgets to the appropriate frames + { + // Touch point editor + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(touch_point_editor_); + ui_->frame_define_touch_off_points->setLayout(layout); + } + { + // Verification point editor + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(verification_point_editor_); + ui_->frame_define_verification_points->setLayout(layout); + } + { + // Tool path editor + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(tool_path_editor_); + ui_->frame_define_toolpaths->setLayout(layout); + } + + // Disable the "downstream" tabs until a model is loaded + setModelTabsEnabled(false); + setJobTabsEnabled(false, false); + + // 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); + loadModelsFromDatabase(); + 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_->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); + + // Signals & Slots for the Buttons on database management page + connect(ui_->push_button_show_part, &QPushButton::clicked, this, &ToolPathPlannerWidget::showPartFromDatabase); + connect(ui_->push_button_suppress_part, &QPushButton::clicked, this, &ToolPathPlannerWidget::deletePart); + connect(ui_->push_button_suppress_job, &QPushButton::clicked, this, &ToolPathPlannerWidget::deleteJob); + connect(ui_->push_button_refresh_parts, &QPushButton::clicked, this, &ToolPathPlannerWidget::refresh); + connect(ui_->push_button_refresh_jobs, &QPushButton::clicked, this, &ToolPathPlannerWidget::refresh); + + // Add a publisher for the mesh marker + pub_ = nh_.advertise(MESH_MARKER_TOPIC, 1, true); + + // Set up the Database views in the third page + std::string query = "`suppressed`!=\"1\""; + model_parts_ = new QSqlTableModel(this, database_.getDatabase()); + model_parts_->setTable(QString::fromStdString(opp_db::PARTS_TABLE_NAME)); + 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_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 + // 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->show(); + ui_->table_view_jobs->setModel(model_jobs_); + ui_->table_view_jobs->setEditTriggers(QTableView::NoEditTriggers); // Make read only + ui_->table_view_jobs->show(); +} + +void ToolPathPlannerWidget::setVisualizationFrame(const QString& text) +{ + marker_frame_ = text.toStdString(); + touch_point_editor_->setMarkerFrame(marker_frame_); + verification_point_editor_->setMarkerFrame(marker_frame_); + tool_path_editor_->setMarkerFrame(marker_frame_); +} + +// Parts Page +void ToolPathPlannerWidget::browseForMeshResource() +{ + QString filename = QFileDialog::getOpenFileName(this, "Load Model", "", "Mesh Files (*.stl *.ply *.obj)"); + if(filename.isEmpty()) + { + ROS_WARN_STREAM(__func__ << ": Empty filename"); + return; + } + + ui_->line_edit_model_filename->setText(filename); + loadMeshFromResource(); + return; +} + +void ToolPathPlannerWidget::loadMeshFromResource() +{ + // Get the filename and package of the model + std::string filename = ui_->line_edit_model_filename->text().toStdString(); + 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"}; + + mesh_resource_.clear(); + for(const std::string& ext : file_extensions) + { + std::regex rgx(".*" + ext + "$"); + std::smatch match; + if(std::regex_search(filename, match, rgx)) + { + mesh_resource_ = "file://" + filename; + break; + } + } + + if(mesh_resource_.empty()) + { + std::string message = "Invalid mesh resource file extension. Acceptable inputs are: "; + for(const std::string& ext : file_extensions) + message += ext + " "; + + QMessageBox::warning(this, "Input Error", QString(message.c_str())); + return; + } + ROS_INFO_STREAM("Attempting to load mesh from resource: '" << mesh_resource_ << "'"); + + if(!loadMesh()) return; +} + +void ToolPathPlannerWidget::loadModelsFromDatabase() +{ + // Create the variables the database interface will fill + std::map parts_map; + std::string error_msg; + + // Retrieve part info from the database + if (!database_.getAllPartsFromDatabase(error_msg, parts_map)) + { + // If the function failed, create a warning pop-up box. + std::string message = "Failed to retrieve parts from database, experienced error: " + error_msg; + QMessageBox::warning(this, "Database Communication Error", QString::fromStdString(message)); + } + else + { + // If the function succeeded, clear existing part lists + existing_parts_.clear(); + ui_->list_widget_parts->clear(); + + // And then add each part to those lists, one at a time + for (const std::pair id_and_part : parts_map) + { + // internal list of parts + existing_parts_.push_back(id_and_part.second); + + // Gui display listing parts to user + QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(id_and_part.second.name)); + item->setData(Qt::ItemDataRole::UserRole, QVariant(QString::fromStdString(id_and_part.second.description))); + ui_->list_widget_parts->addItem(item); + } + } + return; +} + +void ToolPathPlannerWidget::onModelSelectionChanged(QListWidgetItem* current, + QListWidgetItem*) +{ + // Change the description display + if(current != nullptr) + { + ui_->text_edit_part_description->setText(current->data(Qt::ItemDataRole::UserRole).toString()); + } +} + +void ToolPathPlannerWidget::loadSelectedModel() +{ + int row = ui_->list_widget_parts->currentRow(); + 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; + + // Update the UI and widgets with all part information + ui_->line_edit_model_name->setText(QString::fromStdString(part.name)); + ui_->plain_text_edit_model_description->setPlainText(QString::fromStdString(part.description)); + + touch_point_editor_->setPoints(part.touch_points); + verification_point_editor_->setPoints(part.verification_points); + + setJobTabsEnabled(false, true); + loadJobsFromDatabase(); + } + else + { + QMessageBox::warning(this, "Input Error", "Make a selection in the parts list"); + } +} + +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; + + // 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()) + { + QMessageBox::warning(this, "Input Error", "Model ID or description field(s) is empty"); + return; + } + + // Get the touch points and verification points, and make sure there are + // at least 3 of each. (This requirement could probably be relaxed.) + 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) + { + QMessageBox::warning(this, "Invalid Model Definition", "Ensure at least 3 touch points and 3 verification points have been defined"); + return; + } + + // Fill out the part struct with input information + opp_msgs::Part part; + part.name = model_name; + part.description = model_description; + part.mesh_resource = mesh_resource_; + + // Copy the touch points + part.touch_points.reserve(touch_points.size()); + 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) + { + part.verification_points.push_back(pair.second); + } + + // Save the model to the database + std::string error_msg; + long int key = database_.addPartToDatabase(part, error_msg); + if (key < 0) + { + // If the function failed, warn the user. + std::string message = "Failed to add part to database, received error: " + error_msg; + QMessageBox::warning(this, "Database Communication Error", QString(message.c_str())); + } + else + { + // Save the auto generated model id + generated_model_id_ = key; + + // If the save is successful, allow the user to add job data for the part + setJobTabsEnabled(true, true); + + // Make sure the database lists are updated + refresh(); + loadModelsFromDatabase(); + loadJobsFromDatabase(); + } + return; +} + +// Jobs Page +void ToolPathPlannerWidget::newJob() +{ + std::vector empty; + tool_path_editor_->addToolPathData(empty); + setJobTabsEnabled(true, true); + return; +} + +void ToolPathPlannerWidget::loadJobsFromDatabase() +{ + std::map jobs_map; + std::string message; + if (!database_.getAllJobsFromDatabase(generated_model_id_, message, jobs_map)) + { + QMessageBox::warning(this, "Database Error", "Could not load any jobs for this part"); + return; + } + existing_jobs_.clear(); + ui_->list_widget_jobs->clear(); + for (std::pair job_with_id : jobs_map) + { + QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(job_with_id.second.name)); + item->setData(Qt::ItemDataRole::UserRole, QVariant(QString::fromStdString(job_with_id.second.description))); + ui_->list_widget_jobs->addItem(item); + existing_jobs_.push_back(job_with_id.second); + } + + return; +} + +void ToolPathPlannerWidget::onJobSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous) +{ + (void) previous; + + // Change the description display + if(current != nullptr) + { + ui_->text_edit_jobs->setText(current->data(Qt::ItemDataRole::UserRole).toString()); + } + else + { + ui_->text_edit_jobs->setText(QString::fromStdString(std::string(""))); + } + return; +} + +void ToolPathPlannerWidget::loadSelectedJob() +{ + int row = ui_->list_widget_jobs->currentRow(); + if(row >= 0 && row < static_cast(existing_jobs_.size())) + { + // Get the selected part information + const opp_msgs::Job& job = existing_jobs_[static_cast(row)]; + + // Update the UI and widgets with all part information + ui_->line_edit_job_name->setText(QString::fromStdString(job.name)); + ui_->plain_text_edit_job_description->setPlainText(QString::fromStdString(job.description)); + + // Add the old paths to the tool path editor + tool_path_editor_->addToolPathData(job.paths); + + setJobTabsEnabled(true, true); + } + else + { + QMessageBox::warning(this, "Input Error", "Make a selection in the jobs list"); + } + return; +} + +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) + { + return; + } + + // 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()) + { + QMessageBox::warning(this, "Input Error", "Job ID or description is invalid"); + return; + } + + // Construct a job object + opp_msgs::Job job; + job.name = job_name; + job.description = job_description; + job.part_id = generated_model_id_; + job.header.stamp = ros::Time::now(); + + // 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) + { + job.paths.push_back(pair.second); + } + + // Save the job to the database + std::string error_msg; + long int key = database_.addJobToDatabase(job, error_msg); + if (key < 0) + { + // If the function failed, warn the user. + std::string message = "Failed to add job to database, received error: " + error_msg; + QMessageBox::warning(this, "Database Communication Error", QString(message.c_str())); + } + else + { + // update the database views + refresh(); + loadJobsFromDatabase(); + } + + return; +} + +// Database Management Page +void ToolPathPlannerWidget::showPartFromDatabase() +{ + // Get the part id of the currently selected row + QModelIndex index = ui_->table_view_parts->currentIndex(); + QSqlRecord part_record = model_parts_->record(index.row()); + QSqlField part_id_field = part_record.field("part_id"); + QVariant part_id_value = part_id_field.value(); + unsigned int part_id = static_cast(part_id_value.toInt()); + + // Get the part definition from the database + std::string msg; + opp_msgs::Part part; + if (!database_.getPartFromDatabase(part_id, msg, part)) + { + ROS_ERROR_STREAM("could not load selected part, had error: " << msg); + return; + } + + // Set class parameters using the loaded part + generated_model_id_ = part.id; + mesh_resource_ = part.mesh_resource; + + if(!loadMesh()) return; + + // Update the UI and widgets with all part information + ui_->line_edit_model_name->setText(QString::fromStdString(part.name)); + ui_->plain_text_edit_model_description->setPlainText(QString::fromStdString(part.description)); + + touch_point_editor_->setPoints(part.touch_points); + verification_point_editor_->setPoints(part.verification_points); + + setJobTabsEnabled(false, true); + loadJobsFromDatabase(); +} + +void ToolPathPlannerWidget::deletePart() +{ + // Get the part id of the currently selected row + QModelIndex index = ui_->table_view_parts->currentIndex(); + QSqlRecord part_record = model_parts_->record(index.row()); + QSqlField part_id_field = part_record.field("part_id"); + QVariant part_id_value = part_id_field.value(); + unsigned int part_id = static_cast(part_id_value.toInt()); + + // Suppress (but do not actually delete) the part + std::string msg; + if (!database_.suppressPart(part_id, msg)) + { + ROS_ERROR_STREAM("could not suppress selected part, had error: " << msg); + } + + // In case we have the to-be-suppressed part open, disable job creation. + // They will have to save as a new part before creating a new job. + if (part_id == generated_model_id_) + { + setModelTabsEnabled(false); + setJobTabsEnabled(false, false); + } + + // Refresh the various database displays to ensure no 'ghosts' remain. + refresh(); + loadModelsFromDatabase(); + return; +} + +void ToolPathPlannerWidget::deleteJob() +{ + // Get the id of the currently selected row + QModelIndex index = ui_->table_view_jobs->currentIndex(); + QSqlRecord job_record = model_jobs_->record(index.row()); + QSqlField id_field = job_record.field("id"); + QVariant id_value = id_field.value(); + unsigned int id = static_cast(id_value.toInt()); + + // Suppress (but do not actually delete) the job + std::string msg; + if (!database_.suppressJob(id, msg)) + { + ROS_ERROR_STREAM("could not suppress selected job, had error: " << msg); + } + + // Refresh the displays to chase out any 'ghosts' + refresh(); + return; +} + +void ToolPathPlannerWidget::refresh() +{ + std::string query = "`suppressed`!=\"1\""; + model_parts_->setFilter(QString::fromStdString(query)); + model_jobs_->setFilter(QString::fromStdString(query)); + + model_parts_->select(); + model_jobs_->select(); +} + +// Private functions +void ToolPathPlannerWidget::clear() +{ + // Clear the data in the list editor widgets + touch_point_editor_->clear(); + verification_point_editor_->clear(); + tool_path_editor_->clear(); + + // Clear the text input data about the model + ui_->line_edit_model_name->clear(); + ui_->plain_text_edit_model_description->clear(); + + // Clear the text input data about the job + ui_->line_edit_job_name->clear(); + ui_->plain_text_edit_job_description->clear(); +} + +bool ToolPathPlannerWidget::loadMesh() +{ + // Attempt to load this file into a shape_msgs/Mesh type + shape_msgs::Mesh 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()); + return false; + } + + // Clear the touch point and tool path editors' data before continuing + clear(); + + // Initialize the tool path editor with the mesh + tool_path_editor_->init(mesh); + + // Enable the models tabs but not the jobs tabs + setModelTabsEnabled(true); + setJobTabsEnabled(false, false); + + // Publish the mesh marker + visualization_msgs::Marker mesh_marker = + utils::createMeshMarker(0, "mesh", Eigen::Isometry3d::Identity(), + marker_frame_, mesh_resource_); + + pub_.publish(mesh_marker); + + return true; +} + +void ToolPathPlannerWidget::setModelTabsEnabled(bool enabled) +{ + for(int i = 1; i < ui_->tool_box_model_editor->count(); ++i) + { + ui_->tool_box_model_editor->setItemEnabled(i, enabled); + } + + ui_->frame_define_touch_off_points->setEnabled(enabled); + ui_->frame_define_verification_points->setEnabled(enabled); +} + +void ToolPathPlannerWidget::setJobTabsEnabled(bool enabled, bool first_enabled) +{ + for(int i = 1; i < ui_->tool_box_job_editor->count(); ++i) + { + ui_->tool_box_job_editor->setItemEnabled(i, enabled); + } + if (ui_->tool_box_job_editor->count() > 0) + { + ui_->tool_box_job_editor->setItemEnabled(0, first_enabled); + ui_->push_button_new_job->setEnabled(first_enabled); + ui_->push_button_load_jobs_from_database->setEnabled(first_enabled); + ui_->push_button_load_selected_job->setEnabled(first_enabled); + } + ui_->frame_define_toolpaths->setEnabled(enabled); +} + +} // 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 new file mode 100644 index 0000000..f1b1708 --- /dev/null +++ b/opp_gui/src/widgets/touch_point_editor_widget.cpp @@ -0,0 +1,225 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "ui_touch_point_parameters_editor.h" + +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) +{ + point_editor_ = new TouchPointParametersEditorWidget(this); + + // Add the point editor to the parameters frame + QVBoxLayout* layout = new QVBoxLayout(); + layout->addWidget(point_editor_); + ui_->frame_parameters->setLayout(layout); + + // Disable the editor until a point is added + ui_->frame_parameters->setEnabled(false); + + // Set up the signals and slots + // Add and remove buttons + connect(ui_->push_button_add, &QPushButton::clicked, this, &TouchPointEditorWidget::onAddPressed); + connect(ui_->push_button_remove, &QPushButton::clicked, this, &TouchPointEditorWidget::onRemovePressed); + + // Connect the point editor widget when list widget selection item changes + connect(ui_->list_widget, &QListWidget::currentItemChanged, this, &TouchPointEditorWidget::onListSelectionChanged); + + connect(point_editor_, &TouchPointParametersEditorWidget::dataChanged, this, &TouchPointEditorWidget::onDataChanged); + + // Create a marker publisher + marker_pub_ = nh_.advertise("touch_point_marker", 5, true); +} + +void TouchPointEditorWidget::onAddPressed() +{ + bool ok; + QString key = QInputDialog::getText(this, "Add New", "Name", QLineEdit::Normal, "", &ok); + if(ok && !key.isEmpty()) + { + + // Make sure the key doesn't already exist in the map + if(data_.find(key.toStdString()) == data_.end()) + { + // Set the default value of the touch point + opp_msgs::TouchPoint tp; + tp.name = key.toStdString(); + tp.transform.pose.orientation.w = 1.0; + + // Add the entry to the map of points + data_.emplace(key.toStdString(), tp); + + // Add the name to the list widget + ui_->list_widget->addItem(key); + + // 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_); + marker_pub_.publish(marker); + } + } +} + +void TouchPointEditorWidget::onRemovePressed() +{ + // Get the selection from the list widget + QList current_items = ui_->list_widget->selectedItems(); + + for(QListWidgetItem* item : current_items) + { + std::string key = item->text().toStdString(); + auto it = data_.find(key); + if(it != data_.end()) + { + data_.erase(it); + } + else + { + ROS_ERROR_STREAM(__func__ << "Failed to find part '" << key << "' in map"); + } + + // Remove the selections from the list widget + ui_->list_widget->removeItemWidget(item); + 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); + marker_pub_.publish(marker); + } + + // Disable the parameters frame if there are no touch points left + if(data_.empty()) + { + ui_->frame_parameters->setEnabled(false); + } +} + +void TouchPointEditorWidget::onListSelectionChanged(QListWidgetItem* current, + QListWidgetItem* previous) +{ + if(previous) + { + std::string prev_name = previous->text().toStdString(); + + auto prev_it = data_.find(prev_name); + 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(); + + // Overwrite the name with the correct value + tp.name = prev_name; + + prev_it->second = tp; + } + else + { + ROS_WARN_STREAM(__func__ << ": Previous point '" << prev_name << "' does not exist in the map"); + } + } + + if(current) + { + // Enable the parameters frame if the selection is valid + ui_->frame_parameters->setEnabled(true); + + // 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()) + { + point_editor_->setTouchPoint(curr_it->second); + } + else + { + ROS_WARN_STREAM(__func__ << "Current point '" << curr_name << "' does not exist in the map"); + } + } +} + +void TouchPointEditorWidget::onDataChanged() +{ + QListWidgetItem* item = ui_->list_widget->currentItem(); + if(item) + { + std::string key = item->text().toStdString(); + if(!key.empty()) + { + // Save the updated information + auto it = data_.find(key); + if(it != data_.end()) + { + // Get the current point from the editor + opp_msgs::TouchPoint tp = point_editor_->getTouchPoint(); + + // Overwrite the name with the current value + tp.name = key; + + it->second = tp; + + // Publish an update marker for the point + Eigen::Isometry3d pose; + tf::poseMsgToEigen(tp.transform.pose, pose); + + visualization_msgs::Marker marker = + utils::createArrowMarker(0, key, pose, marker_frame_, + marker_color_, + visualization_msgs::Marker::MODIFY); + marker_pub_.publish(marker); + } + } + } +} + +void TouchPointEditorWidget::setPoints(const std::vector& points) +{ + clear(); + + for(const opp_msgs::TouchPoint& point : points) + { + // Add the entry to the map of points + data_.emplace(point.name, point); + + // Add the name to the list widget + ui_->list_widget->addItem(QString::fromStdString(point.name)); + } +} + +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 diff --git a/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp b/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp new file mode 100644 index 0000000..d3885c0 --- /dev/null +++ b/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp @@ -0,0 +1,137 @@ +/* + * 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. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +const std::string CLICKED_POINT_TOPIC = "/selection_point"; + +namespace opp_gui +{ + +TouchPointParametersEditorWidget::TouchPointParametersEditorWidget(QWidget* parent) + : QWidget(parent) + , accept_mouse_input_(false) +{ + ui_ = new Ui::TouchPointParametersEditor(); + ui_->setupUi(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()); + ui_->double_spin_box_y->setRange(-std::numeric_limits::max(), std::numeric_limits::max()); + ui_->double_spin_box_z->setRange(-std::numeric_limits::max(), std::numeric_limits::max()); + + ui_->double_spin_box_qw->setRange(-1.0, 1.0); + ui_->double_spin_box_qx->setRange(-1.0, 1.0); + ui_->double_spin_box_qy->setRange(-1.0, 1.0); + ui_->double_spin_box_qz->setRange(-1.0, 1.0); + + // The threshold on touchpoints should be a positive number + 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_update, &QPushButton::clicked, this, &TouchPointParametersEditorWidget::dataChanged); +} + +void TouchPointParametersEditorWidget::onSelectWithMouse() +{ + 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"); + ui_->push_button_select_with_mouse->setFlat(true); + + // Update the flag for accepting mouse input + accept_mouse_input_ = true; + } + else + { + // Toggle this functionality back off + ui_->push_button_select_with_mouse->setStyleSheet(""); + ui_->push_button_select_with_mouse->setFlat(false); + + accept_mouse_input_ = false; + } +} + +void TouchPointParametersEditorWidget::callback(const geometry_msgs::PoseStampedConstPtr& msg) +{ + if(accept_mouse_input_) + { + // Reset the button visualization + ui_->push_button_select_with_mouse->setStyleSheet(""); + ui_->push_button_select_with_mouse->setFlat(false); + + // Set the pose + setPose(msg->pose); + + // Reset the flag for accepting mouse input + accept_mouse_input_ = false; + + emit dataChanged(); + } +} + +geometry_msgs::Pose TouchPointParametersEditorWidget::getPose() const +{ + geometry_msgs::Pose pose; + + // Get the data from the line edit fields + pose.position.x = ui_->double_spin_box_x->text().toDouble(); + pose.position.y = ui_->double_spin_box_y->text().toDouble(); + pose.position.z = ui_->double_spin_box_z->text().toDouble(); + + pose.orientation.w = ui_->double_spin_box_qw->text().toDouble(); + pose.orientation.x = ui_->double_spin_box_qx->text().toDouble(); + pose.orientation.y = ui_->double_spin_box_qy->text().toDouble(); + pose.orientation.z = ui_->double_spin_box_qz->text().toDouble(); + + return pose; +} + +void TouchPointParametersEditorWidget::setPose(const geometry_msgs::Pose& pose) +{ + // Save the data from the incoming message to the line edit fields + ui_->double_spin_box_x->setValue(pose.position.x); + ui_->double_spin_box_y->setValue(pose.position.y); + ui_->double_spin_box_z->setValue(pose.position.z); + + ui_->double_spin_box_qw->setValue(pose.orientation.w); + ui_->double_spin_box_qx->setValue(pose.orientation.x); + ui_->double_spin_box_qy->setValue(pose.orientation.y); + ui_->double_spin_box_qz->setValue(pose.orientation.z); +} + +void TouchPointParametersEditorWidget::setTouchPoint(const opp_msgs::TouchPoint& tp) +{ + ui_->text_edit_description->setText(QString::fromStdString(tp.description)); + ui_->double_spin_box_threshold->setValue(tp.threshold); + setPose(tp.transform.pose); +} + +opp_msgs::TouchPoint TouchPointParametersEditorWidget::getTouchPoint() const +{ + opp_msgs::TouchPoint out; + out.description = ui_->text_edit_description->toPlainText().toStdString(); + out.threshold = ui_->double_spin_box_threshold->value(); + out.transform.pose = getPose(); + return out; +} + +} // namespace opp_gui diff --git a/opp_gui/ui/database_browser.ui b/opp_gui/ui/database_browser.ui new file mode 100644 index 0000000..af1c176 --- /dev/null +++ b/opp_gui/ui/database_browser.ui @@ -0,0 +1,113 @@ + + + DatabaseBrowser + + + + 0 + 0 + 525 + 447 + + + + Form + + + + + + + + + 0 + 0 + + + + Qt::ScrollBarAlwaysOff + + + QAbstractScrollArea::AdjustToContentsOnFirstShow + + + + Database Entries + + + + + + + + Qt::Vertical + + + + + + + Description + + + + + + QAbstractScrollArea::AdjustToContents + + + true + + + + + + + + + + + + + + Update List + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Enter WCD + + + + + + + + + + Select + + + + + + + + + + diff --git a/opp_gui/ui/database_log.ui b/opp_gui/ui/database_log.ui new file mode 100644 index 0000000..fa8cd54 --- /dev/null +++ b/opp_gui/ui/database_log.ui @@ -0,0 +1,38 @@ + + + DatabaseLog + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + true + + + + + + + Search by WCD + + + + + + + + + + + diff --git a/opp_gui/ui/list_editor.ui b/opp_gui/ui/list_editor.ui new file mode 100644 index 0000000..f98b1d4 --- /dev/null +++ b/opp_gui/ui/list_editor.ui @@ -0,0 +1,81 @@ + + + ListEditor + + + + 0 + 0 + 361 + 515 + + + + Form + + + + + + List + + + + + + + + Add + + + + + + + Remove + + + + + + + + + + + + + + + Qt::Vertical + + + + + + + + + Parameters + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + + + + + diff --git a/opp_gui/ui/polygon_area_selection_widget.ui b/opp_gui/ui/polygon_area_selection_widget.ui new file mode 100644 index 0000000..21b1653 --- /dev/null +++ b/opp_gui/ui/polygon_area_selection_widget.ui @@ -0,0 +1,43 @@ + + + PolygonAreaSelectionWidget + + + + 0 + 0 + 409 + 114 + + + + Form + + + + + + Area Selection +(Use Publish Point tool, selection is automatically applied) + + + + + + + Clear Selection + + + + + + + Apply Selection + + + + + + + + diff --git a/opp_gui/ui/segmentation_parameters_editor.ui b/opp_gui/ui/segmentation_parameters_editor.ui new file mode 100644 index 0000000..62c2113 --- /dev/null +++ b/opp_gui/ui/segmentation_parameters_editor.ui @@ -0,0 +1,97 @@ + + + SegmentationParametersEditor + + + + 0 + 0 + 400 + 130 + + + + + 400 + 115 + + + + Form + + + + + + Segmentation + + + + + + + + + + + + Min Cluster Size (triangles) + + + + + + + + + + Curvature Threshold (rad) + + + + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + 1 + + + 10000 + + + 100 + + + 500 + + + + + + + + + Segment + + + + + + + + diff --git a/opp_gui/ui/surface_selection_combo_widget.ui b/opp_gui/ui/surface_selection_combo_widget.ui new file mode 100644 index 0000000..486957e --- /dev/null +++ b/opp_gui/ui/surface_selection_combo_widget.ui @@ -0,0 +1,30 @@ + + + SurfaceSelectionComboWidget + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + + + + + + + + + + diff --git a/opp_gui/ui/tool_path_parameters_editor.ui b/opp_gui/ui/tool_path_parameters_editor.ui new file mode 100644 index 0000000..9d2a481 --- /dev/null +++ b/opp_gui/ui/tool_path_parameters_editor.ui @@ -0,0 +1,245 @@ + + + ToolPathParametersEditor + + + + 0 + 0 + 612 + 741 + + + + Form + + + + + + Tool Path Generation + + + + + + + + + Process Type + + + + + + + + + + Dwell Time (min) + + + + + + + + + + + + + + + + + Line Spacing (m) + + + + + + + 3 + + + 0.010000000000000 + + + 0.200000000000000 + + + + + + + + + + Point Spacing (m) + + + + + + + 3 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + + + + Tool Z-axis Offset (m) + + + + + + + 3 + + + 0.010000000000000 + + + + + + + + + + Min. Hole Size (m) + + + + + + + 3 + + + 0.010000000000000 + + + 0.025000000000000 + + + + + + + + + + Min. Segment Length (m) + + + + + + + 3 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + Intersecting Plane Height (m) + + + + + + + 3 + + + 0.050000000000000 + + + + + + + Raster Angle (deg) + + + + + + + 3 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + Tool Pitch (degrees) + + + + + + + 3 + + + -60.000000000000000 + + + 60.000000000000000 + + + + + + + + + Generate + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + diff --git a/opp_gui/ui/tool_path_planner.ui b/opp_gui/ui/tool_path_planner.ui new file mode 100644 index 0000000..62d9e2c --- /dev/null +++ b/opp_gui/ui/tool_path_planner.ui @@ -0,0 +1,568 @@ + + + ToolPathPlanner + + + + 0 + 0 + 754 + 1102 + + + + Form + + + + + + <html><head/><body><p><span style=" font-size:14pt; font-weight:600;">SwRI Part &amp; Tool Path Planner</span></p></body></html> + + + false + + + + + + + Qt::Horizontal + + + + + + + 0 + + + + Edit Parts + + + + + + +Use the GUI to define the properties of a new part or modify those of an existing part + + + + true + + + + + + + 0 + + + + + 0 + 0 + 714 + 848 + + + + 1. Load Part Model + + + + + + New Part + + + + + + + + Load New Model File + + + + + + + true + + + + + + + + + + + + Qt::Horizontal + + + + + + + Existing Part + + + + + + + + + + + Qt::Vertical + + + + + + + true + + + + + + + + + Load Selected Part + + + + + + + Refresh Parts List + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + 0 + 0 + 714 + 848 + + + + 2. Define Model Data + + + + + + 0 + + + + Touch Points + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + Verification Points + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + + + + + 0 + 0 + 714 + 848 + + + + 3. Save Model Data + + + + + + + + Name + + + + + + + + + + Description + + + + + + + + + + + + Save + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + Edit Jobs + + + + + + 0 + + + + + 0 + 0 + 714 + 905 + + + + 1. Load Job Data + + + + + + New Job + + + + + + Generate New Job + + + + + + + + + + Qt::Horizontal + + + + + + + Load Previous Job + + + + + + + + + + + Qt::Vertical + + + + + + + + + + + + Load Selected Job + + + + + + + Refresh Job Lists + + + + + + + + + + Qt::Vertical + + + + 20 + 296 + + + + + + + + + + 0 + 0 + 100 + 30 + + + + 2. Define Job Data + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + 0 + 0 + 175 + 158 + + + + 3. Save Job Data + + + + + + + + Job Name + + + + + + + + + + Description + + + + + + + + + + + + Save + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + Manage Database + + + + + + Manage Parts + + + + + + + + + + + Refresh + + + + + + + Show + + + + + + + Delete + + + + + + + + + + + + Manage Jobs + + + + + + + + + + + Refresh + + + + + + + Delete + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + diff --git a/opp_gui/ui/touch_point_parameters_editor.ui b/opp_gui/ui/touch_point_parameters_editor.ui new file mode 100644 index 0000000..3deada3 --- /dev/null +++ b/opp_gui/ui/touch_point_parameters_editor.ui @@ -0,0 +1,253 @@ + + + TouchPointParametersEditor + + + + 0 + 0 + 577 + 689 + + + + Form + + + + + + Measurement Threshold + + + + + + threshold (m) + + + + + + + -100000.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + (This value is ignored for touchoff +points, but used for verification.) + + + + + + + + + + Select with Mouse + + + + + + + Description + + + + + + + + + + Update + + + + + + + Position + + + + + + x (m) + + + + + + + y (m) + + + + + + + z (m) + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + + + + Orientation + + + + + + qw (rad) + + + + + + + qx (rad) + + + + + + + qy (rad) + + + + + + + qz (rad) + + + + + + + -100000.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + -100000.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + -100000.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + -100000.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + From 154b5110843799e767cfd1f1d9e1a7cc48bcbaa8 Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:06:33 -0600 Subject: [PATCH 06/12] Added Startup package --- opp_startup/CMakeLists.txt | 14 ++ opp_startup/config/planner.rviz | 169 ++++++++++++++++++ opp_startup/launch/planner_application.launch | 14 ++ opp_startup/package.xml | 13 ++ 4 files changed, 210 insertions(+) create mode 100644 opp_startup/CMakeLists.txt create mode 100644 opp_startup/config/planner.rviz create mode 100644 opp_startup/launch/planner_application.launch create mode 100644 opp_startup/package.xml diff --git a/opp_startup/CMakeLists.txt b/opp_startup/CMakeLists.txt new file mode 100644 index 0000000..fe65107 --- /dev/null +++ b/opp_startup/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(opp_startup) + +find_package(catkin REQUIRED) + +# Prevents certain runtime errors in Ubuntu 18.04 +add_definitions(${PCL_DEFINITIONS}) + +catkin_package() + +foreach(dir config launch) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach() diff --git a/opp_startup/config/planner.rviz b/opp_startup/config/planner.rviz new file mode 100644 index 0000000..cd31bd7 --- /dev/null +++ b/opp_startup/config/planner.rviz @@ -0,0 +1,169 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 341 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /Publish Point1 + - /Mesh Tool Cursor1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: opp_gui/ToolPathPlannerPanel + Name: ToolPathPlannerPanel +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /mesh_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: /tool_path + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /touch_point_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /roi_markers + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /target_area + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/PublishPoint + Single click: false + Topic: /clicked_point + - Class: rviz_tool_cursor/MeshToolCursor + Color: 255; 255; 255 + Mesh Filename: package://rviz_tool_cursor/resources/default.stl + Patch Size: 10 + Topic: /selection_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 37.458274841308594 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.501092255115509 + Y: 0.9473909139633179 + Z: 0.3266221284866333 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.25145435333251953 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.5652337074279785 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000037200000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000363000001d70100001cfa000000000100000002fb000000280054006f006f006c00500061007400680050006c0061006e006e0065007200500061006e0065006c0100000000ffffffff0000014e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c50000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + ToolPathPlannerPanel: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/opp_startup/launch/planner_application.launch b/opp_startup/launch/planner_application.launch new file mode 100644 index 0000000..7dd0035 --- /dev/null +++ b/opp_startup/launch/planner_application.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/opp_startup/package.xml b/opp_startup/package.xml new file mode 100644 index 0000000..0201492 --- /dev/null +++ b/opp_startup/package.xml @@ -0,0 +1,13 @@ + + + opp_startup + 0.0.0 + The opp_startup package + + mripperger + mripperger + + Apache 2.0 + + catkin + From 5b52c184f99155ebd21b31db0553dfb491546fee Mon Sep 17 00:00:00 2001 From: dmerz Date: Thu, 19 Dec 2019 14:07:03 -0600 Subject: [PATCH 07/12] Updated README.md and added rosinstall file --- README.md | 104 ++++++++++++++++++++++++++++++++++++++++++++++++- opp.rosinstall | 7 ++++ 2 files changed, 109 insertions(+), 2 deletions(-) create mode 100644 opp.rosinstall diff --git a/README.md b/README.md index a5ea4cc..70e4636 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,102 @@ -# Toolpath-Offline-Planning -Tools for generating offline toolpaths from CAD +# 10-24228 +ROS packages for generating offline toolpaths from CAD. Built to run on Ubuntu 16.04 or 18.04. + +## TODO +1) More thorough usage instructions +2) Pretty pictures + +## Install instructions +1) Clone the repo into your workspace. Then from the workspace root run `wstool init src opp.rosinstall` + +2) Run rosdep from the root of your workspace to pull dependencies `rosdep install --from-paths src --ignore-src -r -y` + + - Ensure that the following dependencies were downloaded - they are necessary to use the C++ database interface + + ``` + libqt5sql5-mysql + libmysqlclient-dev + libssl-dev + ``` + +3) Follow the instructions at the [noether repository](https://github.com/ros-industrial/noether) for installing the correct versions of PCL (1.9+) and VTK (1.7+). + + - Noether requires at least PCL 1.9 and VTK 1.7 + + +4) Build the workspace `catkin build` + +5) Source the workspace + +6) Set up the MySQL database. + - an "offline_generated_paths" folder needs to be created. (Press CTRL+H to show hidden folders): ~/.local/share/offline_generated_paths + - install MySQL and make sure you have root/admin access. + a) If you do not already have mysql installed: + 1. Install **mysql**: + + Follow the directions in the [mysql website to install the debian](https://dev.mysql.com/doc/mysql-apt-repo-quick-guide/en/#apt-repo-fresh-install) + + *NOTE: If you set a password you will need to remember it in order to log in into a mysql session. + 2. Initialize **mysql**: + Run the following command as root or with sudo in order to initialize mysql. We'll use the `--initialize-insecure` option at first: + ``` + mysqld --initialize-insecure --user=mysql + ``` + + Remember to use `sudo` if you aren't logged in as **root**. For more details go to the [mysql webpage](https://dev.mysql.com/doc/refman/8.0/en/data-directory-initialization-mysqld.html) + 3. Set Password + - Connect to the server as root with no password + ``` + mysql -u root --skip-password + ``` + **NOTE: Should this failed because a password is required then log in as follows:** + ``` + mysql -u root -p + ``` + *Enter your password when asked.* + - You'll now be in a mysql console session, next set `0000` as the password for the root account if one hasn't been set yet. (You should probably use something more secure than '0000', but what would be the fun in that?) + ``` + ALTER USER 'root'@'localhost' IDENTIFIED BY '0000'; + ``` + - Press Ctrl-D to log out + - Log in again using the password + ``` + mysql -u root -p + ``` + + *NOTES:* + - *For more info see the [mysql webpage](https://dev.mysql.com/doc/refman/8.0/en/default-privileges.html)* + - *Additional help on resetting the password can be found [here](https://www.digitalocean.com/community/tutorials/how-to-import-and-export-databases-and-reset-a-root-password-in-mysql)* + + b) If you do already have mysql installed, but you do not know the root password: + - My favorite hack for getting into a mysql server with an unknown password is detailed [here](https://askubuntu.com/questions/766900/mysql-doesnt-ask-for-root-password-when-installing), in the answer by pwxcoo. + + - an "opp" database needs to be created manually. The necessary tables will be automatically generated. + ``` + $ mysql -u root -p + mysql> CREATE DATABASE opp; + mysql> SHOW DATABASES; + ``` + - 'ros-client' mysql user needs to be created and its password should be '0000'. It only needs permission to the opp database created above. If you want it to have all permissions, you can use the command below. + ``` + $ mysql -u root -p + mysql> CREATE USER 'ros-client'@'localhost' IDENTIFIED BY '0000'; + mysql> GRANT ALL PRIVILEGES ON opp.* TO 'ros-client'@'localhost' WITH GRANT OPTION; + ``` + **NOTE: This example uses '0000' for the password. You could use a safer password, but this one is currently hardcoded.** + See [here](https://dev.mysql.com/doc/refman/8.0/en/adding-users.html) for more info on user privileges. + + +## Offline Tool Path Planner + +The tool path planner GUI is an RViz panel designed to facilitate the setup of new models and tool path +planning on those models. The GUI also hosts an interface to the database for saving this information. + +### Usage + +1. Build and source the package +2. Run the application + ``` + roslaunch opp_startup planner_application.launch + ``` +3. Follow the order of operations of the GUI + diff --git a/opp.rosinstall b/opp.rosinstall new file mode 100644 index 0000000..975a54c --- /dev/null +++ b/opp.rosinstall @@ -0,0 +1,7 @@ +# ROS-Install File + +# Noether +- git: {local-name: noether, uri: 'https://github.com/ros-industrial/noether.git', version: master} + +# RViz Tool Cursor +- git: {local-name: rviz_tool_cursor, uri: 'https://github.com/marip8/rviz_tool_cursor.git', version: master} From be6db92f927426d1ce9dbe4a03b41239ad406880 Mon Sep 17 00:00:00 2001 From: dmerz Date: Fri, 3 Jan 2020 16:40:12 -0600 Subject: [PATCH 08/12] Remove unnecessary whitespace --- opp_area_selection/src/area_selector.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/opp_area_selection/src/area_selector.cpp b/opp_area_selection/src/area_selector.cpp index fb310da..264d34c 100644 --- a/opp_area_selection/src/area_selector.cpp +++ b/opp_area_selection/src/area_selector.cpp @@ -218,7 +218,6 @@ std::vector AreaSelector::getPointsInROI( cluster_indices.push_back(output_set); } - if(cluster_indices.size() > 1) { ROS_INFO("%lu clusters found in ROI selection; choosing closest cluster to ROI selection centroid", cluster_indices.size()); From de344a8d84b229d87e68cfd2c96c67ee5703ec26 Mon Sep 17 00:00:00 2001 From: dmerz Date: Fri, 3 Jan 2020 16:46:51 -0600 Subject: [PATCH 09/12] Clarify usage of (void) --- opp_area_selection/src/selection_artist.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/opp_area_selection/src/selection_artist.cpp b/opp_area_selection/src/selection_artist.cpp index e552afb..fadba03 100644 --- a/opp_area_selection/src/selection_artist.cpp +++ b/opp_area_selection/src/selection_artist.cpp @@ -237,7 +237,8 @@ bool SelectionArtist::clearROIPointsCb( std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { - (void)req; + (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) { it->points.clear(); From b645af8fb5ea07653675c699deb43dfc5d3cd71a Mon Sep 17 00:00:00 2001 From: dmerz Date: Mon, 6 Jan 2020 15:11:33 -0600 Subject: [PATCH 10/12] Fixing Header Guard Naming Conventions --- .../include/opp_area_selection/filter.h | 6 +++--- .../include/opp_area_selection/filter_impl.h | 6 +++--- .../include/opp_gui/register_ros_msgs_for_qt.h | 6 +++--- .../opp_gui/widgets/database_browser_widget.h | 6 +++--- .../include/opp_gui/widgets/database_log_widget.h | 15 +++++++++------ .../widgets/polygon_area_selection_widget.h | 6 +++--- .../widgets/surface_selection_combo_widget.h | 6 +++--- .../opp_gui/widgets/tool_path_planner_widget.h | 6 +++--- .../opp_msgs_serialization/binary_serialization.h | 8 ++++---- .../include/opp_msgs_serialization/eigen_yaml.h | 6 +++--- .../opp_msgs_serialization/geometry_msgs_yaml.h | 6 +++--- .../opp_msgs_serialization/sensor_msgs_yaml.h | 6 +++--- .../include/opp_msgs_serialization/serialize.h | 6 +++--- .../opp_msgs_serialization/std_msgs_yaml.h | 6 +++--- .../opp_msgs_serialization/trajectory_msgs_yaml.h | 6 +++--- 15 files changed, 52 insertions(+), 49 deletions(-) diff --git a/opp_area_selection/include/opp_area_selection/filter.h b/opp_area_selection/include/opp_area_selection/filter.h index 1277a5f..f66de91 100644 --- a/opp_area_selection/include/opp_area_selection/filter.h +++ b/opp_area_selection/include/opp_area_selection/filter.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef FILTER_H -#define FILTER_H +#ifndef OPP_AREA_SELECTION_FILTER_H +#define OPP_AREA_SELECTION_FILTER_H #include @@ -44,4 +44,4 @@ bool planeFit( #include -#endif // 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 e5f4940..f257f2d 100644 --- a/opp_area_selection/include/opp_area_selection/filter_impl.h +++ b/opp_area_selection/include/opp_area_selection/filter_impl.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef AREA_SELECTOR_IMPL_H -#define AREA_SELECTOR_IMPL_H +#ifndef OPP_AREA_SELECTION_FILTER_IMPL_H +#define OPP_AREA_SELECTION_FILTER_IMPL_H #include @@ -72,4 +72,4 @@ bool planeFit(const CloudPtr input_cloud, } // end namespace opp_area_selection -#endif // AREA_SELECTOR_IMPL_H +#endif // OPP_AREA_SELECTION_FILTER_IMPL_H 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 b3992ed..89e8ae3 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 @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef REGISTER_ROS_MSGS_FOR_QT_H -#define REGISTER_ROS_MSGS_FOR_QT_H +#ifndef OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H +#define OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H #include @@ -26,4 +26,4 @@ Q_DECLARE_METATYPE(shape_msgs::Mesh::Ptr); Q_DECLARE_METATYPE(std::vector); Q_DECLARE_METATYPE(std::vector); -#endif // REGISTER_ROS_MSGS_FOR_QT_H +#endif // OPP_GUI_REGISTER_ROS_MSGS_FOR_QT_H diff --git a/opp_gui/include/opp_gui/widgets/database_browser_widget.h b/opp_gui/include/opp_gui/widgets/database_browser_widget.h index b423cbb..f44b267 100644 --- a/opp_gui/include/opp_gui/widgets/database_browser_widget.h +++ b/opp_gui/include/opp_gui/widgets/database_browser_widget.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef DATABASE_BROWSER_H -#define DATABASE_BROWSER_H +#ifndef OPP_GUI_WIDGETS_DATABASE_BROWSER_H +#define OPP_GUI_WIDGETS_DATABASE_BROWSER_H #include #include @@ -76,4 +76,4 @@ protected Q_SLOTS: } // namespace opp_gui -#endif // DATABASE_BROWSER_H +#endif // OPP_GUI_WIDGETS_DATABASE_BROWSER_H diff --git a/opp_gui/include/opp_gui/widgets/database_log_widget.h b/opp_gui/include/opp_gui/widgets/database_log_widget.h index 837971b..3d32b4c 100644 --- a/opp_gui/include/opp_gui/widgets/database_log_widget.h +++ b/opp_gui/include/opp_gui/widgets/database_log_widget.h @@ -14,16 +14,19 @@ * limitations under the License. */ -#ifndef DATABASE_LOG_WIDGET_H -#define DATABASE_LOG_WIDGET_H +#ifndef OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_H +#define OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_H + +#include #include #include #include -#include + #include -#include -#include // TODO add the Log service to msgs, and change this to Log.h + +#include +#include namespace Ui { class DatabaseLog; @@ -63,4 +66,4 @@ protected Q_SLOTS: }; } // end of namespace opp_gui -#endif // DATABASE_LOG_WIDGET_H +#endif // OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_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 d39459f..fce6f9f 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 @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef POLYGON_AREA_SELECTION_WIDGET_H -#define POLYGON_AREA_SELECTION_WIDGET_H +#ifndef OPP_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H +#define OPP_GUI_WIDGETS_POLYGON_AREA_SELECTION_WIDGET_H #include @@ -67,4 +67,4 @@ private Q_SLOTS: } // end namespace opp_gui -#endif // 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 563b8be..866f738 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 @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef SURFACE_SELECTION_COMBO_WIDGET_H -#define SURFACE_SELECTION_COMBO_WIDGET_H +#ifndef OPP_GUI_WIDGETS_SURFACE_SELECTION_COMBO_WIDGET_H +#define OPP_GUI_WIDGETS_SURFACE_SELECTION_COMBO_WIDGET_H #include @@ -87,4 +87,4 @@ private Q_SLOTS: } // end namespace opp_gui -#endif // 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_planner_widget.h b/opp_gui/include/opp_gui/widgets/tool_path_planner_widget.h index 97719dc..44108b6 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 @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef TOOL_PATH_PLANNER_WIDGET_H -#define TOOL_PATH_PLANNER_WIDGET_H +#ifndef OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H +#define OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H #include #include @@ -119,4 +119,4 @@ protected Q_SLOTS: } // namespace opp_gui -#endif // TOOL_PATH_PLANNER_WIDGET_H +#endif // OPP_GUI_WIDGETS_TOOL_PATH_PLANNER_WIDGET_H 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 b09640e..b48df96 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h @@ -1,5 +1,5 @@ -#ifndef MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H -#define MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H +#ifndef OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H +#define OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H #include #include @@ -51,6 +51,6 @@ inline bool deserializeFromBinary(const std::string& file, return true; } -} // namespace amsted_opp_msgs_serialization +} // namespace opp_msgs_serialization -#endif // AMSTED_MESSAGE_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 16da41f..936091d 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_EIGEN_YAML_H -#define MESSAGE_SERIALIZATION_EIGEN_YAML_H +#ifndef OPP_MSGS_SERIALIZATION_EIGEN_YAML_H +#define OPP_MSGS_SERIALIZATION_EIGEN_YAML_H #include #include @@ -64,4 +64,4 @@ struct convert } -#endif // MESSAGE_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 ed5ef44..7747b91 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 @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML -#define MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML +#ifndef OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML +#define OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML #include #include @@ -215,4 +215,4 @@ struct convert } -#endif // MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML 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 13b33f6..a9befa7 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 @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML -#define MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML +#ifndef OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML +#define OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML #include #include @@ -141,4 +141,4 @@ struct convert } // namespace YAML -#endif // MESSAGE_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 698005d..afd1b12 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_SERIALIZE_H -#define MESSAGE_SERIALIZATION_SERIALIZE_H +#ifndef OPP_MSGS_SERIALIZATION_SERIALIZE_H +#define OPP_MSGS_SERIALIZATION_SERIALIZE_H #include #include @@ -57,4 +57,4 @@ bool deserialize(const std::string &file, T& val) } // namespace opp_msgs_serialization -#endif // MESSAGE_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 f8526ac..0f8b633 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 @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_STD_MSGS_YAML -#define MESSAGE_SERIALIZATION_STD_MSGS_YAML +#ifndef OPP_MSGS_SERIALIZATION_STD_MSGS_YAML +#define OPP_MSGS_SERIALIZATION_STD_MSGS_YAML #include #include @@ -72,4 +72,4 @@ struct convert } -#endif // MESSAGE_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 1ef37c0..f99d1bb 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 @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML -#define MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML +#ifndef OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML +#define OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML #include #include @@ -77,4 +77,4 @@ struct convert } -#endif // MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML +#endif // OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML From c8ba0881909423cefee7fbbb6edfdeaf223ad8eb Mon Sep 17 00:00:00 2001 From: dmerz Date: Mon, 6 Jan 2020 15:36:25 -0600 Subject: [PATCH 11/12] Removing Unnecessary Widgets and Corresponding Files --- opp_gui/CMakeLists.txt | 2 - .../opp_gui/widgets/database_browser_widget.h | 79 ---- .../opp_gui/widgets/database_log_widget.h | 69 ---- .../widgets/database_selection_widget.h | 128 ------ .../src/widgets/database_browser_widget.cpp | 71 ---- opp_gui/src/widgets/database_log_widget.cpp | 69 ---- .../src/widgets/database_selection_widget.cpp | 389 ------------------ opp_gui/ui/database_browser.ui | 113 ----- opp_gui/ui/database_log.ui | 38 -- 9 files changed, 958 deletions(-) delete mode 100644 opp_gui/include/opp_gui/widgets/database_browser_widget.h delete mode 100644 opp_gui/include/opp_gui/widgets/database_log_widget.h delete mode 100644 opp_gui/include/opp_gui/widgets/database_selection_widget.h delete mode 100644 opp_gui/src/widgets/database_browser_widget.cpp delete mode 100644 opp_gui/src/widgets/database_log_widget.cpp delete mode 100644 opp_gui/src/widgets/database_selection_widget.cpp delete mode 100644 opp_gui/ui/database_browser.ui delete mode 100644 opp_gui/ui/database_log.ui diff --git a/opp_gui/CMakeLists.txt b/opp_gui/CMakeLists.txt index 0788bca..f623618 100644 --- a/opp_gui/CMakeLists.txt +++ b/opp_gui/CMakeLists.txt @@ -70,7 +70,6 @@ include_directories( ) set(rpg_gui_UIS - ui/database_browser.ui ui/list_editor.ui ui/polygon_area_selection_widget.ui ui/segmentation_parameters_editor.ui @@ -78,7 +77,6 @@ set(rpg_gui_UIS ui/tool_path_parameters_editor.ui ui/tool_path_planner.ui ui/touch_point_parameters_editor.ui - ui/database_log.ui ) set(rpg_gui_widget_HEADERS diff --git a/opp_gui/include/opp_gui/widgets/database_browser_widget.h b/opp_gui/include/opp_gui/widgets/database_browser_widget.h deleted file mode 100644 index f44b267..0000000 --- a/opp_gui/include/opp_gui/widgets/database_browser_widget.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef OPP_GUI_WIDGETS_DATABASE_BROWSER_H -#define OPP_GUI_WIDGETS_DATABASE_BROWSER_H - -#include -#include -#include -#include -#include - -class QTreeWidgetItem; - -namespace Ui -{ -class DatabaseBrowser; -} - -namespace opp_gui -{ - -/** - * @brief Widget that displays all parts and jobs known to the database in a tree hierarchy form - */ -class DatabaseBrowserWidget : public QWidget -{ -Q_OBJECT -public: - - DatabaseBrowserWidget(QWidget* parent = nullptr); - - virtual ~DatabaseBrowserWidget() = default; - -Q_SIGNALS: - - /** - * @brief Signal indicating whether or not the selection in the tree (part or job) has been set - * active in the application. This signal can be used by dependent widgets (namely a higher level - * application GUI) to determine whether they can proceed in their process - * @param status - */ - void okToProceed(bool status); - -protected Q_SLOTS: - - void onTreeItemSelected(QTreeWidgetItem* item); - -protected: - - Ui::DatabaseBrowser* ui_; - - ros::NodeHandle nh_; - ros::ServiceClient get_all_parts_client_; - ros::ServiceClient get_all_jobs_client_; - - std::map part_map_; - std::map job_map_; - - const static int PART_ROLE = Qt::ItemDataRole::UserRole + 1; - const static int JOB_ROLE = Qt::ItemDataRole::UserRole + 2; -}; - -} // namespace opp_gui - -#endif // OPP_GUI_WIDGETS_DATABASE_BROWSER_H diff --git a/opp_gui/include/opp_gui/widgets/database_log_widget.h b/opp_gui/include/opp_gui/widgets/database_log_widget.h deleted file mode 100644 index 3d32b4c..0000000 --- a/opp_gui/include/opp_gui/widgets/database_log_widget.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_H -#define OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_H - -#include - -#include -#include -#include - -#include - -#include -#include - -namespace Ui { - class DatabaseLog; -} - -class QSqlTableModel; - -namespace opp_gui -{ - -/** - * @brief Widget that displays all recent jobs performed - */ -class DatabaseLogWidget : public QWidget -{ - Q_OBJECT - - public: - - DatabaseLogWidget(opp_application::ApplicationContextBasePtr app, - QWidget *parent = nullptr); - - ~DatabaseLogWidget(); - -protected Q_SLOTS: - void refresh(); - void filter(const QString& text); - -protected: - - Ui::DatabaseLog* ui_; - ros::NodeHandle nh_; - QTimer refresh_timer_; - QSqlTableModel *model_; - - opp_application::ApplicationContextBasePtr app_; -}; -} // end of namespace opp_gui - -#endif // OPP_GUI_WIDGETS_DATABASE_LOG_WIDGET_H diff --git a/opp_gui/include/opp_gui/widgets/database_selection_widget.h b/opp_gui/include/opp_gui/widgets/database_selection_widget.h deleted file mode 100644 index 44f9b7a..0000000 --- a/opp_gui/include/opp_gui/widgets/database_selection_widget.h +++ /dev/null @@ -1,128 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_H -#define OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_H - -#include - -#include - -namespace opp_application -{ -class ApplicationContextBase; -typedef typename std::shared_ptr ApplicationContextBasePtr; -typedef typename boost::function NotifyCbType; -} - -namespace opp_gui -{ - -/** - * @brief Inherits the DatabaseBrowserWidget class and uses its functionality to set the active part - * in the application from the parts available in the database - */ -class PartSelectorWidget : public DatabaseBrowserWidget -{ -Q_OBJECT -public: - - /** - * @brief PartSelectorWidget - Constructor that sets the initial part - * transform to identity - * @param app - input - the application context object being used to - * connect to the underlying ROS system - * @param parent - input - the QT object which is this widget's parent - */ - PartSelectorWidget(opp_application::ApplicationContextBasePtr app, - QWidget* parent = nullptr); - - /** - * @brief PartSelectorWidget - Constructor that sets the initial part - * transform to a calling-code-defined transform - * @param app - input - the application context object being used to - * connect to the underlying ROS system - * @param initial_part_location - input - the position at which parts - * should first appear when added - * @param parent - input - the QT object which is this widget's parent - */ - PartSelectorWidget(opp_application::ApplicationContextBasePtr app, - const geometry_msgs::PoseStamped initial_part_location, - QWidget* parent = nullptr); - - public: - /** &brief the currently selected part's work control document number **/ - std::string selected_part_wcd_; - -protected Q_SLOTS: - - /** @brief a function that is called whenever the 'select part' button - * in this widget is clicked. It adds the part to the list of parts in - * the application context object. - */ - bool onSelectPart(); - /** @brief a function that is called whenever the WID is entered - * It enables the "Select" button so a part is added - */ - void onEnterWID(); - /** @brief loads all parts/jobs into the part selector widget - */ - void onLoadDatabase(); - -private: - - /** @brief the application context ojbect used to connect to the system */ - opp_application::ApplicationContextBasePtr app_; - - /** @brief the position at which parts should first appear */ - geometry_msgs::PoseStamped initial_part_location_; - -}; - -/** - * @brief Inherits the DatabaseBrowserWidget class and uses its functionality to set the active job - * in the application from the jobs available in the database - */ -class JobSelectorWidget : public DatabaseBrowserWidget -{ -Q_OBJECT -public: - - JobSelectorWidget(opp_application::ApplicationContextBasePtr app, - QWidget* parent = nullptr); - /** @brief a function that is called when the active part is chagned - * displays the active parts jobs and hides the jobs for all other parts - */ - void onPartSelect(); -protected Q_SLOTS: - - /** @brief a function that is called whenever the select job button is pressed - */ - void onSelectJob(); - /** @brief loads all parts/jobs into the part selector widget. - * Only shows the entry for the active job - */ - void onLoadDatabase(); - - -private: - - opp_application::ApplicationContextBasePtr app_; -}; - -} // namespace opp_gui - -#endif // OPP_GUI_WIDGETS_DATABASE_SELECTION_WIDGET_H diff --git a/opp_gui/src/widgets/database_browser_widget.cpp b/opp_gui/src/widgets/database_browser_widget.cpp deleted file mode 100644 index 0e8dd6a..0000000 --- a/opp_gui/src/widgets/database_browser_widget.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include "ui_database_browser.h" - -const static std::string GET_ALL_PARTS_SERVICE = "get_all_parts_from_db"; -const static std::string GET_ALL_JOBS_SERVICE = "get_all_jobs_from_db"; - -namespace opp_gui -{ - -DatabaseBrowserWidget::DatabaseBrowserWidget(QWidget* parent) - : QWidget(parent) -{ - ui_ = new Ui::DatabaseBrowser(); - ui_->setupUi(this); - - // Disable the jobs pane until a part selection has been made - ui_->tree_widget_db_entries->setEnabled(false); - ui_->text_edit_description->setEnabled(false); - ui_->push_button_select->setEnabled(false); - - connect(ui_->tree_widget_db_entries, &QTreeWidget::itemClicked, this, &DatabaseBrowserWidget::onTreeItemSelected); - - get_all_parts_client_ = nh_.serviceClient(GET_ALL_PARTS_SERVICE); - get_all_jobs_client_ = nh_.serviceClient(GET_ALL_JOBS_SERVICE); -} - - -void DatabaseBrowserWidget::onTreeItemSelected(QTreeWidgetItem* item) -{ - if(item) - { - QVariant part_data = item->data(0, PART_ROLE); - auto part_it = part_map_.find(qvariant_cast(part_data)); - - if(part_it != part_map_.end()) - { - ui_->text_edit_description->setText(QString::fromStdString(part_it->second.description)); - } - - QVariant job_data = item->data(0, JOB_ROLE); - auto job_it = job_map_.find(qvariant_cast(job_data)); - - if(job_it != job_map_.end()) - { - ui_->text_edit_description->setText(QString::fromStdString(job_it->second.description)); - } - } - - emit okToProceed(false); -} - -} // namespace opp_gui diff --git a/opp_gui/src/widgets/database_log_widget.cpp b/opp_gui/src/widgets/database_log_widget.cpp deleted file mode 100644 index 8aa29e8..0000000 --- a/opp_gui/src/widgets/database_log_widget.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "opp_gui/widgets/database_log_widget.h" -#include "ui_database_log.h" -#include -#include -#include - -namespace opp_gui -{ - -DatabaseLogWidget::DatabaseLogWidget(opp_application::ApplicationContextBasePtr app, - QWidget *parent) : - QWidget(parent) - , app_(app) -{ - ui_ = new Ui::DatabaseLog(); - ui_->setupUi(this); - - model_ = new QSqlTableModel(this, app_->database_.getDatabase()); - model_->setTable(QString::fromStdString(opp_db::JOB_LOG_TABLE_NAME)); - model_->setEditStrategy(QSqlTableModel::OnManualSubmit); - model_->select(); - model_->removeColumn(0); // don't show the ID - - connect(&refresh_timer_, SIGNAL(timeout()), this, SLOT(refresh())); - refresh_timer_.setInterval(10000); - refresh_timer_.start(); - - connect(ui_->line_edit_wcd_entry, &QLineEdit::textChanged, this, &DatabaseLogWidget::filter); - - // Attach it to the view - ui_->log_table_view->setModel(model_); - ui_->log_table_view->setEditTriggers(QTableView::NoEditTriggers); // Make read only - ui_->log_table_view->show(); -} - -DatabaseLogWidget::~DatabaseLogWidget() -{ - delete ui_; -} - -void DatabaseLogWidget::refresh() -{ - model_->select(); - refresh_timer_.start(); -} - -void DatabaseLogWidget::filter(const QString& text) -{ - std::string query = "`work control document number` LIKE \"" + text.toStdString() + "%\""; - model_->setFilter(QString::fromStdString(query)); -} - -}// end namespace opp_gui diff --git a/opp_gui/src/widgets/database_selection_widget.cpp b/opp_gui/src/widgets/database_selection_widget.cpp deleted file mode 100644 index 181340a..0000000 --- a/opp_gui/src/widgets/database_selection_widget.cpp +++ /dev/null @@ -1,389 +0,0 @@ -/* - * 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. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include - -#include -#include -#include -#include -#include "ui_database_browser.h" - -namespace opp_gui -{ - -// Constructor that uses default initial-transform value -PartSelectorWidget::PartSelectorWidget(opp_application::ApplicationContextBasePtr app, - QWidget* parent) - : DatabaseBrowserWidget(parent) - , app_(app), - selected_part_wcd_("") -{ - // Initialize the initial_part_location transform to identity - initial_part_location_.pose.orientation.w = 1.0; - - // Connect the button - ui_->push_button_select->setText("Add Part"); - ui_->tree_widget_db_entries->setRootIsDecorated(false); - ui_->WorkCD_lineEdit->setReadOnly(false); - connect(ui_->push_button_update, &QPushButton::clicked, this, &PartSelectorWidget::onLoadDatabase); - connect(ui_->push_button_select, &QPushButton::clicked, this, &PartSelectorWidget::onSelectPart); - - // Connect the lineEdit - connect(ui_->WorkCD_lineEdit, &QLineEdit::editingFinished, this, &PartSelectorWidget::onEnterWID); - ui_->WCD_Label->setText("Enter WCD"); -} - -// Constructor that specifies the initial transform -PartSelectorWidget::PartSelectorWidget(opp_application::ApplicationContextBasePtr app, - const geometry_msgs::PoseStamped initial_part_location, - QWidget* parent) - : DatabaseBrowserWidget(parent) - , app_(app) - , initial_part_location_(initial_part_location), - selected_part_wcd_("") -{ - // Connect the button - ui_->push_button_select->setText("Add Part"); - ui_->tree_widget_db_entries->setRootIsDecorated(false); - connect(ui_->push_button_update, &QPushButton::clicked, this, &PartSelectorWidget::onLoadDatabase); - connect(ui_->push_button_select, &QPushButton::clicked, this, &PartSelectorWidget::onSelectPart); - - // Connect the lineEdit - connect(ui_->WorkCD_lineEdit, &QLineEdit::editingFinished, this, &PartSelectorWidget::onEnterWID); -} - -bool PartSelectorWidget::onSelectPart() -{ - selected_part_wcd_ = ui_->WorkCD_lineEdit->text().toStdString(); - if(selected_part_wcd_ == ""){ - QMessageBox MB; - MB.setText("No Work ID entered"); - MB.exec(); - return false; - } - QTreeWidgetItem* current = ui_->tree_widget_db_entries->currentItem(); - if(current) - { - // Define a lambda for attempting to set the current part - auto try_set_part = [this](QVariant& part_data)->bool - { - auto part_it = part_map_.find(qvariant_cast(part_data)); - - if(part_it != part_map_.end()) - { - // Set the active part - opp_application::TaskResponse res = app_->addPart(part_it->second, initial_part_location_, selected_part_wcd_); - if(res.success) - { - emit okToProceed(true); - ui_->WorkCD_lineEdit->setText(""); - return true; - } - else - { - QMessageBox::warning(this, "Application Error", QString::fromStdString(res.message)); - } - } - - return false; - }; - - // Get the data from the current selection - QVariant part_data = current->data(0, PART_ROLE); - - // Try to set the active part in the application - if(try_set_part(part_data)) - { - ui_->WorkCD_lineEdit->setText(""); - return true; - } - else - { - /* Maybe the user has selected a job; get the current selection's parent (which may be a part) - * and try to set it as the active part */ - QTreeWidgetItem* part = current->parent(); - - // Make sure the current selection has a parent - if(part) - { - part_data = part->data(0, PART_ROLE); - if(try_set_part(part_data)) - { - ui_->WorkCD_lineEdit->setText(""); - return true; - } - } - else - { - QMessageBox::warning(this, "Widget Error", "The current selection does not have a parent"); - } - } - } - else - { - QMessageBox::warning(this, "Input Error", "Please select a part in the tree"); - } - - emit okToProceed(false); -} - -void PartSelectorWidget::onEnterWID() -{ - // TODO add something here if necessary or delete. This function was created for debugging, but no longer used -} - -void PartSelectorWidget::onLoadDatabase() -{ - ui_->tree_widget_db_entries->clear(); - part_map_.clear(); - job_map_.clear(); - - opp_msgs::GetAllPartsFromDatabase srv; - if(!get_all_parts_client_.call(srv)) - { - std::string message = "Failed to call '" + get_all_parts_client_.getService() + "' service"; - QMessageBox::warning(this, "ROS Communication Error", QString::fromStdString(message)); - return; - } - else if(!srv.response.success) - { - QMessageBox::warning(this, "Database Error", QString::fromStdString(srv.response.message)); - return; - } - - // Error message - std::string job_load_error; - // Iterate over the parts and add the jobs associated with each one - ui_->tree_widget_db_entries->setSortingEnabled(false); - for(const opp_msgs::Part& part : srv.response.parts) - { - // Add the current part to the persistent part map - part_map_.emplace(part.id, part); - - // Add a QTreeWidgetItem for the part - QTreeWidgetItem* q_part = new QTreeWidgetItem(); - q_part->setText(0, QString::fromStdString(part.name)); - q_part->setData(0, PART_ROLE, QVariant(part.id)); - - // Call the service to get the jobs for this part from the database - opp_msgs::GetAllJobsFromDatabase srv; - srv.request.part_id = part.id; - - if(!get_all_jobs_client_.call(srv)) - { - job_load_error += part.name + ": Failed to call '" + get_all_jobs_client_.getService() + "' service\n"; - } - else if(!srv.response.success) - { - job_load_error += part.name + "\n"; - } - else - { - // Iterate over all of the jobs associated with this part and add them as children to the tree visualization - for(const opp_msgs::Job& job : srv.response.jobs) - { - job_map_.emplace(job.id, job); - - QTreeWidgetItem* q_job = new QTreeWidgetItem(); - q_job->setText(0, QString::fromStdString(job.name)); - q_job->setData(0, JOB_ROLE, QVariant(job.id)); - - q_part->addChild(q_job); - } - } - - // Add the part and its children jobs to the tree widget - ui_->tree_widget_db_entries->addTopLevelItem(q_part); - } - ui_->tree_widget_db_entries->setSortingEnabled(true); - - // Enable the panes - ui_->tree_widget_db_entries->setEnabled(true); - ui_->text_edit_description->setEnabled(true); - ui_->push_button_select->setEnabled(true); - - if(!job_load_error.empty()) - { - std::string message = "Failed to load jobs for the following parts:\n\n" + job_load_error; - QMessageBox::warning(this, "Database Error", QString::fromStdString(message)); - } - - emit okToProceed(false); -} - - -JobSelectorWidget::JobSelectorWidget(opp_application::ApplicationContextBasePtr app, - QWidget* parent) - : DatabaseBrowserWidget(parent) - , app_(app) -{ - ui_->push_button_select->setText("Select Job"); - ui_->WorkCD_lineEdit->setText(app_->active_part_->second.work_control_document.c_str()); - ui_->WCD_Label->setText("Active WCD"); - ui_->WorkCD_lineEdit->setReadOnly(true); - connect(ui_->push_button_update, &QPushButton::clicked, this, &JobSelectorWidget::onLoadDatabase); - connect(ui_->push_button_select, &QPushButton::clicked, this, &JobSelectorWidget::onSelectJob); -} - -void JobSelectorWidget::onSelectJob() -{ - QTreeWidgetItem* current = ui_->tree_widget_db_entries->currentItem(); - if(current) - { - QVariant job_data = current->data(0, JOB_ROLE); - auto job_it = job_map_.find(qvariant_cast(job_data)); - - if(job_it != job_map_.end()) - { - // Set the active job - ui_->WorkCD_lineEdit->setText(app_->active_part_->second.work_control_document.c_str()); - opp_application::TaskResponse res = app_->setActiveJob(job_it->second); - if(res.success) - { - emit okToProceed(true); - return; - } - else - { - QMessageBox::warning(this, "Application Error", QString::fromStdString(res.message)); - } - } - else - { - QMessageBox::warning(this, "Input Error", "Selection is not a known job"); - } - } - else - { - QMessageBox::warning(this, "Input Error", "Please select a job in the tree"); - } - - emit okToProceed(false); -} - -void JobSelectorWidget::onPartSelect() -{ - int topCount_ = ui_->tree_widget_db_entries->topLevelItemCount(); - for (int i = 0; i < topCount_; i++) - { - QTreeWidgetItem* current = ui_->tree_widget_db_entries->topLevelItem(i); - if(current) - { - QVariant part_data = current->data(0, PART_ROLE); - auto part_it = part_map_.find(qvariant_cast(part_data)); - - if(app_->active_part_->second.part.id == part_it->second.id) - { - current->setHidden(false); - current->setExpanded(true); - } - else - { - current->setHidden(true); - } - } - } -} - -void JobSelectorWidget::onLoadDatabase() -{ - ui_->tree_widget_db_entries->clear(); - part_map_.clear(); - job_map_.clear(); - - opp_msgs::GetAllPartsFromDatabase srv; - if(!get_all_parts_client_.call(srv)) - { - std::string message = "Failed to call '" + get_all_parts_client_.getService() + "' service"; - QMessageBox::warning(this, "ROS Communication Error", QString::fromStdString(message)); - return; - } - else if(!srv.response.success) - { - QMessageBox::warning(this, "Database Error", QString::fromStdString(srv.response.message)); - return; - } - - // Error message - std::string job_load_error; - // Iterate over the parts and add the jobs associated with each one - ui_->tree_widget_db_entries->setSortingEnabled(false); - for(const opp_msgs::Part& part : srv.response.parts) - { - // Add the current part to the persistent part map - part_map_.emplace(part.id, part); - - // Add a QTreeWidgetItem for the part - QTreeWidgetItem* q_part = new QTreeWidgetItem(); - q_part->setText(0, QString::fromStdString(part.name)); - q_part->setData(0, PART_ROLE, QVariant(part.id)); - - // Call the service to get the jobs for this part from the database - opp_msgs::GetAllJobsFromDatabase srv; - srv.request.part_id = part.id; - - if(!get_all_jobs_client_.call(srv)) - { - job_load_error += part.name + ": Failed to call '" + get_all_jobs_client_.getService() + "' service\n"; - } - else if(!srv.response.success) - { - job_load_error += part.name + "\n"; - } - else - { - // Iterate over all of the jobs associated with this part and add them as children to the tree visualization - for(const opp_msgs::Job& job : srv.response.jobs) - { - job_map_.emplace(job.id, job); - - QTreeWidgetItem* q_job = new QTreeWidgetItem(); - q_job->setText(0, QString::fromStdString(job.name)); - q_job->setData(0, JOB_ROLE, QVariant(job.id)); - - q_part->addChild(q_job); - } - } - // Add the part and its children jobs to the tree widget - ui_->tree_widget_db_entries->addTopLevelItem(q_part); - QTreeWidgetItem* current = ui_->tree_widget_db_entries->topLevelItem(ui_->tree_widget_db_entries->topLevelItemCount()-1); - if(part.id != app_->active_part_->second.part.id) - { - current->setHidden(true); - } - else - { - current->setExpanded(true); - current->setHidden(false); - } - } - ui_->tree_widget_db_entries->setSortingEnabled(true); - - // Enable the panes - ui_->tree_widget_db_entries->setEnabled(true); - ui_->text_edit_description->setEnabled(true); - ui_->push_button_select->setEnabled(true); - - if(!job_load_error.empty()) - { - std::string message = "Failed to load jobs for the following parts:\n\n" + job_load_error; - QMessageBox::warning(this, "Database Error", QString::fromStdString(message)); - } - - emit okToProceed(false); -} -} // namespace opp_gui diff --git a/opp_gui/ui/database_browser.ui b/opp_gui/ui/database_browser.ui deleted file mode 100644 index af1c176..0000000 --- a/opp_gui/ui/database_browser.ui +++ /dev/null @@ -1,113 +0,0 @@ - - - DatabaseBrowser - - - - 0 - 0 - 525 - 447 - - - - Form - - - - - - - - - 0 - 0 - - - - Qt::ScrollBarAlwaysOff - - - QAbstractScrollArea::AdjustToContentsOnFirstShow - - - - Database Entries - - - - - - - - Qt::Vertical - - - - - - - Description - - - - - - QAbstractScrollArea::AdjustToContents - - - true - - - - - - - - - - - - - - Update List - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Enter WCD - - - - - - - - - - Select - - - - - - - - - - diff --git a/opp_gui/ui/database_log.ui b/opp_gui/ui/database_log.ui deleted file mode 100644 index fa8cd54..0000000 --- a/opp_gui/ui/database_log.ui +++ /dev/null @@ -1,38 +0,0 @@ - - - DatabaseLog - - - - 0 - 0 - 400 - 300 - - - - Form - - - - - - true - - - - - - - Search by WCD - - - - - - - - - - - From 4dadc67f58ee15cdec4f9910612b85b9184fc732 Mon Sep 17 00:00:00 2001 From: dmerz Date: Mon, 6 Jan 2020 16:20:16 -0600 Subject: [PATCH 12/12] Reformatting Includes --- .../include/opp_area_selection/area_selector.h | 2 +- .../include/opp_area_selection/filter.h | 2 +- .../include/opp_area_selection/filter_impl.h | 2 +- opp_area_selection/src/area_selection_node.cpp | 4 ++-- opp_area_selection/src/area_selector.cpp | 4 ++-- opp_area_selection/src/selection_artist.cpp | 6 +++--- opp_database/src/opp_database_interface_cpp.cpp | 2 +- opp_gui/include/opp_gui/tool_path_planner_panel.h | 2 +- opp_gui/include/opp_gui/utils.h | 7 ++++--- .../opp_gui/widgets/polygon_area_selection_widget.h | 2 +- .../widgets/segmentation_parameters_editor_widget.h | 2 +- .../opp_gui/widgets/surface_selection_combo_widget.h | 6 +++--- .../include/opp_gui/widgets/tool_path_editor_widget.h | 6 +++--- .../widgets/tool_path_parameters_editor_widget.h | 4 +++- .../opp_gui/widgets/touch_point_editor_widget.h | 10 ++++++---- .../widgets/touch_point_parameters_editor_widget.h | 5 ++++- opp_gui/src/nodes/segmentation_demo_app.cpp | 9 +++++---- opp_gui/src/nodes/tool_path_editor_demo_app.cpp | 8 +++++--- opp_gui/src/nodes/tool_path_planner_demo_app.cpp | 8 +++++--- opp_gui/src/nodes/touch_point_editor_demo_app.cpp | 6 ++++-- opp_gui/src/tool_path_planner_panel.cpp | 6 ++++-- opp_gui/src/utils.cpp | 5 +++-- opp_gui/src/widgets/polygon_area_selection_widget.cpp | 2 +- .../widgets/segmentation_parameters_editor_widget.cpp | 4 ++-- .../src/widgets/surface_selection_combo_widget.cpp | 6 +++--- opp_gui/src/widgets/tool_path_editor_widget.cpp | 4 ++-- .../widgets/tool_path_parameters_editor_widget.cpp | 6 +++--- opp_gui/src/widgets/tool_path_planner_widget.cpp | 10 +++++----- opp_gui/src/widgets/touch_point_editor_widget.cpp | 11 +++++++---- .../widgets/touch_point_parameters_editor_widget.cpp | 4 ++-- .../opp_msgs_serialization/binary_serialization.h | 1 + .../include/opp_msgs_serialization/eigen_yaml.h | 3 ++- .../opp_msgs_serialization/geometry_msgs_yaml.h | 5 +++-- .../include/opp_msgs_serialization/opp_msgs_yaml.h | 4 ++-- .../include/opp_msgs_serialization/sensor_msgs_yaml.h | 3 ++- .../include/opp_msgs_serialization/serialize.h | 2 ++ .../include/opp_msgs_serialization/std_msgs_yaml.h | 3 ++- .../opp_msgs_serialization/trajectory_msgs_yaml.h | 3 ++- 38 files changed, 104 insertions(+), 75 deletions(-) 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 d25f77b..3efd601 100644 --- a/opp_area_selection/include/opp_area_selection/area_selector.h +++ b/opp_area_selection/include/opp_area_selection/area_selector.h @@ -25,7 +25,7 @@ #include #include -#include +#include "opp_area_selection/area_selector_parameters.h" namespace opp_area_selection { diff --git a/opp_area_selection/include/opp_area_selection/filter.h b/opp_area_selection/include/opp_area_selection/filter.h index f66de91..8029a51 100644 --- a/opp_area_selection/include/opp_area_selection/filter.h +++ b/opp_area_selection/include/opp_area_selection/filter.h @@ -42,6 +42,6 @@ bool planeFit( } // end namespace opp_area_selection -#include +#include "opp_area_selection/filter_impl.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 f257f2d..d37e20c 100644 --- a/opp_area_selection/include/opp_area_selection/filter_impl.h +++ b/opp_area_selection/include/opp_area_selection/filter_impl.h @@ -17,7 +17,7 @@ #ifndef OPP_AREA_SELECTION_FILTER_IMPL_H #define OPP_AREA_SELECTION_FILTER_IMPL_H -#include +#include "opp_area_selection/filter.h" #include #include diff --git a/opp_area_selection/src/area_selection_node.cpp b/opp_area_selection/src/area_selection_node.cpp index 03d00ac..9e45295 100644 --- a/opp_area_selection/src/area_selection_node.cpp +++ b/opp_area_selection/src/area_selection_node.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include "opp_area_selection/area_selector.h" +#include "opp_area_selection/selection_artist.h" static const float TIMEOUT = 15.0; diff --git a/opp_area_selection/src/area_selector.cpp b/opp_area_selection/src/area_selector.cpp index 264d34c..398b1ea 100644 --- a/opp_area_selection/src/area_selector.cpp +++ b/opp_area_selection/src/area_selector.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include "opp_area_selection/area_selector.h" #include #include @@ -25,7 +25,7 @@ #include #include -#include +#include "opp_area_selection/filter.h" namespace { diff --git a/opp_area_selection/src/selection_artist.cpp b/opp_area_selection/src/selection_artist.cpp index fadba03..1ea5758 100644 --- a/opp_area_selection/src/selection_artist.cpp +++ b/opp_area_selection/src/selection_artist.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include "opp_area_selection/selection_artist.h" #include #include @@ -29,8 +29,8 @@ #include #include -#include -#include +#include "opp_area_selection/area_selector.h" +#include "opp_area_selection/area_selector_parameters.h" #include namespace opp_area_selection diff --git a/opp_database/src/opp_database_interface_cpp.cpp b/opp_database/src/opp_database_interface_cpp.cpp index 73d1f1c..9a21fdf 100644 --- a/opp_database/src/opp_database_interface_cpp.cpp +++ b/opp_database/src/opp_database_interface_cpp.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include "opp_database/opp_database_interface_cpp.h" #include #include 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 8cf0d82..fe731c6 100644 --- a/opp_gui/include/opp_gui/tool_path_planner_panel.h +++ b/opp_gui/include/opp_gui/tool_path_planner_panel.h @@ -17,8 +17,8 @@ #ifndef OPP_GUI_TOOL_PATH_PLANNER_PANEL_H #define OPP_GUI_TOOL_PATH_PLANNER_PANEL_H -#include #include +#include namespace opp_gui { diff --git a/opp_gui/include/opp_gui/utils.h b/opp_gui/include/opp_gui/utils.h index ddd1fa2..a96e3c4 100644 --- a/opp_gui/include/opp_gui/utils.h +++ b/opp_gui/include/opp_gui/utils.h @@ -17,11 +17,12 @@ #ifndef OPP_GUI_UTILS_H #define OPP_GUI_UTILS_H -#include -#include -#include #include + +#include #include +#include +#include namespace opp_gui { 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 fce6f9f..46238f3 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 @@ -23,7 +23,7 @@ #include #include -#include +#include "opp_gui/register_ros_msgs_for_qt.h" namespace Ui { class PolygonAreaSelectionWidget; diff --git a/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h b/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h index d2b5aa0..74206dd 100644 --- a/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h +++ b/opp_gui/include/opp_gui/widgets/segmentation_parameters_editor_widget.h @@ -23,7 +23,7 @@ #include #include -#include +#include "opp_gui/register_ros_msgs_for_qt.h" namespace Ui { 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 866f738..cb60c87 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 @@ -23,9 +23,9 @@ #include -#include -#include -#include +#include "opp_gui/register_ros_msgs_for_qt.h" +#include "opp_gui/widgets/polygon_area_selection_widget.h" +#include "opp_gui/widgets/segmentation_parameters_editor_widget.h" namespace Ui { class SurfaceSelectionComboWidget; 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 5166a08..5a5f556 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 @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include "opp_gui/widgets/list_editor_widget_base.h" +#include "opp_gui/widgets/surface_selection_combo_widget.h" +#include "opp_gui/widgets/tool_path_parameters_editor_widget.h" #include namespace opp_gui 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 d714fe9..c8cf0a7 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 @@ -17,11 +17,13 @@ #ifndef OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H #define OPP_GUI_WIDGETS_TOOL_PATH_PARAMETERS_EDITOR_WIDGET_H +#include + #include #include #include + #include -#include namespace Ui { 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 a871b85..6e0b70c 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 @@ -17,14 +17,16 @@ #ifndef OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H #define OPP_GUI_WIDGETS_TOUCH_POINT_EDITOR_WIDGET_H -#include -#include -#include #include -#include + +#include #include +#include #include +#include "opp_gui/widgets/list_editor_widget_base.h" +#include + namespace Ui { class TouchPointParametersEditor; 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 53ba6f5..fa43b9e 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 @@ -18,12 +18,15 @@ #define OPP_GUI_WIDGETS_TOUCH_POINT_PARAMETERS_EDITOR_H #include + +#include + #include #include #include #include + #include -#include namespace Ui { diff --git a/opp_gui/src/nodes/segmentation_demo_app.cpp b/opp_gui/src/nodes/segmentation_demo_app.cpp index 5ce691d..fab2a38 100644 --- a/opp_gui/src/nodes/segmentation_demo_app.cpp +++ b/opp_gui/src/nodes/segmentation_demo_app.cpp @@ -14,14 +14,15 @@ * limitations under the License. */ -#include -#include -#include +#include #include -#include #include #include +#include + +#include "opp_gui/utils.h" +#include "opp_gui/widgets/segmentation_parameters_editor_widget.h" int main(int argc, char** argv) { 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 9140904..6d74d6c 100644 --- a/opp_gui/src/nodes/tool_path_editor_demo_app.cpp +++ b/opp_gui/src/nodes/tool_path_editor_demo_app.cpp @@ -14,11 +14,13 @@ * limitations under the License. */ -#include -#include -#include #include +#include + +#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, 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 00f6ca5..ea84813 100644 --- a/opp_gui/src/nodes/tool_path_planner_demo_app.cpp +++ b/opp_gui/src/nodes/tool_path_planner_demo_app.cpp @@ -14,11 +14,13 @@ * limitations under the License. */ -#include -#include -#include #include +#include + +#include "opp_gui/utils.h" +#include "opp_gui/widgets/tool_path_planner_widget.h" + int main(int argc, char** argv) { ros::init(argc, argv, "tool_path_planner_demo_app"); 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 f2ca2af..22a55d2 100644 --- a/opp_gui/src/nodes/touch_point_editor_demo_app.cpp +++ b/opp_gui/src/nodes/touch_point_editor_demo_app.cpp @@ -14,10 +14,12 @@ * limitations under the License. */ -#include -#include #include +#include + +#include "opp_gui/widgets/touch_point_editor_widget.h" + int main(int argc, char** argv) { ros::init(argc, argv, "touch_point_editor_demo_app"); diff --git a/opp_gui/src/tool_path_planner_panel.cpp b/opp_gui/src/tool_path_planner_panel.cpp index b111626..4aa2792 100644 --- a/opp_gui/src/tool_path_planner_panel.cpp +++ b/opp_gui/src/tool_path_planner_panel.cpp @@ -14,14 +14,16 @@ * limitations under the License. */ -#include -#include +#include "opp_gui/tool_path_planner_panel.h" + #include #include #include #include +#include "opp_gui/widgets/tool_path_planner_widget.h" + namespace opp_gui { diff --git a/opp_gui/src/utils.cpp b/opp_gui/src/utils.cpp index 04f0147..925e4ac 100644 --- a/opp_gui/src/utils.cpp +++ b/opp_gui/src/utils.cpp @@ -14,11 +14,12 @@ * limitations under the License. */ -#include +#include "opp_gui/utils.h" + +#include #include #include #include -#include #include #include #include diff --git a/opp_gui/src/widgets/polygon_area_selection_widget.cpp b/opp_gui/src/widgets/polygon_area_selection_widget.cpp index 8b8de31..6b052c4 100644 --- a/opp_gui/src/widgets/polygon_area_selection_widget.cpp +++ b/opp_gui/src/widgets/polygon_area_selection_widget.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include "opp_gui/widgets/polygon_area_selection_widget.h" #include #include diff --git a/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp b/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp index 61bdddb..4603f36 100644 --- a/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/segmentation_parameters_editor_widget.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include "opp_gui/widgets/segmentation_parameters_editor_widget.h" #include #include @@ -23,8 +23,8 @@ #include #include +#include "opp_gui/utils.h" #include "ui_segmentation_parameters_editor.h" -#include const static std::string SEGMENTATION_ACTION = "/mesh_segmenter_server_node/segmenter/"; diff --git a/opp_gui/src/widgets/surface_selection_combo_widget.cpp b/opp_gui/src/widgets/surface_selection_combo_widget.cpp index 37a7cc6..83e7e0e 100644 --- a/opp_gui/src/widgets/surface_selection_combo_widget.cpp +++ b/opp_gui/src/widgets/surface_selection_combo_widget.cpp @@ -14,15 +14,15 @@ * limitations under the License. */ -#include +#include "opp_gui/widgets/surface_selection_combo_widget.h" #include #include #include -#include -#include +#include "opp_gui/widgets/polygon_area_selection_widget.h" +#include "opp_gui/widgets/segmentation_parameters_editor_widget.h" #include "ui_surface_selection_combo_widget.h" 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 795577d..4cafd6d 100644 --- a/opp_gui/src/widgets/tool_path_editor_widget.cpp +++ b/opp_gui/src/widgets/tool_path_editor_widget.cpp @@ -14,8 +14,8 @@ * limitations under the License. */ -#include -#include +#include "opp_gui/widgets/tool_path_editor_widget.h" +#include "opp_gui/widgets/tool_path_parameters_editor_widget.h" const static std::string TOOL_PATH_TOPIC = "tool_path"; 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 bd4515a..539c984 100644 --- a/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp @@ -14,8 +14,7 @@ * limitations under the License. */ -#include -#include "ui_tool_path_parameters_editor.h" +#include "opp_gui/widgets/tool_path_parameters_editor_widget.h" #include #include @@ -25,7 +24,8 @@ #include #include -#include +#include "opp_gui/utils.h" +#include "ui_tool_path_parameters_editor.h" const static std::string GENERATE_TOOLPATHS_ACTION = "generate_tool_paths"; diff --git a/opp_gui/src/widgets/tool_path_planner_widget.cpp b/opp_gui/src/widgets/tool_path_planner_widget.cpp index 26a7296..31bb048 100644 --- a/opp_gui/src/widgets/tool_path_planner_widget.cpp +++ b/opp_gui/src/widgets/tool_path_planner_widget.cpp @@ -14,8 +14,7 @@ * limitations under the License. */ -#include -#include "ui_tool_path_planner.h" +#include "opp_gui/widgets/tool_path_planner_widget.h" #include #include @@ -31,9 +30,10 @@ #include #include -#include -#include -#include +#include "opp_gui/utils.h" +#include "opp_gui/widgets/tool_path_editor_widget.h" +#include "opp_gui/widgets/touch_point_editor_widget.h" +#include "ui_tool_path_planner.h" const static std::string MESH_MARKER_TOPIC = "mesh_marker"; diff --git a/opp_gui/src/widgets/touch_point_editor_widget.cpp b/opp_gui/src/widgets/touch_point_editor_widget.cpp index f1b1708..1354749 100644 --- a/opp_gui/src/widgets/touch_point_editor_widget.cpp +++ b/opp_gui/src/widgets/touch_point_editor_widget.cpp @@ -14,11 +14,14 @@ * limitations under the License. */ -#include -#include -#include -#include +#include "opp_gui/widgets/touch_point_editor_widget.h" + #include + +#include + +#include "opp_gui/utils.h" +#include "opp_gui/widgets/touch_point_parameters_editor_widget.h" #include "ui_touch_point_parameters_editor.h" 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 d3885c0..c8151c3 100644 --- a/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp +++ b/opp_gui/src/widgets/touch_point_parameters_editor_widget.cpp @@ -14,8 +14,8 @@ * limitations under the License. */ -#include -#include +#include "opp_gui/widgets/touch_point_parameters_editor_widget.h" +#include "ui_touch_point_parameters_editor.h" const std::string CLICKED_POINT_TOPIC = "/selection_point"; 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 b48df96..c9c7a55 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h @@ -2,6 +2,7 @@ #define OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H #include + #include namespace opp_msgs_serialization 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 936091d..7758b91 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h @@ -16,9 +16,10 @@ #ifndef OPP_MSGS_SERIALIZATION_EIGEN_YAML_H #define OPP_MSGS_SERIALIZATION_EIGEN_YAML_H -#include #include +#include "opp_msgs_serialization/geometry_msgs_yaml.h" + namespace YAML { 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 7747b91..12cd808 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 @@ -17,9 +17,10 @@ #define OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML #include -#include -#include #include +#include + +#include "opp_msgs_serialization/std_msgs_yaml.h" namespace 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 674cc48..ee61b3d 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 @@ -21,8 +21,8 @@ #include #include -#include -#include +#include "opp_msgs_serialization/geometry_msgs_yaml.h" +#include "opp_msgs_serialization/sensor_msgs_yaml.h" namespace YAML { 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 a9befa7..e59611d 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 @@ -16,10 +16,11 @@ #ifndef OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML #define OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML -#include #include #include +#include "opp_msgs_serialization/std_msgs_yaml.h" + namespace YAML { diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h index afd1b12..1858232 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h @@ -17,7 +17,9 @@ #define OPP_MSGS_SERIALIZATION_SERIALIZE_H #include + #include + #include namespace opp_msgs_serialization 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 0f8b633..d5f58d8 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 @@ -16,9 +16,10 @@ #ifndef OPP_MSGS_SERIALIZATION_STD_MSGS_YAML #define OPP_MSGS_SERIALIZATION_STD_MSGS_YAML -#include #include +#include + namespace 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 f99d1bb..406535e 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 @@ -17,7 +17,8 @@ #define OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML #include -#include + +#include "opp_msgs_serialization/std_msgs_yaml.h" namespace YAML {