From 6eee02baccbbe7ecde744b806e2a7ed419f5d5d9 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 9 Nov 2023 15:39:19 +0300 Subject: [PATCH] clang-format --- .../include/env_interface/env_interface.hpp | 17 +- .../env_interface/env_interface_base.hpp | 36 +- .../env_interface/src/env_interface_base.cpp | 68 ++-- .../include/env_manager/env_manager.hpp | 113 +++--- env_manager/env_manager/src/env_manager.cpp | 269 +++++++-------- .../env_manager/src/run_env_manager.cpp | 25 +- env_manager/gz_enviroment/CMakeLists.txt | 1 + .../include/gz_enviroment/gz_enviroment.hpp | 45 +-- .../gz_enviroment/src/gz_enviroment.cpp | 272 ++++++++------- .../add_planning_scene_objects_service.cpp | 59 ++-- .../src/assemble_state_server.cpp | 324 ++++++++--------- .../src/gripper_control_action_server.cpp | 155 +++++---- .../src/move_cartesian_path_action_server.cpp | 266 +++++++------- .../move_to_joint_states_action_server.cpp | 233 ++++++------- .../src/move_topose_action_server.cpp | 249 +++++++------- .../src/moveit_update_planning_scene.cpp | 325 +++++++++--------- .../src/pick_place_pose_loader.cpp | 188 +++++----- 17 files changed, 1299 insertions(+), 1346 deletions(-) diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp index 3a69802..09fb681 100644 --- a/env_manager/env_interface/include/env_interface/env_interface.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface.hpp @@ -2,13 +2,10 @@ namespace env_interface { - class EnvInterface : public EnvInterfaceBase - { - public: - EnvInterface() = default; - virtual ~EnvInterface() = default; - - }; -} // namespace env_interface - - +class EnvInterface : public EnvInterfaceBase +{ +public: + EnvInterface() = default; + virtual ~EnvInterface() = default; +}; +} // namespace env_interface diff --git a/env_manager/env_interface/include/env_interface/env_interface_base.hpp b/env_manager/env_interface/include/env_interface/env_interface_base.hpp index b4c0c5f..5977b71 100644 --- a/env_manager/env_interface/include/env_interface/env_interface_base.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface_base.hpp @@ -1,38 +1,40 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include +#include namespace env_interface { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// template class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - EnvInterfaceBase() = default; + EnvInterfaceBase() = default; - virtual ~EnvInterfaceBase() = default; + virtual ~EnvInterfaceBase() = default; - virtual CallbackReturn init( - const std::string& t_env_name, - const std::string & t_namespace = "", - const rclcpp::NodeOptions & t_node_options = rclcpp::NodeOptions() - ); + virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "", + const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions()); - const rclcpp_lifecycle::State& configure(); + const rclcpp_lifecycle::State& configure(); - virtual CallbackReturn on_init() = 0; + virtual CallbackReturn on_init() = 0; - std::shared_ptr getNode(); - std::shared_ptr getNode() const; + std::shared_ptr getNode(); + std::shared_ptr getNode() const; - const rclcpp_lifecycle::State & getState() const; + const rclcpp_lifecycle::State& getState() const; + // virtual void setActionSpace(const A_space& action, const std::size_t& size); + +protected: + // A_space action_space; + // S_space observation_space; - private: - std::shared_ptr m_node; - - + std::shared_ptr m_node; }; -} // namespace env_interface +} // namespace env_interface diff --git a/env_manager/env_interface/src/env_interface_base.cpp b/env_manager/env_interface/src/env_interface_base.cpp index 5f5f841..c4e2d26 100644 --- a/env_manager/env_interface/src/env_interface_base.cpp +++ b/env_manager/env_interface/src/env_interface_base.cpp @@ -3,61 +3,43 @@ namespace env_interface { -CallbackReturn -EnvInterfaceBase::init( - const std::string& t_env_name, - const std::string & t_namespace, - const rclcpp::NodeOptions & t_node_options) +CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace, + const rclcpp::NodeOptions& t_node_options) { - m_node = std::make_shared( - t_env_name, t_namespace, t_node_options, false); + m_node = std::make_shared(t_env_name, t_namespace, t_node_options, false); - if (on_init() == CallbackReturn::FAILURE) - { - return CallbackReturn::FAILURE; - } + if (on_init() == CallbackReturn::FAILURE) + { + return CallbackReturn::FAILURE; + } - m_node->register_on_configure( - std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1) - ); - m_node->register_on_activate( - std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1) - ); - m_node->register_on_cleanup( - std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1) - ); - m_node->register_on_deactivate( - std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1) - ); - m_node->register_on_error( - std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1) - ); - m_node->register_on_shutdown( - std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1) - ); + m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1)); + m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1)); + m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1)); + m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1)); + m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1)); + m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1)); - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -const rclcpp_lifecycle::State& -EnvInterfaceBase::configure() +const rclcpp_lifecycle::State& EnvInterfaceBase::configure() { - return getNode()->configure(); + return getNode()->configure(); } -std::shared_ptr -EnvInterfaceBase::getNode() +std::shared_ptr EnvInterfaceBase::getNode() { - if (!m_node.get()) - { - throw std::runtime_error("Lifecycle node hasn't been initialized yet"); - } - return m_node; + if (!m_node.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet"); + } + return m_node; } -const rclcpp_lifecycle::State & EnvInterfaceBase::getState() const +const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const { - return m_node->get_current_state(); + return m_node->get_current_state(); } -} // namespace env_interface +} // namespace env_interface diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index b6a563d..692478f 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -11,7 +11,6 @@ #include "pluginlib/class_loader.hpp" - namespace env_manager { using EnvInterface = env_interface::EnvInterface; @@ -20,80 +19,68 @@ using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInter struct EnvSpec { - std::string name; - std::string type; - EnvInterfaceSharedPtr env_ptr; + std::string name; + std::string type; + EnvInterfaceSharedPtr env_ptr; }; class EnvManager : public rclcpp::Node { -public: - // EnvManager(rclcpp::Executor::SharedPtr executor, - // const std::string & json_config_path, - // const std::string & node_name = "env_manager", - // const std::string & node_namespace = "", - // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() - // .allow_undeclared_parameters(true) - // .automatically_declare_parameters_from_overrides(true)); - EnvManager(rclcpp::Executor::SharedPtr executor, - const std::string & node_name = "env_manager", - const std::string & node_namespace = "", - rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - virtual ~EnvManager() = default; +public: + // EnvManager(rclcpp::Executor::SharedPtr executor, + // const std::string & json_config_path, + // const std::string & node_name = "env_manager", + // const std::string & node_namespace = "", + // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + // .allow_undeclared_parameters(true) + // .automatically_declare_parameters_from_overrides(true)); + EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager", + const std::string& node_namespace = "", + rclcpp::NodeOptions& node_options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + virtual ~EnvManager() = default; + // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); + EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); + EnvStateReturnType unloadEnv(const std::string& env_name); + EnvStateReturnType configureEnv(const std::string& env_name); + EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); - // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); - EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); - EnvStateReturnType unloadEnv(const std::string &env_name); - EnvStateReturnType configureEnv(const std::string& env_name); - EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); - - //TODO: Define the input data format - // Set Target for RL enviroment - EnvInterfaceSharedPtr setTarget(); - EnvInterfaceSharedPtr switchTarget(); - EnvInterfaceSharedPtr unsetTarget(); - // Load Constraints for RL enviroment - EnvInterfaceSharedPtr loadConstraints(); - EnvInterfaceSharedPtr switchConstraints(); - EnvInterfaceSharedPtr unloadConstraints(); - + // TODO: Define the input data format + // Set Target for RL enviroment + EnvInterfaceSharedPtr setTarget(); + EnvInterfaceSharedPtr switchTarget(); + EnvInterfaceSharedPtr unsetTarget(); + // Load Constraints for RL enviroment + EnvInterfaceSharedPtr loadConstraints(); + EnvInterfaceSharedPtr switchConstraints(); + EnvInterfaceSharedPtr unloadConstraints(); protected: - void initServices(); - rclcpp::Node::SharedPtr getNode(); - void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); + void initServices(); + rclcpp::Node::SharedPtr getNode(); + void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); - void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, - env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); + void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); - void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); - - + void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); private: - rclcpp::Node::SharedPtr m_node; - std::mutex m_env_mutex; - std::vector m_active_envs; - rclcpp::Service::SharedPtr - m_load_env_srv; - rclcpp::Service::SharedPtr - m_configure_env_srv; - rclcpp::Service::SharedPtr - m_unload_env_srv; - - - rclcpp::CallbackGroup::SharedPtr m_callback_group; - rclcpp::Executor::SharedPtr m_executor; - - std::shared_ptr> m_loader; - + rclcpp::Node::SharedPtr m_node; + std::mutex m_env_mutex; + std::vector m_active_envs; + rclcpp::Service::SharedPtr m_load_env_srv; + rclcpp::Service::SharedPtr m_configure_env_srv; + rclcpp::Service::SharedPtr m_unload_env_srv; + rclcpp::CallbackGroup::SharedPtr m_callback_group; + rclcpp::Executor::SharedPtr m_executor; + std::shared_ptr> m_loader; }; -} // namespace env_manager -#endif // __ENV_MANAGER_HPP__ \ No newline at end of file +} // namespace env_manager +#endif // __ENV_MANAGER_HPP__ \ No newline at end of file diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index c69e5d2..00231da 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -1,22 +1,22 @@ #include "env_manager/env_manager.hpp" #include "nlohmann/json.hpp" -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "env_interface"; -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "env_interface::EnvInterface"; +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = + "env_interface"; +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME = + "env_interface::EnvInterface"; -namespace env_manager -{ -EnvManager::EnvManager( - rclcpp::Executor::SharedPtr executor, - const std::string & node_name, - const std::string & node_namespace, - rclcpp::NodeOptions & options) - : rclcpp::Node(node_name, node_namespace, options), - m_executor(executor), - m_loader(std::make_shared>( - ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) -{ - initServices(); +namespace env_manager { +EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor, + const std::string &node_name, + const std::string &node_namespace, + rclcpp::NodeOptions &options) + : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), + m_loader( + std::make_shared>( + ENV_INTERFACE_BASE_CLASS_NAMESPACE, + ENV_INTERFACE_BASE_CLASS_NAME)) { + initServices(); } // EnvManager::EnvManager( @@ -24,7 +24,7 @@ EnvManager::EnvManager( // const std::string & json_config_path, // const std::string & node_name, // const std::string & node_namespace, -// rclcpp::NodeOptions & options) +// rclcpp::NodeOptions & options) // : rclcpp::Node(node_name, node_namespace, options), // m_executor(executor), // m_loader(std::make_shared>( @@ -33,130 +33,111 @@ EnvManager::EnvManager( // initServices(); // } +void EnvManager::initServices() { + using namespace std::placeholders; - -void -EnvManager::initServices() -{ - using namespace std::placeholders; - - m_load_env_srv = create_service - ("~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); - m_configure_env_srv = create_service - ("~/configure_env", std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); - m_unload_env_srv = create_service - ("~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); + m_load_env_srv = create_service( + "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); + m_configure_env_srv = + create_service( + "~/configure_env", + std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); + m_unload_env_srv = create_service( + "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); } -rclcpp::Node::SharedPtr -EnvManager::getNode() -{ - return m_node; -} +rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } -void -EnvManager::loadEnv_cb( +void EnvManager::loadEnv_cb( const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) -{ - std::lock_guard lock(m_env_mutex); - response->ok = loadEnv(request->name, request->type).get() != nullptr; + env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { + std::lock_guard lock(m_env_mutex); + response->ok = loadEnv(request->name, request->type).get() != nullptr; } -void -EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, - env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) -{ - std::lock_guard guard(m_env_mutex); - response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; +void EnvManager::configureEnv_cb( + const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) { + std::lock_guard guard(m_env_mutex); + response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; } -void -EnvManager::unloadEnv_cb( +void EnvManager::unloadEnv_cb( const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) -{ - std::lock_guard lock(m_env_mutex); - response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) { + std::lock_guard lock(m_env_mutex); + response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; } +EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) { + std::lock_guard lock(m_env_mutex); + // generate list of active enviroments + // std::string unique_env_name = enviroment.name;// + "_" + + // std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] = + // enviroment.env_ptr; + if (enviroment.env_ptr->init(enviroment.name, get_namespace()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::ERROR) { + RCLCPP_ERROR(get_logger(), + "Enviroment with name %s cannot been initialized", + enviroment.name.c_str()); + return nullptr; + } -EnvInterfaceSharedPtr -EnvManager::addEnv(const EnvSpec& enviroment) -{ - std::lock_guard lock(m_env_mutex); - // generate list of active enviroments - // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); - // m_active_envs[unique_env_name] = enviroment.env_ptr; - if (enviroment.env_ptr->init( - enviroment.name, get_namespace()) == - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) - { - RCLCPP_ERROR(get_logger(), "Enviroment with name %s cannot been initialized", enviroment.name.c_str()); - return nullptr; - } - - m_active_envs.push_back(enviroment); - m_executor->add_node(enviroment.env_ptr->getNode()->get_node_base_interface()); - return enviroment.env_ptr; + m_active_envs.push_back(enviroment); + m_executor->add_node( + enviroment.env_ptr->getNode()->get_node_base_interface()); + return enviroment.env_ptr; } - -EnvInterfaceSharedPtr -EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type) -{ - EnvInterfaceSharedPtr loaded_env = nullptr; - RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); - try - { - if (m_loader->isClassAvailable(env_class_type)) - { - loaded_env = m_loader->createSharedInstance(env_class_type); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s' with class type '%s'", - env_name.c_str(), env_class_type.c_str()); - RCLCPP_INFO(this->get_logger(), "Available classes"); - for (const auto& available_class : m_loader->getDeclaredClasses()) - { - RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); - } - return nullptr; - } - } - catch (const pluginlib::PluginlibException& e) - { - RCLCPP_ERROR( - get_logger(), "Error happened during creation of enviroment '%s' with type '%s':\n%s", - env_name.c_str(), env_class_type.c_str(), e.what()); - return nullptr; - } - EnvSpec enviroment; - enviroment.name = env_name; - enviroment.type = env_class_type; - enviroment.env_ptr = loaded_env; - return addEnv(enviroment); -} - -EnvStateReturnType -EnvManager::unloadEnv(const std::string& env_name) -{ - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { - return env.name == env_name; - }); - - if (it != m_active_envs.end()) { - m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); - m_active_envs.erase(it); - return EnvStateReturnType::SUCCESS; +EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name, + const std::string &env_class_type) { + EnvInterfaceSharedPtr loaded_env = nullptr; + RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); + try { + if (m_loader->isClassAvailable(env_class_type)) { + loaded_env = m_loader->createSharedInstance(env_class_type); } else { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::ERROR; + RCLCPP_ERROR( + this->get_logger(), + "Loading enviroment is not available '%s' with class type '%s'", + env_name.c_str(), env_class_type.c_str()); + RCLCPP_INFO(this->get_logger(), "Available classes"); + for (const auto &available_class : m_loader->getDeclaredClasses()) { + RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); + } + return nullptr; } - return EnvStateReturnType::FAILURE; + } catch (const pluginlib::PluginlibException &e) { + RCLCPP_ERROR( + get_logger(), + "Error happened during creation of enviroment '%s' with type '%s':\n%s", + env_name.c_str(), env_class_type.c_str(), e.what()); + return nullptr; + } + EnvSpec enviroment; + enviroment.name = env_name; + enviroment.type = env_class_type; + enviroment.env_ptr = loaded_env; + return addEnv(enviroment); +} + +EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) { + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); + m_active_envs.erase(it); + return EnvStateReturnType::SUCCESS; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::ERROR; + } + return EnvStateReturnType::FAILURE; } // EnvInterfaceSharedPtr @@ -167,27 +148,25 @@ EnvManager::unloadEnv(const std::string& env_name) // return loadEnv(env_name, env_class_type); // } -EnvStateReturnType -EnvManager::configureEnv(const std::string& env_name) -{ - std::lock_guard guard(m_env_mutex); - - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { - return env.name == env_name; - }); - - if (it == m_active_envs.end()) { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::FAILURE; - } - it->env_ptr->configure(); - // it->env_ptr->getNode()->configure(); - // it->env_ptr->activate(); - it->env_ptr->getNode()->activate(); - - return EnvStateReturnType::SUCCESS; +EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) { + std::lock_guard guard(m_env_mutex); + + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it == m_active_envs.end()) { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::FAILURE; + } + it->env_ptr->configure(); + // it->env_ptr->getNode()->configure(); + // it->env_ptr->activate(); + it->env_ptr->getNode()->activate(); + + return EnvStateReturnType::SUCCESS; } -} \ No newline at end of file +} // namespace env_manager diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index 419e409..1506122 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -1,22 +1,21 @@ #include "rclcpp/rclcpp.hpp" #include "env_manager/env_manager.hpp" -int main(int argc, char ** argv) +int main(int argc, char** argv) { - rclcpp::init(argc, argv); - std::shared_ptr executor = - std::make_shared(); - std::string manager_node_name = "env_manager"; + rclcpp::init(argc, argv); + std::shared_ptr executor = std::make_shared(); + std::string manager_node_name = "env_manager"; - auto em = std::make_shared(executor, manager_node_name); + auto em = std::make_shared(executor, manager_node_name); - RCLCPP_INFO(em->get_logger(), "Env manager is starting"); + RCLCPP_INFO(em->get_logger(), "Env manager is starting"); - em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); - em->configureEnv("gz_enviroment"); + em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); + em->configureEnv("gz_enviroment"); - executor->add_node(em); - executor->spin(); - rclcpp::shutdown(); - return 0; + executor->add_node(em); + executor->spin(); + rclcpp::shutdown(); + return 0; } diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index a49bff5..90ecb8d 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ignition-msgs8 ros_gz_bridge env_interface + ignition-gazebo6 ) # find dependencies diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 3797527..852e875 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -1,6 +1,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" // #include "ros_gz_bridge/convert.hpp" +#include #include #include #include @@ -8,6 +9,10 @@ #include "gz/msgs/pose_v.pb.h" #include "env_interface/env_interface.hpp" +#include +#include +#include + namespace gz_enviroment { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -15,32 +20,28 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class GzEnviroment : public env_interface::EnvInterface { public: - - GzEnviroment(); - CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; - + GzEnviroment(); + CallbackReturn on_init() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; protected: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - - bool doGzSpawn(); + bool doGzSpawn(); private: - std::unique_ptr m_tf2_broadcaster; - std::shared_ptr m_gz_node; - std::vector m_follow_frames; - std::string m_topic_name; - std::string m_service_spawn; - - std::string getGzWorldName(); - std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, const std::string& name); - + std::unique_ptr m_tf2_broadcaster; + std::shared_ptr m_gz_node; + std::vector m_follow_frames; + std::string m_topic_name; + std::string m_service_spawn; + std::string m_world_name; + std::string getGzWorldName(); + std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, + const std::string& name); }; -} - +} // namespace gz_enviroment diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index 3a80041..25d1d03 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -2,173 +2,179 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "ros_gz_bridge/convert.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include +#include +#include -namespace gz_enviroment { +#include +#include -GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {} +namespace gz_enviroment +{ + +GzEnviroment::GzEnviroment() : env_interface::EnvInterface() +{ +} CallbackReturn GzEnviroment::on_init() { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); - return CallbackReturn::SUCCESS; -} + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); + gz::sim::EntityComponentManager ecm; -CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) { - // Configuration of parameters - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); - m_gz_node = std::make_shared(); - m_topic_name = std::string("/world/") + getGzWorldName() +"/dynamic_pose/info"; - m_service_spawn = std::string("/world/") + getGzWorldName() + "/create"; - - if (!doGzSpawn()) { - return CallbackReturn::ERROR; + // Получение всех сущностей, которые представляют модели + const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model()); + + // Вывод списка моделей + for (const auto entity : modelEntities) + { + const auto modelName = ecm.Component(entity); + if (modelName) + { + std::cout << "Model Name: " << modelName->Data() << std::endl; } - - return CallbackReturn::SUCCESS; + } + return CallbackReturn::SUCCESS; } -CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { - // Setting up the subscribers and publishers - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); - - m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); - m_tf2_broadcaster = std::make_unique(getNode()); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn -GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); - - m_tf2_broadcaster.reset(); - m_gz_node.reset(); - - return CallbackReturn::SUCCESS; + // Configuration of parameters + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); + m_gz_node = std::make_shared(); + m_world_name = getGzWorldName(); + m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info"; + m_service_spawn = std::string("/world/") + m_world_name + "/create"; + + if (!doGzSpawn()) + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; } -CallbackReturn -GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); - - - return CallbackReturn::SUCCESS; + // Setting up the subscribers and publishers + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); + + m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); + m_tf2_broadcaster = std::make_unique(getNode()); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); + + m_tf2_broadcaster.reset(); + m_gz_node.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); + + return CallbackReturn::SUCCESS; } // TODO: Check do this via EntityComponentManager -void -GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) +void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) { - // TODO: Read from config - m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "monkey"}; - for (const auto &it: pose_v.pose()) - { - if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) - continue; - - geometry_msgs::msg::PoseStamped rpose; - ros_gz_bridge::convert_gz_to_ros(it, rpose); - - geometry_msgs::msg::TransformStamped t; + // TODO: Read from config + m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6", "monkey" }; + for (const auto& it : pose_v.pose()) + { + if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) + continue; - t.header.stamp = getNode()->get_clock()->now(); - t.header.frame_id = "world"; - t.child_frame_id = it.name(); + geometry_msgs::msg::PoseStamped rpose; + ros_gz_bridge::convert_gz_to_ros(it, rpose); - t.transform.translation.x = rpose.pose.position.x; - t.transform.translation.y = rpose.pose.position.y; - t.transform.translation.z = rpose.pose.position.z; + geometry_msgs::msg::TransformStamped t; - t.transform.rotation.x = rpose.pose.orientation.x; - t.transform.rotation.y = rpose.pose.orientation.y; - t.transform.rotation.z = rpose.pose.orientation.z; - t.transform.rotation.w = rpose.pose.orientation.w; - - m_tf2_broadcaster->sendTransform(t); - } + t.header.stamp = getNode()->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = it.name(); + + t.transform.translation.x = rpose.pose.position.x; + t.transform.translation.y = rpose.pose.position.y; + t.transform.translation.z = rpose.pose.position.z; + + t.transform.rotation.x = rpose.pose.orientation.x; + t.transform.rotation.y = rpose.pose.orientation.y; + t.transform.rotation.z = rpose.pose.orientation.z; + t.transform.rotation.w = rpose.pose.orientation.w; + + m_tf2_broadcaster->sendTransform(t); + } } -std::string -GzEnviroment::getGzWorldName() +std::string GzEnviroment::getGzWorldName() { - bool executed{false}; - bool result{false}; - unsigned int timeout{5000}; - std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worlds_msg; + bool executed{ false }; + bool result{ false }; + unsigned int timeout{ 5000 }; + std::string service{ "/gazebo/worlds" }; + gz::msgs::StringMsg_V worlds_msg; - while (rclcpp::ok() && !executed) { - RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); - executed = m_gz_node->Request(service, timeout, worlds_msg, result); - } + while (rclcpp::ok() && !executed) + { + RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); + executed = m_gz_node->Request(service, timeout, worlds_msg, result); + } - if (!executed) { - RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); - return ""; - } + if (!executed) + { + RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); + return ""; + } - if (!result || worlds_msg.data().empty()) { - RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); - return ""; - } - RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); - return worlds_msg.data(0); + if (!result || worlds_msg.data().empty()) + { + RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); + return ""; + } + RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); + return worlds_msg.data(0); } -bool -GzEnviroment::doGzSpawn() +bool GzEnviroment::doGzSpawn() { - gz::msgs::EntityFactory req; - gz::msgs::Boolean rep; - bool result; - unsigned int timeout = 5000; - // TODO: From Config - std::vector pps{1.0, 0.0, 1.0, 0.0, 0.0}; - std::string mesh_file = "monkey.stl"; - std::string mesh_name = "monkey"; - // ENDTODO - std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); - req.set_sdf(sdf_string); - bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); + gz::msgs::EntityFactory req; + gz::msgs::Boolean rep; + bool result; + unsigned int timeout = 5000; + // TODO: From Config + std::vector pps{ 1.0, 0.0, 1.0, 0.0, 0.0 }; + std::string mesh_file = "monkey.stl"; + std::string mesh_name = "monkey"; + // ENDTODO + std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); + req.set_sdf(sdf_string); + bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); - return executed; + return executed; } -std::string -GzEnviroment::createGzModelString( - const std::vector& pose, - const std::string& mesh_filepath, - const std::string& name - ) +std::string GzEnviroment::createGzModelString(const std::vector& pose, const std::string& mesh_filepath, + const std::string& name) { - std::string model_template = - std::string("") + - "" + - "" + - "" - +std::to_string(pose[0])+" " - +std::to_string(pose[1])+" " - +std::to_string(pose[2])+" " - +std::to_string(pose[3])+" " - +std::to_string(pose[4])+" " - +std::to_string(pose[5])+"" + - "" + - "file://"+mesh_filepath+"" + - "" + - "" + - "file://"+mesh_filepath+"" + - "" + - "" + - "" + - ""; - return model_template; + std::string model_template = std::string("") + "" + + "" + "" + std::to_string(pose[0]) + " " + + std::to_string(pose[1]) + " " + std::to_string(pose[2]) + " " + std::to_string(pose[3]) + + " " + std::to_string(pose[4]) + " " + std::to_string(pose[5]) + "" + + "" + "file://" + mesh_filepath + + "" + "" + "" + + "file://" + mesh_filepath + "" + + "" + "" + "" + ""; + return model_template; } -} +} // namespace gz_enviroment #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - gz_enviroment::GzEnviroment, env_interface::EnvInterface -); \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface); \ No newline at end of file diff --git a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp index 3ecb596..008107d 100644 --- a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp +++ b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp @@ -1,24 +1,25 @@ +#include "moveit/move_group_interface/move_group_interface.h" +#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" +#include #include #include -#include #include -#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" -#include "moveit/move_group_interface/move_group_interface.h" #include "rclcpp/rclcpp.hpp" -using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject; +using AddPlanningSceneObject = + rbs_skill_interfaces::srv::AddPlanningSceneObject; rclcpp::Node::SharedPtr g_node = nullptr; void handle_service( const std::shared_ptr request_header, const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - // Create collision object for the robot to avoid - auto const collision_object = [frame_id = - "world", object_name=request->object_id, object_pose=request->object_pose] { + const std::shared_ptr response) { + (void)request_header; + // Create collision object for the robot to avoid + auto const collision_object = [frame_id = "world", + object_name = request->object_id, + object_pose = request->object_pose] { moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = frame_id; collision_object.id = object_name; @@ -40,32 +41,32 @@ void handle_service( box_pose.orientation.y = object_pose[4]; box_pose.orientation.z = object_pose[5]; box_pose.orientation.w = object_pose[6]; - - collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; return collision_object; - }(); + }(); - // Add the collision object to the scene - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - planning_scene_interface.applyCollisionObject(collision_object); + // Add the collision object to the scene + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + planning_scene_interface.applyCollisionObject(collision_object); - response->success = true; + response->success = true; } -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); - g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); - auto server = g_node->create_service("add_planing_scene_object_service", handle_service); - rclcpp::spin(g_node); - rclcpp::shutdown(); - g_node = nullptr; - return 0; -} \ No newline at end of file +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + g_node = + rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); + auto server = g_node->create_service( + "add_planing_scene_object_service", handle_service); + rclcpp::spin(g_node); + rclcpp::shutdown(); + g_node = nullptr; + return 0; +} diff --git a/rbs_skill_servers/src/assemble_state_server.cpp b/rbs_skill_servers/src/assemble_state_server.cpp index d359528..25ec2c5 100644 --- a/rbs_skill_servers/src/assemble_state_server.cpp +++ b/rbs_skill_servers/src/assemble_state_server.cpp @@ -1,18 +1,18 @@ +#include #include #include #include #include -#include -#include #include +#include -#include -#include #include +#include +#include -#include #include +#include #include #include @@ -24,12 +24,11 @@ #define ASSEMBLE_POSITION_OFFSET 0.5 #define ASSEMBLE_ORIENTATION_OFFSET 0.5 -#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ +#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ (package_dir) + "/models/" + (assemble_name) + "/model.sdf" -static inline geometry_msgs::msg::Pose get_pose( - const std::vector & tokens) -{ +static inline geometry_msgs::msg::Pose +get_pose(const std::vector &tokens) { geometry_msgs::msg::Pose p; p.position.set__x(std::stod(tokens.at(0))); p.position.set__y(std::stod(tokens.at(1))); @@ -41,23 +40,22 @@ static inline geometry_msgs::msg::Pose get_pose( return p; } -static inline geometry_msgs::msg::PoseStamped get_pose_stamped( - const std::string & pose_string) -{ +static inline geometry_msgs::msg::PoseStamped +get_pose_stamped(const std::string &pose_string) { std::stringstream ss(pose_string); std::istream_iterator begin(ss); std::istream_iterator end; std::vector tokens(begin, end); - + geometry_msgs::msg::PoseStamped ps; ps.set__pose(get_pose(tokens)); return ps; } -static std::vector get_assemble_poses( - const std::string & assemble_name, const std::string & part_name, - const std::string& package_dir, const std::string & assemble_prefix) -{ +static std::vector +get_assemble_poses(const std::string &assemble_name, + const std::string &part_name, const std::string &package_dir, + const std::string &assemble_prefix) { std::vector result; std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf"; @@ -66,12 +64,11 @@ static std::vector get_assemble_poses( auto model = doc.RootElement()->FirstChildElement("model"); auto joint = model->FirstChildElement("joint"); - while (joint) - { + while (joint) { auto frame_id = joint->FirstChildElement("child")->GetText(); if (frame_id != part_name) continue; - auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); + auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); ps.header.set__frame_id(assemble_prefix + part_name); result.push_back(ps); joint = joint->NextSiblingElement("joint"); @@ -80,164 +77,167 @@ static std::vector get_assemble_poses( return result; } -class AssembleStateServer: public rclcpp::Node -{ +class AssembleStateServer : public rclcpp::Node { public: - AssembleStateServer(rclcpp::NodeOptions options) - : Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); - assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); - tf_buffer_ = std::make_unique(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - service_ = create_service( - SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + AssembleStateServer(rclcpp::NodeOptions options) + : Node(SERVICE_NAME, + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); + assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + service_ = create_service( + SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + void + callback(std::shared_ptr + request, + std::shared_ptr + response) { + auto state = static_cast(request->req_kind); + bool call_status = false; + std::string assemble_name = request->assemble_name; + std::string part_name = request->part_name; + auto assemble = assembles_.find(request->assemble_name); + if (assemble == assembles_.end()) + assembles_.insert(std::make_pair( + request->assemble_name, + Assemble{.part = request->part_name, + .ws = request->workspace, + .state = NONE, + .poses = + get_assemble_poses(assemble_name, part_name, + assemble_dir_, assemble_prefix_)})); + RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", + assemble_name.c_str()); + switch (state) { + case NONE: + call_status = false; + break; + case INITIALIZE: + call_status = (assembles_.at(assemble_name).state == NONE) && + on_initialize(&assembles_.at(assemble_name)); + break; + case VALIDATE: + response->validate_status = + (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && + on_validate(&assembles_.at(assemble_name)); + break; + case COMPLETE: + call_status = (assembles_.at(assemble_name).state == VALIDATE) && + on_complete(&assembles_.at(assemble_name)); + if (call_status) + assembles_.erase(assemble_name); + break; } - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - bool call_status = false; - std::string assemble_name = request->assemble_name; - std::string part_name = request->part_name; - auto assemble = assembles_.find(request->assemble_name); - if (assemble == assembles_.end()) - assembles_.insert(std::make_pair(request->assemble_name, Assemble { - .part=request->part_name, - .ws=request->workspace, - .state=NONE, - .poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_) - })); - RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str()); - switch(state) - { - case NONE: - call_status = false; - break; - case INITIALIZE: - call_status = (assembles_.at(assemble_name).state == NONE) && - on_initialize(&assembles_.at(assemble_name)); - break; - case VALIDATE: - response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && - on_validate(&assembles_.at(assemble_name)); - break; - case COMPLETE: - call_status = (assembles_.at(assemble_name).state == VALIDATE) && - on_complete(&assembles_.at(assemble_name)); - if (call_status) - assembles_.erase(assemble_name); - break; - } + response->call_status = call_status; + response->assemble_name = assemble_name; + } - response->call_status = call_status; - response->assemble_name = assemble_name; - } - private: - enum AssembleReqState - { - NONE=-1, - INITIALIZE=0, - VALIDATE=1, - COMPLETE=2 - }; + enum AssembleReqState { + NONE = -1, + INITIALIZE = 0, + VALIDATE = 1, + COMPLETE = 2 + }; - struct Assemble - { - std::string part; - std::string ws; - AssembleReqState state; - std::vector poses; - std::unique_ptr tf2_broadcaster; - }; + struct Assemble { + std::string part; + std::string ws; + AssembleReqState state; + std::vector poses; + std::unique_ptr tf2_broadcaster; + }; - bool on_initialize(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str()); - try - { - assemble->tf2_broadcaster = std::make_unique(this); - } catch (const tf2::TransformException &ex){ - RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what()); - return false; - } - assemble->state = INITIALIZE; - for (const auto& it: assemble->poses) - { - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = assemble->ws; - t.child_frame_id = it.header.frame_id; - auto pose = it.pose; - t.transform.translation.x = pose.position.x; - t.transform.translation.y = pose.position.y; - t.transform.translation.z = pose.position.z; - t.transform.rotation.x = pose.orientation.x; - t.transform.rotation.y = pose.orientation.y; - t.transform.rotation.z = pose.orientation.z; - t.transform.rotation.w = pose.orientation.w; - assemble->tf2_broadcaster->sendTransform(t); - } - - return true; + bool on_initialize(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster = + std::make_unique(this); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", + ex.what()); + return false; + } + assemble->state = INITIALIZE; + for (const auto &it : assemble->poses) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = assemble->ws; + t.child_frame_id = it.header.frame_id; + auto pose = it.pose; + t.transform.translation.x = pose.position.x; + t.transform.translation.y = pose.position.y; + t.transform.translation.z = pose.position.z; + t.transform.rotation.x = pose.orientation.x; + t.transform.rotation.y = pose.orientation.y; + t.transform.rotation.z = pose.orientation.z; + t.transform.rotation.w = pose.orientation.w; + assemble->tf2_broadcaster->sendTransform(t); } - bool on_validate(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str()); - std::string frame_from = assemble_prefix_ + assemble->part; - std::string frame_to = assemble->part; - geometry_msgs::msg::TransformStamped ts; - try - { - ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); - } catch (const tf2::TransformException &ex) { - return false; - } - auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; - auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; + return true; + } - RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate); - assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE; + bool on_validate(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", + assemble->part.c_str()); + std::string frame_from = assemble_prefix_ + assemble->part; + std::string frame_to = assemble->part; + geometry_msgs::msg::TransformStamped ts; + try { + ts = + tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + return false; + } + auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; + auto orient_validate = + ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; - return assemble->state == VALIDATE; + RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", + pos_validate, orient_validate); + assemble->state = (pos_validate && orient_validate) ? VALIDATE : INITIALIZE; + + return assemble->state == VALIDATE; + } + + bool on_complete(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster.reset(); + assemble->tf2_broadcaster = NULL; + assemble->poses.clear(); + } catch (const std::exception &ex) { + RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", + ex.what()); + return false; } - bool on_complete(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str()); - try - { - assemble->tf2_broadcaster.reset(); - assemble->tf2_broadcaster = NULL; - assemble->poses.clear(); - } catch (const std::exception &ex) { - RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what()); - return false; - } + assemble->state = COMPLETE; - assemble->state = COMPLETE; + return true; + } - return true; - } - - private: - std::map assembles_; - std::shared_ptr tf_listener_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf2_broadcaster_; - std::mutex mt; - rclcpp::Service::SharedPtr service_; - std::string assemble_dir_; - std::string assemble_prefix_; + std::map assembles_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf2_broadcaster_; + std::mutex mt; + rclcpp::Service::SharedPtr service_; + std::string assemble_dir_; + std::string assemble_prefix_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_skill_servers/src/gripper_control_action_server.cpp b/rbs_skill_servers/src/gripper_control_action_server.cpp index 8c6f682..74addd0 100644 --- a/rbs_skill_servers/src/gripper_control_action_server.cpp +++ b/rbs_skill_servers/src/gripper_control_action_server.cpp @@ -7,108 +7,113 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/gripper_command.hpp" +#include "rclcpp_action/rclcpp_action.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" #include "sensor_msgs/msg/joint_state.hpp" - - +#include "std_msgs/msg/float64_multi_array.hpp" namespace rbs_skill_actions { - class GripperControlActionServer: public rclcpp::Node { +class GripperControlActionServer : public rclcpp::Node { - public: - using GripperCommand = rbs_skill_interfaces::action::GripperCommand; - explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true)) - { - this->actionServer_ = rclcpp_action::create_server( - this, - "gripper_control", - std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1) - ); - publisher = this->create_publisher("/gripper_controller/commands", 10); - join_state_subscriber = this->create_subscription( - "/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1)); - } +public: + using GripperCommand = rbs_skill_interfaces::action::GripperCommand; + explicit GripperControlActionServer(rclcpp::NodeOptions options) + : Node("gripper_control_action_server", + options.allow_undeclared_parameters(true)) { + this->actionServer_ = rclcpp_action::create_server( + this, "gripper_control", + std::bind(&GripperControlActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperControlActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&GripperControlActionServer::accepted_callback, this, + std::placeholders::_1)); + publisher = this->create_publisher( + "/gripper_controller/commands", 10); + join_state_subscriber = + this->create_subscription( + "/joint_states", 10, + std::bind(&GripperControlActionServer::joint_state_callback, this, + std::placeholders::_1)); + } +private: + rclcpp_action::Server::SharedPtr actionServer_; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr + join_state_subscriber; + double gripper_joint_state{1.0}; - private: - - rclcpp_action::Server::SharedPtr actionServer_; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr join_state_subscriber; - double gripper_joint_state{1.0}; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + void joint_state_callback(const sensor_msgs::msg::JointState &msg) { + gripper_joint_state = msg.position.back(); + } - void joint_state_callback(const sensor_msgs::msg::JointState & msg) - { - gripper_joint_state = msg.position.back(); - } + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { - rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - - RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position); - (void)uuid; - if(goal->position > 0.9 or goal->position < 0) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } + RCLCPP_INFO(this->get_logger(), + "goal request recieved for gripper [%.2f m]", goal->position); + (void)uuid; + if (goal->position > 0.9 or goal->position < 0) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) { + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - void accepted_callback(const std::shared_ptr goal_handle) { - std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach(); - } - - void execute(const std::shared_ptr goal_handle){ + void accepted_callback(const std::shared_ptr goal_handle) { + std::thread{std::bind(&GripperControlActionServer::execute, this, + std::placeholders::_1), + goal_handle} + .detach(); + } - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - auto feedback = std::make_shared(); - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); - return; - } - RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state); + void execute(const std::shared_ptr goal_handle) { - std_msgs::msg::Float64MultiArray command; + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + auto feedback = std::make_shared(); - using namespace std::chrono_literals; + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); + return; + } + RCLCPP_INFO(this->get_logger(), "Current gripper state %f", + gripper_joint_state); - command.data.push_back(goal->position); - RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper"); - publisher->publish(command); - std::this_thread::sleep_for(3s); + std_msgs::msg::Float64MultiArray command; + using namespace std::chrono_literals; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); - - } - }; -} - + command.data.push_back(goal->position); + RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper"); + publisher->publish(command); + std::this_thread::sleep_for(3s); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); + } +}; +} // namespace rbs_skill_actions RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer); - // int main(int argc, char ** argv) { // rclcpp::init(argc, argv); // rbs_skill_actions::GripperControlActionServer server; // rclcpp::spin(server); // return 0; -// } \ No newline at end of file +// } diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp index c3deb63..f223160 100644 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,156 +26,156 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveCartesianActionServer : public rclcpp::Node -{ +class MoveCartesianActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), - // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); - } + // explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_cartesian_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), + // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_cartesian", - std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_cartesian", + std::bind(&MoveCartesianActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveCartesianActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveCartesianActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), + goal_handle) + .detach(); + // std::thread( + // [this, goal_handle]() { + // execute(goal_handle); + // }).detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + moveit::core::RobotState start_state( + *move_group_interface.getCurrentState()); + + std::vector waypoints; + auto current_pose = move_group_interface.getCurrentPose(); + // waypoints.push_back(current_pose.pose); + geometry_msgs::msg::Pose target_pose = goal->target_pose; + // target_pose.position = goal->target_pose.position; + waypoints.push_back(target_pose); + + RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", + target_pose.position.x, target_pose.position.y, + target_pose.position.z); + // waypoints.push_back(start_pose.pose); + moveit_msgs::msg::RobotTrajectory trajectory; + const double jump_threshold = 0.0; + const double eef_step = 0.001; + double fraction = move_group_interface.computeCartesianPath( + waypoints, eef_step, jump_threshold, trajectory); + if (fraction > 0) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + moveit::core::MoveItErrorCode execute_err_code = + move_group_interface.execute(trajectory); + if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "Failure in move:"); + } + + // move_group_interface.move(); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); + goal_handle->abort(result); } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach(); - // std::thread( - // [this, goal_handle]() { - // execute(goal_handle); - // }).detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); - - std::vector waypoints; - auto current_pose = move_group_interface.getCurrentPose(); - //waypoints.push_back(current_pose.pose); - geometry_msgs::msg::Pose target_pose = goal->target_pose; - //target_pose.position = goal->target_pose.position; - waypoints.push_back(target_pose); - - RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", - target_pose.position.x, target_pose.position.y, target_pose.position.z); - //waypoints.push_back(start_pose.pose); - moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; - const double eef_step = 0.001; - double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - if(fraction>0) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory); - if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - }else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - goal_handle->abort(result); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + } }; // class MoveCartesianActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); - rbs_skill_actions::MoveCartesianActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveCartesianActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp index 9f79472..b7adb84 100644 --- a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp +++ b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp @@ -1,16 +1,15 @@ +#include #include #include #include -#include - #include "rclcpp/rclcpp.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,143 +25,139 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToJointStateActionServer : public rclcpp::Node -{ +class MoveToJointStateActionServer : public rclcpp::Node { public: - using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates; + using MoveitSendJointStates = + rbs_skill_interfaces::action::MoveitSendJointStates; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_to_joint_states_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_to_joint_states", - std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_to_joint_states", + std::bind(&MoveToJointStateActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToJointStateActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToJointStateActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = + rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str()); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", + goal->robot_name.c_str()); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), + goal_handle) + .detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + std::vector joint_states = goal->joint_values; + for (auto &joint : joint_states) { + RCLCPP_INFO(this->get_logger(), "Joint value %f", joint); } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach(); + + move_group_interface.setJointValueTarget(goal->joint_values); + move_group_interface.setMaxVelocityScalingFactor( + goal->joints_velocity_scaling_factor); + move_group_interface.setMaxAccelerationScalingFactor( + goal->joints_acceleration_scaling_factor); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_interface.plan(plan) == + moveit::core::MoveItErrorCode::SUCCESS); + if (success) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + move_group_interface.execute(plan); + // move_group_interface.move(); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - std::vector joint_states = goal->joint_values; - for (auto &joint : joint_states) - { - RCLCPP_INFO(this->get_logger(), "Joint value %f", joint); - } - - - move_group_interface.setJointValueTarget(goal->joint_values); - move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor); - move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor); - - - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); - if(success) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - move_group_interface.execute(plan); - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } - - - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } }; // class MoveToJointStateActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToJointStateActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToJointStateActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp index 4bf83ad..aba0780 100644 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ b/rbs_skill_servers/src/move_topose_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -18,8 +18,8 @@ // moveit libs #include "moveit/move_group_interface/move_group_interface.h" #include "moveit/planning_interface/planning_interface.h" -#include "moveit/robot_model_loader/robot_model_loader.h" #include "moveit/planning_scene_interface/planning_scene_interface.h" +#include "moveit/robot_model_loader/robot_model_loader.h" /* #include @@ -27,148 +27,149 @@ #include */ // For Visualization -//#include -#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" +#include -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToPoseActionServer : public rclcpp::Node -{ +class MoveToPoseActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_topose_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_topose", - std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_topose", + std::bind(&MoveToPoseActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToPoseActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToPoseActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - move_group_interface.setPoseTarget(goal->target_pose); - move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); - move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration); - moveit::planning_interface::MoveGroupInterface::Plan plan; - moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan); - if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){ - move_group_interface.plan(plan); - } - if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - //move_group_interface.execute(plan); - moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan); - if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - }else{ - RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code)); - goal_handle->abort(result); - } + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), + goal_handle) + .detach(); + } - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + move_group_interface.setPoseTarget(goal->target_pose); + move_group_interface.setMaxVelocityScalingFactor( + goal->end_effector_velocity); + move_group_interface.setMaxAccelerationScalingFactor( + goal->end_effector_acceleration); + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveit::core::MoveItErrorCode plan_err_code = + move_group_interface.plan(plan); + if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) { + move_group_interface.plan(plan); } + if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + // move_group_interface.execute(plan); + moveit::core::MoveItErrorCode move_err_code = + move_group_interface.execute(plan); + if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "Failure in move:"); + } + + } else { + RCLCPP_WARN_STREAM(this->get_logger(), + "Failed to generate plan because " + << error_code_to_string(plan_err_code)); + goal_handle->abort(result); + } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; + } + } }; // class MoveToPoseActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToPoseActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToPoseActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp index 366bf35..8840992 100644 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -1,20 +1,20 @@ +#include #include #include -#include -#include #include #include -#include #include +#include +#include #include -#include -#include #include +#include +#include -#include #include +#include #include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp" @@ -22,8 +22,8 @@ #define INIT_SCENE_PARAM_NAME "init_scene" #define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths" -static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &pose) -{ +static geometry_msgs::msg::Pose +pose_from_pose3d(const ignition::math::Pose3d &pose) { geometry_msgs::msg::Pose result; auto position = pose.Pos(); result.position.set__x(position.X()); @@ -37,129 +37,125 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p return result; } -static std::string get_correct_mesh_path( - const std::string& uri, const std::vector &resources) -{ +static std::string +get_correct_mesh_path(const std::string &uri, + const std::vector &resources) { std::string result = ""; std::regex reg(R"((?:model|package)(?:\:\/)(.*))"); std::smatch m; - if (std::regex_match(uri, m, reg)) - { + if (std::regex_match(uri, m, reg)) { std::string rel_path = m[1]; - std::for_each(resources.begin(), resources.end(), - [&result, &rel_path](const std::string& res){ - if (result.empty()) - result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result; - }); + std::for_each(resources.begin(), resources.end(), + [&result, &rel_path](const std::string &res) { + if (result.empty()) + result = std::filesystem::exists(res + rel_path) + ? std::string(res + rel_path) + : result; + }); } - return "file://" + result; + return "file://" + result; } -static moveit_msgs::msg::CollisionObject -get_object(const sdf::Model *model, const std::vector &resources) -{ +static moveit_msgs::msg::CollisionObject +get_object(const sdf::Model *model, const std::vector &resources) { moveit_msgs::msg::CollisionObject obj; obj.header.frame_id = "world"; obj.id = model->Name(); obj.pose = pose_from_pose3d(model->RawPose()); size_t link_count = model->LinkCount(); - for (size_t i = 0; i < link_count; ++i) - { + for (size_t i = 0; i < link_count; ++i) { auto link = model->LinkByIndex(i); auto collision = link->CollisionByIndex(0); auto link_pose = pose_from_pose3d(link->RawPose()); auto geometry = collision->Geom(); - switch(geometry->Type()) - { - case sdf::GeometryType::MESH: - { - auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); - shapes::Mesh *m = shapes::createMeshFromResource( - !path.empty()? path: geometry->MeshShape()->Uri()); - auto scale = geometry->MeshShape()->Scale().X(); - m->scale(scale); + switch (geometry->Type()) { + case sdf::GeometryType::MESH: { + auto path = + get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); + shapes::Mesh *m = shapes::createMeshFromResource( + !path.empty() ? path : geometry->MeshShape()->Uri()); + auto scale = geometry->MeshShape()->Scale().X(); + m->scale(scale); - Eigen::Vector3d centroid = {0, 0, 0}; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) - { - const auto x = m->vertices[i]; - const auto y = m->vertices[i+1]; - const auto z = m->vertices[i+2]; + Eigen::Vector3d centroid = {0, 0, 0}; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + const auto x = m->vertices[i]; + const auto y = m->vertices[i + 1]; + const auto z = m->vertices[i + 2]; - centroid.x() += x*(1-scale); - centroid.y() += y*(1-scale); - centroid.z() += z*(1-scale); - } - - centroid = centroid / m->vertex_count; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { - m->vertices[i] -= centroid.x(); - m->vertices[i + 1] -= centroid.y(); - m->vertices[i + 2] -= centroid.z(); - } - - shape_msgs::msg::Mesh mesh; - shapes::ShapeMsg m_msg; - shapes::constructMsgFromShape(m, m_msg); - mesh = boost::get(m_msg); - obj.meshes.push_back(mesh); - obj.mesh_poses.push_back(link_pose); - break; - } case sdf::GeometryType::BOX: { - auto sdf_box = geometry->BoxShape(); - shape_msgs::msg::SolidPrimitive box; - box.type = shape_msgs::msg::SolidPrimitive::BOX; - auto sdf_box_size = sdf_box->Size(); - box.dimensions.push_back(sdf_box_size.X()); - box.dimensions.push_back(sdf_box_size.Y()); - box.dimensions.push_back(sdf_box_size.Z()); - obj.primitives.push_back(box); - obj.primitive_poses.push_back(link_pose); - break; + centroid.x() += x * (1 - scale); + centroid.y() += y * (1 - scale); + centroid.z() += z * (1 - scale); } - case sdf::GeometryType::PLANE: - { - auto sdf_plane = geometry->PlaneShape(); - shape_msgs::msg::Plane plane; - auto normal = sdf_plane->Normal(); - plane.coef[0] = normal.X(); - plane.coef[1] = normal.Y(); - plane.coef[2] = normal.Z(); - obj.planes.push_back(plane); - obj.plane_poses.push_back(link_pose); - break; + + centroid = centroid / m->vertex_count; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + m->vertices[i] -= centroid.x(); + m->vertices[i + 1] -= centroid.y(); + m->vertices[i + 2] -= centroid.z(); } - case sdf::GeometryType::EMPTY: - case sdf::GeometryType::CYLINDER: - case sdf::GeometryType::SPHERE: - case sdf::GeometryType::HEIGHTMAP: - case sdf::GeometryType::CAPSULE: - case sdf::GeometryType::ELLIPSOID: - case sdf::GeometryType::POLYLINE: - break; + + shape_msgs::msg::Mesh mesh; + shapes::ShapeMsg m_msg; + shapes::constructMsgFromShape(m, m_msg); + mesh = boost::get(m_msg); + obj.meshes.push_back(mesh); + obj.mesh_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::BOX: { + auto sdf_box = geometry->BoxShape(); + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + auto sdf_box_size = sdf_box->Size(); + box.dimensions.push_back(sdf_box_size.X()); + box.dimensions.push_back(sdf_box_size.Y()); + box.dimensions.push_back(sdf_box_size.Z()); + obj.primitives.push_back(box); + obj.primitive_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::PLANE: { + auto sdf_plane = geometry->PlaneShape(); + shape_msgs::msg::Plane plane; + auto normal = sdf_plane->Normal(); + plane.coef[0] = normal.X(); + plane.coef[1] = normal.Y(); + plane.coef[2] = normal.Z(); + obj.planes.push_back(plane); + obj.plane_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::EMPTY: + case sdf::GeometryType::CYLINDER: + case sdf::GeometryType::SPHERE: + case sdf::GeometryType::HEIGHTMAP: + case sdf::GeometryType::CAPSULE: + case sdf::GeometryType::ELLIPSOID: + case sdf::GeometryType::POLYLINE: + break; } } return obj; } -static std::vector -get_objects(const sdf::World *world, const std::string &model_resources) -{ +static std::vector +get_objects(const sdf::World *world, const std::string &model_resources) { std::vector resources; std::regex reg("\\:+"); - std::sregex_token_iterator begin( - model_resources.begin(), model_resources.end(), reg, -1), end; + std::sregex_token_iterator begin(model_resources.begin(), + model_resources.end(), reg, -1), + end; std::copy(++begin, end, std::back_inserter(resources)); std::vector result; auto models_count = world->ModelCount(); - for (size_t i = 0; i < models_count; ++i) - { - try{ + for (size_t i = 0; i < models_count; ++i) { + try { auto model = world->ModelByIndex(i); result.push_back(get_object(model, resources)); - } catch (std::exception &ex){ + } catch (std::exception &ex) { std::cerr << ex.what() << std::endl; } } @@ -167,81 +163,80 @@ get_objects(const sdf::World *world, const std::string &model_resources) return result; } -class UpdatePlanningSceneServer: public rclcpp::Node -{ +class UpdatePlanningSceneServer : public rclcpp::Node { public: - UpdatePlanningSceneServer(rclcpp::NodeOptions options) - : Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); - std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); + UpdatePlanningSceneServer(rclcpp::NodeOptions options) + : Node("update_planning_scene_node", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); + std::string model_resources = + get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); - if (!scene.empty()) - { - RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); - init_scene(scene, model_resources); - } - service_ = create_service( - SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + if (!scene.empty()) { + RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); + init_scene(scene, model_resources); + } + service_ = + create_service( + SERVICE_NAME, + std::bind(&UpdatePlanningSceneServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + bool init_scene(const std::string &init_scene, + const std::string &model_resources) { + sdf::Root root; + sdf::ParserConfig config; + config.AddURIPath("package://", model_resources); + config.AddURIPath("model://", model_resources); + sdf::Errors errors = root.Load(init_scene, config); + if (!errors.empty()) { + for (auto const &e : errors) + RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str()); + return false; + } + auto world = root.WorldByIndex(0); + auto objects = get_objects(world, model_resources); + planning_scene_.applyCollisionObjects(objects); + return true; + } + + void + callback(std::shared_ptr< + rbs_skill_interfaces::srv::PlanningSceneObjectState::Request> + request, + std::shared_ptr< + rbs_skill_interfaces::srv::PlanningSceneObjectState::Response> + response) { + auto state = + static_cast(request->req_kind); + moveit_msgs::msg::CollisionObject obj; + obj.id = request->frame_name; + switch (state) { + case ADD: + case UPDATE: + obj.meshes.resize(1); + obj.mesh_poses.resize(1); + obj.operation = state == ADD ? moveit_msgs::msg::CollisionObject::ADD + : moveit_msgs::msg::CollisionObject::MOVE; + obj.meshes.at(0) = std::move(request->mesh); + obj.mesh_poses.at(0) = std::move(request->pose.pose); + break; + case REMOVE: + obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; + break; } - bool init_scene(const std::string &init_scene, const std::string &model_resources) - { - sdf::Root root; - sdf::ParserConfig config; - config.AddURIPath("package://", model_resources); - config.AddURIPath("model://", model_resources); - sdf::Errors errors = root.Load(init_scene, config); - if (!errors.empty()) - { - for (auto const &e: errors) - RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str()); - return false; - } - auto world = root.WorldByIndex(0); - auto objects = get_objects(world, model_resources); - planning_scene_.applyCollisionObjects(objects); - return true; - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - moveit_msgs::msg::CollisionObject obj; - obj.id = request->frame_name; - switch(state) - { - case ADD: - case UPDATE: - obj.meshes.resize(1); - obj.mesh_poses.resize(1); - obj.operation = state == ADD? - moveit_msgs::msg::CollisionObject::ADD: - moveit_msgs::msg::CollisionObject::MOVE; - obj.meshes.at(0) = std::move(request->mesh); - obj.mesh_poses.at(0) = std::move(request->pose.pose); - break; - case REMOVE: - obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; - break; - } - - response->call_status = planning_scene_.applyCollisionObject(obj); - } + response->call_status = planning_scene_.applyCollisionObject(obj); + } private: - enum UpdatePlanningSceneRequestState - { - ADD=0, - REMOVE=1, - UPDATE=2 - }; + enum UpdatePlanningSceneRequestState { ADD = 0, REMOVE = 1, UPDATE = 2 }; - rclcpp::Service::SharedPtr service_; - moveit::planning_interface::PlanningSceneInterface planning_scene_; + rclcpp::Service< + rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_; + moveit::planning_interface::PlanningSceneInterface planning_scene_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp index 423f983..1818727 100644 --- a/rbs_skill_servers/src/pick_place_pose_loader.cpp +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -1,4 +1,4 @@ -//#include "rbs_skill_servers" +// #include "rbs_skill_servers" #include "rbs_skill_servers/pick_place_pose_loader.hpp" using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer; @@ -6,105 +6,107 @@ using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses; using namespace std::chrono_literals; // rclcpp::Node::SharedPtr g_node = nullptr; - - GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options) -: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) -{ - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + : Node("grasp_place_pose_loader", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - srv_ = create_service("/get_pick_place_pose_service", - std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2)); + srv_ = create_service( + "/get_pick_place_pose_service", + std::bind(&GetGraspPickPoseServer::handle_server, this, + std::placeholders::_1, std::placeholders::_2)); } -void -GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, - rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response) -{ - std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name - try { - tf_data = tf_buffer_->lookupTransform( - "world", rrr.c_str(), - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not transform %s to %s: %s", - rrr.c_str(), "world", ex.what()); - return; - } - // TODO: Here need to check the parameter - // We can use another parameter library from PickNik - grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array(); - RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->grasp_direction.x, - request->grasp_direction.y, - request->grasp_direction.z); - RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->place_direction.x, - request->place_direction.y, - request->place_direction.z); - Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); - Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); - //std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl; - //std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl; - Eigen::Vector3d vec_grasp(0.15,0.15,0.02); - Eigen::Vector3d vec_place(0,0,0.15); - response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp); - response->place = collect_pose(place_pose, request->place_direction, vec_place); +void GetGraspPlacePoseServer::handle_server( + const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr + request, + rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr + response) { + std::string rrr = + "ASSEMBLE_" + request->object_name; // TODO: replace with better name + try { + tf_data = + tf_buffer_->lookupTransform("world", rrr.c_str(), tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s", + rrr.c_str(), "world", ex.what()); + return; + } + // TODO: Here need to check the parameter + // We can use another parameter library from PickNik + grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose") + .as_double_array(); + RCLCPP_INFO(this->get_logger(), + "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->grasp_direction.x, request->grasp_direction.y, + request->grasp_direction.z); + RCLCPP_INFO(this->get_logger(), + "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->place_direction.x, request->place_direction.y, + request->place_direction.z); + Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); + Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); + // std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << + // std::endl << grasp_pose.linear() << std::endl; std::cout << "place_pose = " + // << std::endl << place_pose.translation() << std::endl << + // place_pose.linear() << std::endl; + Eigen::Vector3d vec_grasp(0.15, 0.15, 0.02); + Eigen::Vector3d vec_place(0, 0, 0.15); + response->grasp = + collect_pose(grasp_pose, request->grasp_direction, vec_grasp); + response->place = + collect_pose(place_pose, request->place_direction, vec_place); } -std::vector -GetGraspPlacePoseServer::collect_pose( - const Eigen::Affine3d pose, - const geometry_msgs::msg::Vector3 direction, - const Eigen::Vector3d scale_vec) -{ - std::vector pose_v_; - pose_v_.push_back(tf2::toMsg(pose)); - Eigen::Vector3d posedir; - Eigen::Affine3d pose_ = pose; - tf2::fromMsg(direction, posedir); - Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose(); - Ixy.diagonal() = posedir; - Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero(); - Iz.diagonal() = Eigen::Vector3d::UnitZ(); +std::vector GetGraspPlacePoseServer::collect_pose( + const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, + const Eigen::Vector3d scale_vec) { + std::vector pose_v_; + pose_v_.push_back(tf2::toMsg(pose)); + Eigen::Vector3d posedir; + Eigen::Affine3d pose_ = pose; + tf2::fromMsg(direction, posedir); + Eigen::Matrix3d Ixy = + Eigen::Matrix3d::Zero(); // posedir * posedir.transpose(); + Ixy.diagonal() = posedir; + Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero(); + Iz.diagonal() = Eigen::Vector3d::UnitZ(); - if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0] - { - std::cout << "\n TBD" << std::endl; - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0] - { - // Gp -- grasp point frame - pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Iz * scale_vec); // Gp-y + z - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Ixy * scale_vec); // Gp+z - pose_v_.push_back(tf2::toMsg(pose_)); - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1] - { - pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp - pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp - } - else - { - std::cout << "\n TBD" << std::endl; - } - return pose_v_; + if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()) + .all()) // IF Direction == [1,0,0] + { + std::cout << "\n TBD" << std::endl; + } else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()) + .all()) // IF Direction == [0,1,0] + { + // Gp -- grasp point frame + pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y + pose_v_.push_back(tf2::toMsg(pose_)); + pose_.pretranslate(Iz * scale_vec); // Gp-y + z + pose_v_.push_back(tf2::toMsg(pose_)); + pose_.pretranslate(Ixy * scale_vec); // Gp+z + pose_v_.push_back(tf2::toMsg(pose_)); + } else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()) + .all()) // IF Direction == [0,0,1] + { + pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp + pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp + } else { + std::cout << "\n TBD" << std::endl; + } + return pose_v_; } -Eigen::Affine3d -GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) -{ - Eigen::Vector3d point(std::vector(pose.begin(), pose.begin()+3).data()); - Eigen::Quaterniond quat(std::vector(pose.begin()+3, pose.end()).data()); - Eigen::Affine3d aff; - aff.translation() = point; - aff.linear() = quat.toRotationMatrix(); - return aff; +Eigen::Affine3d +GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) { + Eigen::Vector3d point( + std::vector(pose.begin(), pose.begin() + 3).data()); + Eigen::Quaterniond quat( + std::vector(pose.begin() + 3, pose.end()).data()); + Eigen::Affine3d aff; + aff.translation() = point; + aff.linear() = quat.toRotationMatrix(); + return aff; }