From a4afc2cbc186f17bfcf6546a6f189dfc33f7736a Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 11 May 2023 17:21:20 -0500 Subject: [PATCH] Exposed plugin implementations for linking by downstream projects (#45) --- CMakeLists.txt | 16 ++-- .../plugins/boost_progress_console_logger.h | 52 ++++++++++++ include/reach/plugins/console_logger.h | 48 +++++++++++ .../reach/plugins/multiplicative_evaluator.h | 49 +++++++++++ include/reach/plugins/no_op.h | 66 +++++++++++++++ .../point_cloud_target_pose_generator.h | 42 ++++++++++ src/plugins/boost_progress_console_logger.cpp | 82 +++++++----------- src/plugins/console_logger.cpp | 72 ++++++---------- src/plugins/multiplicative_evaluator.cpp | 77 +++++++---------- src/plugins/no_op.cpp | 84 +++++++------------ src/plugins/plugins.cpp | 14 ++++ .../point_cloud_target_pose_generator.cpp | 64 ++++++-------- 12 files changed, 424 insertions(+), 242 deletions(-) create mode 100644 include/reach/plugins/boost_progress_console_logger.h create mode 100644 include/reach/plugins/console_logger.h create mode 100644 include/reach/plugins/multiplicative_evaluator.h create mode 100644 include/reach/plugins/no_op.h create mode 100644 include/reach/plugins/point_cloud_target_pose_generator.h create mode 100644 src/plugins/plugins.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f273c19..952b5b44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,13 @@ add_library( src/types.cpp src/reach_visualizer.cpp src/reach_study.cpp - src/reach_study_comparison.cpp) + src/reach_study_comparison.cpp + # Implementations + src/plugins/multiplicative_evaluator.cpp + src/plugins/point_cloud_target_pose_generator.cpp + src/plugins/console_logger.cpp + src/plugins/boost_progress_console_logger.cpp + src/plugins/no_op.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_interface @@ -72,13 +78,7 @@ target_cxx_version(${PROJECT_NAME} PUBLIC VERSION 14) list(APPEND TARGETS ${PROJECT_NAME}) # Plugins Library -add_library( - ${PROJECT_NAME}_plugins SHARED - src/plugins/multiplicative_evaluator.cpp - src/plugins/point_cloud_target_pose_generator.cpp - src/plugins/console_logger.cpp - src/plugins/boost_progress_console_logger.cpp - src/plugins/no_op.cpp) +add_library(${PROJECT_NAME}_plugins SHARED src/plugins/plugins.cpp) target_link_libraries( ${PROJECT_NAME}_plugins PUBLIC ${PROJECT_NAME} diff --git a/include/reach/plugins/boost_progress_console_logger.h b/include/reach/plugins/boost_progress_console_logger.h new file mode 100644 index 00000000..16de4a81 --- /dev/null +++ b/include/reach/plugins/boost_progress_console_logger.h @@ -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. + */ +#ifndef REACH_PLUGINS_BOOST_PROGRESS_CONSOLE_LOGGER_H +#define REACH_PLUGINS_BOOST_PROGRESS_CONSOLE_LOGGER_H + +#include + +#include +#include +#include + +namespace reach +{ +/** + * @brief Thread-safe logger that prints messages to the console via stdout, with boost progress bar progress logging + */ +class BoostProgressConsoleLogger : public Logger +{ +public: + BoostProgressConsoleLogger(); + + void setMaxProgress(unsigned long max_progress) override; + void printProgress(unsigned long progress) const override; + void printResults(const ReachResultSummary& results) const override; + void print(const std::string& message) const override; + +protected: + mutable std::mutex mutex_; + mutable boost::shared_ptr display_; +}; + +struct BoostProgressConsoleLoggerFactory : public LoggerFactory +{ + Logger::Ptr create(const YAML::Node& /*config*/) const override; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_BOOST_PROGRESS_CONSOLE_LOGGER_H diff --git a/include/reach/plugins/console_logger.h b/include/reach/plugins/console_logger.h new file mode 100644 index 00000000..d2538856 --- /dev/null +++ b/include/reach/plugins/console_logger.h @@ -0,0 +1,48 @@ +/* + * 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 REACH_PLUGINS_CONSOLE_LOGGER_H +#define REACH_PLUGINS_CONSOLE_LOGGER_H + +#include + +#include + +namespace reach +{ +/** + * @brief Thread-safe logger that prints messages to the console via stdout + */ +class ConsoleLogger : public Logger +{ +public: + void setMaxProgress(unsigned long progress) override; + void printProgress(unsigned long progress) const override; + void printResults(const ReachResultSummary& results) const override; + void print(const std::string& message) const override; + +protected: + mutable std::mutex mutex_; + unsigned long max_progress_{ 0 }; +}; + +struct ConsoleLoggerFactory : public LoggerFactory +{ + Logger::Ptr create(const YAML::Node& /*config*/) const override; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_CONSOLE_LOGGER_H diff --git a/include/reach/plugins/multiplicative_evaluator.h b/include/reach/plugins/multiplicative_evaluator.h new file mode 100644 index 00000000..a52c26e5 --- /dev/null +++ b/include/reach/plugins/multiplicative_evaluator.h @@ -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. + */ +#ifndef REACH_PLUGINS_MULTIPLICATIVE_EVALUATOR_H +#define REACH_PLUGINS_MULTIPLICATIVE_EVALUATOR_H + +#include + +#include + +namespace reach +{ +class MultiplicativeEvaluator : public Evaluator +{ +public: + MultiplicativeEvaluator(std::vector evaluators); + + double calculateScore(const std::map& pose) const override; + +private: + std::vector evaluators_; +}; + +class MultiplicativeEvaluatorFactory : public EvaluatorFactory +{ +public: + MultiplicativeEvaluatorFactory(); + + Evaluator::ConstPtr create(const YAML::Node& config) const override; + +private: + mutable boost_plugin_loader::PluginLoader loader_; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_MULTIPLICATIVE_EVALUATOR_H diff --git a/include/reach/plugins/no_op.h b/include/reach/plugins/no_op.h new file mode 100644 index 00000000..12a0c09f --- /dev/null +++ b/include/reach/plugins/no_op.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 REACH_PLUGINS_NOOP_H +#define REACH_PLUGINS_NOOP_H + +#include +#include +#include + +#include + +namespace reach +{ +struct NoOpEvaluator : public Evaluator +{ + double calculateScore(const std::map&) const override; +}; + +struct NoOpEvaluatorFactory : public EvaluatorFactory +{ + virtual Evaluator::ConstPtr create(const YAML::Node&) const override; +}; + +struct NoOpIKSolver : public IKSolver +{ +public: + std::vector getJointNames() const override; + + std::vector> solveIK(const Eigen::Isometry3d&, + const std::map&) const override; +}; + +struct NoOpIKSolverFactory : public IKSolverFactory +{ + IKSolver::ConstPtr create(const YAML::Node&) const override; +}; + +struct NoOpDisplay : public Display +{ + void showEnvironment() const override; + void updateRobotPose(const std::map&) const override; + void showReachNeighborhood(const std::map&) const override; + void showResults(const ReachResult&) const override; +}; + +struct NoOpDisplayFactory : public DisplayFactory +{ + Display::ConstPtr create(const YAML::Node&) const override; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_NOOP_H diff --git a/include/reach/plugins/point_cloud_target_pose_generator.h b/include/reach/plugins/point_cloud_target_pose_generator.h new file mode 100644 index 00000000..ed25114b --- /dev/null +++ b/include/reach/plugins/point_cloud_target_pose_generator.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 REACH_PLUGINS_POINT_CLOUD_TARGET_POSE_GENERATOR_H +#define REACH_PLUGINS_POINT_CLOUD_TARGET_POSE_GENERATOR_H + +#include + +namespace reach +{ +class PointCloudTargetPoseGenerator : public TargetPoseGenerator +{ +public: + PointCloudTargetPoseGenerator(std::string filename); + + VectorIsometry3d generate() const override; + +private: + std::string filename_; +}; + +struct PointCloudTargetPoseGeneratorFactory : public TargetPoseGeneratorFactory +{ + using TargetPoseGeneratorFactory::TargetPoseGeneratorFactory; + TargetPoseGenerator::ConstPtr create(const YAML::Node& config) const override; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_POINT_CLOUD_TARGET_POSE_GENERATOR_H diff --git a/src/plugins/boost_progress_console_logger.cpp b/src/plugins/boost_progress_console_logger.cpp index 9a8eb5fb..2d17bb48 100644 --- a/src/plugins/boost_progress_console_logger.cpp +++ b/src/plugins/boost_progress_console_logger.cpp @@ -1,62 +1,42 @@ -#include +#include #include -#include -#include #include #include -#include namespace reach { -/** - * @brief Thread-safe logger that prints messages to the console via stdout, with boost progress bar progress logging - */ -class BoostProgressConsoleLogger : public Logger +BoostProgressConsoleLogger::BoostProgressConsoleLogger() : display_(nullptr) { -public: - BoostProgressConsoleLogger() : display_(nullptr) - { - } - - void setMaxProgress(unsigned long max_progress) override - { - std::lock_guard lock{ mutex_ }; - display_ = boost::make_shared(max_progress); - } - - void printProgress(unsigned long progress) const override - { - std::lock_guard lock{ mutex_ }; - if (progress > display_->count()) - *display_ += progress - display_->count(); - } - - void printResults(const ReachResultSummary& results) const override - { - print(results.print()); - } - - void print(const std::string& message) const override - { - std::lock_guard lock{ mutex_ }; - std::cout << message << std::endl; - } - -protected: - mutable std::mutex mutex_; - mutable boost::shared_ptr display_; -}; - -struct BoostProgressConsoleLoggerFactory : public LoggerFactory +} + +void BoostProgressConsoleLogger::setMaxProgress(unsigned long max_progress) { - Logger::Ptr create(const YAML::Node& /*config*/) const override - { - return std::make_shared(); - } -}; + std::lock_guard lock{ mutex_ }; + display_ = boost::make_shared(max_progress); +} -} // namespace reach +void BoostProgressConsoleLogger::printProgress(unsigned long progress) const +{ + std::lock_guard lock{ mutex_ }; + if (progress > display_->count()) + *display_ += progress - display_->count(); +} + +void BoostProgressConsoleLogger::printResults(const ReachResultSummary& results) const +{ + print(results.print()); +} -#include -EXPORT_LOGGER_PLUGIN(reach::BoostProgressConsoleLoggerFactory, BoostProgressConsoleLogger) +void BoostProgressConsoleLogger::print(const std::string& message) const +{ + std::lock_guard lock{ mutex_ }; + std::cout << message << std::endl; +} + +Logger::Ptr BoostProgressConsoleLoggerFactory::create(const YAML::Node& /*config*/) const +{ + return std::make_shared(); +} + +} // namespace reach diff --git a/src/plugins/console_logger.cpp b/src/plugins/console_logger.cpp index 203a139b..68910c91 100644 --- a/src/plugins/console_logger.cpp +++ b/src/plugins/console_logger.cpp @@ -1,61 +1,43 @@ -#include +#include #include #include -#include namespace reach { -/** - * @brief Thread-safe logger that prints messages to the console via stdout - */ -class ConsoleLogger : public Logger +void ConsoleLogger::setMaxProgress(unsigned long progress) { -public: - void setMaxProgress(unsigned long progress) override - { - std::lock_guard lock{ mutex_ }; - max_progress_ = progress; - } - - void printProgress(unsigned long progress) const override - { - static std::atomic previous_pct{ 0.0 }; - const double current_pct_float = (static_cast(progress) / static_cast(max_progress_)) * 100.0; - const int current_pct = static_cast(current_pct_float); - if (current_pct > previous_pct) - { - std::lock_guard lock{ mutex_ }; - std::cout << "[" << current_pct << "%]" << std::endl; - } - previous_pct = current_pct; - } - - void printResults(const ReachResultSummary& results) const override - { - print(results.print()); - } + std::lock_guard lock{ mutex_ }; + max_progress_ = progress; +} - void print(const std::string& message) const override +void ConsoleLogger::printProgress(unsigned long progress) const +{ + static std::atomic previous_pct{ 0.0 }; + const double current_pct_float = (static_cast(progress) / static_cast(max_progress_)) * 100.0; + const int current_pct = static_cast(current_pct_float); + if (current_pct > previous_pct) { std::lock_guard lock{ mutex_ }; - std::cout << message << std::endl; + std::cout << "[" << current_pct << "%]" << std::endl; } + previous_pct = current_pct; +} -protected: - mutable std::mutex mutex_; - unsigned long max_progress_{ 0 }; -}; +void ConsoleLogger::printResults(const ReachResultSummary& results) const +{ + print(results.print()); +} -struct ConsoleLoggerFactory : public LoggerFactory +void ConsoleLogger::print(const std::string& message) const { - Logger::Ptr create(const YAML::Node& /*config*/) const override - { - return std::make_shared(); - } -}; + std::lock_guard lock{ mutex_ }; + std::cout << message << std::endl; +} -} // namespace reach +Logger::Ptr ConsoleLoggerFactory::create(const YAML::Node& /*config*/) const +{ + return std::make_shared(); +} -#include -EXPORT_LOGGER_PLUGIN(reach::ConsoleLoggerFactory, ConsoleLogger) +} // namespace reach diff --git a/src/plugins/multiplicative_evaluator.cpp b/src/plugins/multiplicative_evaluator.cpp index 150d33e2..b1d00734 100644 --- a/src/plugins/multiplicative_evaluator.cpp +++ b/src/plugins/multiplicative_evaluator.cpp @@ -13,70 +13,53 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - -#include +#include #include #include -#include #include namespace reach { -class MultiplicativeEvaluator : public Evaluator +MultiplicativeEvaluator::MultiplicativeEvaluator(std::vector evaluators) + : evaluators_(std::move(evaluators)) { -public: - MultiplicativeEvaluator(std::vector evaluators) : evaluators_(std::move(evaluators)) - { - } +} - virtual double calculateScore(const std::map& pose) const override +double MultiplicativeEvaluator::calculateScore(const std::map& pose) const +{ + double score = 1.0; + for (const Evaluator::ConstPtr& eval : evaluators_) { - double score = 1.0; - for (const Evaluator::ConstPtr& eval : evaluators_) - { - score *= eval->calculateScore(pose); - } - return score; + score *= eval->calculateScore(pose); } + return score; +} -private: - std::vector evaluators_; -}; +MultiplicativeEvaluatorFactory::MultiplicativeEvaluatorFactory() +{ + loader_.search_libraries_env = SEARCH_LIBRARIES_ENV; + boost::split(loader_.search_libraries, PLUGIN_LIBRARIES, boost::is_any_of(":"), boost::token_compress_on); + loader_.search_system_folders = true; +} -class MultiplicativeEvaluatorFactory : public EvaluatorFactory +Evaluator::ConstPtr MultiplicativeEvaluatorFactory::create(const YAML::Node& config) const { -public: - MultiplicativeEvaluatorFactory() - { - loader_.search_libraries_env = SEARCH_LIBRARIES_ENV; - boost::split(loader_.search_libraries, PLUGIN_LIBRARIES, boost::is_any_of(":"), boost::token_compress_on); - loader_.search_system_folders = true; - } + const YAML::Node plugin_configs = config["plugins"]; + std::vector evaluators; + evaluators.reserve(plugin_configs.size()); - Evaluator::ConstPtr create(const YAML::Node& config) const override + for (auto it = plugin_configs.begin(); it != plugin_configs.end(); ++it) { - const YAML::Node plugin_configs = config["plugins"]; - std::vector evaluators; - evaluators.reserve(plugin_configs.size()); - - for (auto it = plugin_configs.begin(); it != plugin_configs.end(); ++it) - { - const YAML::Node& plugin_config = *it; - EvaluatorFactory::Ptr factory = loader_.createInstance(get(plugin_config, "name")); - evaluators.push_back(factory->create(plugin_config)); - } - - if (evaluators.empty()) - throw std::runtime_error("No valid plugins remain"); - - return std::make_shared(evaluators); + const YAML::Node& plugin_config = *it; + EvaluatorFactory::Ptr factory = loader_.createInstance(get(plugin_config, "name")); + evaluators.push_back(factory->create(plugin_config)); } -private: - mutable boost_plugin_loader::PluginLoader loader_; -}; + if (evaluators.empty()) + throw std::runtime_error("No valid plugins remain"); -} // namespace reach + return std::make_shared(evaluators); +} -EXPORT_EVALUATOR_PLUGIN(reach::MultiplicativeEvaluatorFactory, MultiplicativeEvaluator) +} // namespace reach diff --git a/src/plugins/no_op.cpp b/src/plugins/no_op.cpp index 3b276f35..ae73cbc4 100644 --- a/src/plugins/no_op.cpp +++ b/src/plugins/no_op.cpp @@ -1,69 +1,49 @@ -#include -#include -#include - -#include +#include namespace reach { -struct NoOpEvaluator : public Evaluator +double NoOpEvaluator::calculateScore(const std::map&) const { - double calculateScore(const std::map&) const override - { - return 0.0; - } -}; + return 0.0; +} -struct NoOpEvaluatorFactory : public EvaluatorFactory +Evaluator::ConstPtr NoOpEvaluatorFactory::create(const YAML::Node&) const { - virtual Evaluator::ConstPtr create(const YAML::Node&) const override - { - return std::make_shared(); - } -}; + return std::make_shared(); +} -struct NoOpIKSolver : public IKSolver +std::vector NoOpIKSolver::getJointNames() const { -public: - std::vector getJointNames() const override - { - return { "j1" }; - } + return { "j1" }; +} - std::vector> solveIK(const Eigen::Isometry3d&, - const std::map&) const override - { - return { { 0.0 } }; - } -}; +std::vector> NoOpIKSolver::solveIK(const Eigen::Isometry3d&, + const std::map&) const +{ + return { { 0.0 } }; +} -struct NoOpIKSolverFactory : public IKSolverFactory +IKSolver::ConstPtr NoOpIKSolverFactory::create(const YAML::Node&) const { - IKSolver::ConstPtr create(const YAML::Node&) const override - { - return std::make_shared(); - } -}; + return std::make_shared(); +} -struct NoOpDisplay : public Display +void NoOpDisplay::showEnvironment() const +{ +} +void NoOpDisplay::updateRobotPose(const std::map&) const { - void showEnvironment() const override{}; - void updateRobotPose(const std::map&) const override{}; - void showReachNeighborhood(const std::map&) const override{}; - void showResults(const ReachResult&) const override{}; -}; +} +void NoOpDisplay::showReachNeighborhood(const std::map&) const +{ +} +void NoOpDisplay::showResults(const ReachResult&) const +{ +} -struct NoOpDisplayFactory : public DisplayFactory +Display::ConstPtr NoOpDisplayFactory::create(const YAML::Node&) const { - Display::ConstPtr create(const YAML::Node&) const override - { - return std::make_shared(); - } -}; + return std::make_shared(); +} } // namespace reach - -#include -EXPORT_EVALUATOR_PLUGIN(reach::NoOpEvaluatorFactory, NoOpEvaluator) -EXPORT_IK_SOLVER_PLUGIN(reach::NoOpIKSolverFactory, NoOpIKSolver) -EXPORT_DISPLAY_PLUGIN(reach::NoOpDisplayFactory, NoOpDisplay) diff --git a/src/plugins/plugins.cpp b/src/plugins/plugins.cpp new file mode 100644 index 00000000..e1d6b303 --- /dev/null +++ b/src/plugins/plugins.cpp @@ -0,0 +1,14 @@ +#include +#include +#include +#include +#include + +#include +EXPORT_LOGGER_PLUGIN(reach::BoostProgressConsoleLoggerFactory, BoostProgressConsoleLogger) +EXPORT_LOGGER_PLUGIN(reach::ConsoleLoggerFactory, ConsoleLogger) +EXPORT_EVALUATOR_PLUGIN(reach::MultiplicativeEvaluatorFactory, MultiplicativeEvaluator) +EXPORT_EVALUATOR_PLUGIN(reach::NoOpEvaluatorFactory, NoOpEvaluator) +EXPORT_IK_SOLVER_PLUGIN(reach::NoOpIKSolverFactory, NoOpIKSolver) +EXPORT_DISPLAY_PLUGIN(reach::NoOpDisplayFactory, NoOpDisplay) +EXPORT_TARGET_POSE_GENERATOR_PLUGIN(reach::PointCloudTargetPoseGeneratorFactory, PointCloudTargetPoseGenerator) diff --git a/src/plugins/point_cloud_target_pose_generator.cpp b/src/plugins/point_cloud_target_pose_generator.cpp index c32e69ce..16bcebb1 100644 --- a/src/plugins/point_cloud_target_pose_generator.cpp +++ b/src/plugins/point_cloud_target_pose_generator.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -26,52 +26,38 @@ static bool hasNormals(pcl::PCLPointCloud2& cloud) namespace reach { -class PointCloudTargetPoseGenerator : public TargetPoseGenerator +PointCloudTargetPoseGenerator::PointCloudTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename)) { -public: - PointCloudTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename)) - { - } - - VectorIsometry3d generate() const override - { - // Check if file exists - if (!boost::filesystem::exists(filename_)) - throw std::runtime_error("File '" + filename_ + "' does not exist"); +} - pcl::PCLPointCloud2 cloud_msg; - if (pcl::io::loadPCDFile(filename_, cloud_msg) < 0) - throw std::runtime_error("Failed to load point cloud from '" + filename_ + "'"); +VectorIsometry3d PointCloudTargetPoseGenerator::generate() const +{ + // Check if file exists + if (!boost::filesystem::exists(filename_)) + throw std::runtime_error("File '" + filename_ + "' does not exist"); - if (!hasNormals(cloud_msg)) - throw std::runtime_error("Point cloud file does not contain normals. Please regenerate the cloud with normal " - "vectors"); + pcl::PCLPointCloud2 cloud_msg; + if (pcl::io::loadPCDFile(filename_, cloud_msg) < 0) + throw std::runtime_error("Failed to load point cloud from '" + filename_ + "'"); - pcl::PointCloud cloud; - pcl::fromPCLPointCloud2(cloud_msg, cloud); + if (!hasNormals(cloud_msg)) + throw std::runtime_error("Point cloud file does not contain normals. Please regenerate the cloud with normal " + "vectors"); - VectorIsometry3d target_poses; - target_poses.reserve(cloud.size()); - std::transform(cloud.begin(), cloud.end(), std::back_inserter(target_poses), [](const pcl::PointNormal& pt) { - return createFrame(pt.getArray3fMap(), pt.getNormalVector3fMap()); - }); + pcl::PointCloud cloud; + pcl::fromPCLPointCloud2(cloud_msg, cloud); - return target_poses; - } + VectorIsometry3d target_poses; + target_poses.reserve(cloud.size()); + std::transform(cloud.begin(), cloud.end(), std::back_inserter(target_poses), + [](const pcl::PointNormal& pt) { return createFrame(pt.getArray3fMap(), pt.getNormalVector3fMap()); }); -private: - std::string filename_; -}; + return target_poses; +} -struct PointCloudTargetPoseGeneratorFactory : public TargetPoseGeneratorFactory +TargetPoseGenerator::ConstPtr PointCloudTargetPoseGeneratorFactory::create(const YAML::Node& config) const { - using TargetPoseGeneratorFactory::TargetPoseGeneratorFactory; - TargetPoseGenerator::ConstPtr create(const YAML::Node& config) const override - { - return std::make_shared(get(config, "pcd_file")); - } -}; + return std::make_shared(get(config, "pcd_file")); +} } // namespace reach - -EXPORT_TARGET_POSE_GENERATOR_PLUGIN(reach::PointCloudTargetPoseGeneratorFactory, PointCloudTargetPoseGenerator)