robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

clang-format

Browse source
This commit is contained in:
Ilya Uraev 2023-11-09 15:39:19 +03:00
parent 18d2772cef
commit 6eee02bacc
17 changed files with 1299 additions and 1346 deletions
Download patch file Download diff file Expand all files Collapse all files

17
env_manager/env_interface/include/env_interface/env_interface.hpp
View file

@ -2,13 +2,10 @@
namespace env_interface namespace env_interface
{ {
class EnvInterface : public EnvInterfaceBase class EnvInterface : public EnvInterfaceBase
{ {
public: public:
EnvInterface() = default; EnvInterface() = default;
virtual ~EnvInterface() = default; virtual ~EnvInterface() = default;
};
}; } // namespace env_interface
} // namespace env_interface

36
env_manager/env_interface/include/env_interface/env_interface_base.hpp
View file

@ -1,38 +1,40 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp"
#include <cstddef>
#include <cstdint>
namespace env_interface namespace env_interface
{ {
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// template <typename A_space, typename S_space>
class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{ {
public: public:
EnvInterfaceBase() = default; EnvInterfaceBase() = default;
virtual ~EnvInterfaceBase() = default; virtual ~EnvInterfaceBase() = default;
virtual CallbackReturn init( virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "",
const std::string& t_env_name, const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions());
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<rclcpp_lifecycle::LifecycleNode> getNode(); std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode();
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode() const; std::shared_ptr<rclcpp_lifecycle::LifecycleNode> 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: private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_node; std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_node;
}; };
} // namespace env_interface } // namespace env_interface

68
env_manager/env_interface/src/env_interface_base.cpp
View file

@ -3,61 +3,43 @@
namespace env_interface namespace env_interface
{ {
CallbackReturn CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace,
EnvInterfaceBase::init( const rclcpp::NodeOptions& t_node_options)
const std::string& t_env_name,
const std::string & t_namespace,
const rclcpp::NodeOptions & t_node_options)
{ {
m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(t_env_name, t_namespace, t_node_options, false);
t_env_name, t_namespace, t_node_options, false);
if (on_init() == CallbackReturn::FAILURE) if (on_init() == CallbackReturn::FAILURE)
{ {
return CallbackReturn::FAILURE; return CallbackReturn::FAILURE;
} }
m_node->register_on_configure( m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1));
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_activate( m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1));
std::bind(&EnvInterfaceBase::on_activate, 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_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& const rclcpp_lifecycle::State& EnvInterfaceBase::configure()
EnvInterfaceBase::configure()
{ {
return getNode()->configure(); return getNode()->configure();
} }
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> std::shared_ptr<rclcpp_lifecycle::LifecycleNode> EnvInterfaceBase::getNode()
EnvInterfaceBase::getNode()
{ {
if (!m_node.get()) if (!m_node.get())
{ {
throw std::runtime_error("Lifecycle node hasn't been initialized yet"); throw std::runtime_error("Lifecycle node hasn't been initialized yet");
} }
return m_node; 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

113
env_manager/env_manager/include/env_manager/env_manager.hpp
View file

@ -11,7 +11,6 @@
#include "pluginlib/class_loader.hpp" #include "pluginlib/class_loader.hpp"
namespace env_manager namespace env_manager
{ {
using EnvInterface = env_interface::EnvInterface; using EnvInterface = env_interface::EnvInterface;
@ -20,80 +19,68 @@ using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInter
struct EnvSpec struct EnvSpec
{ {
std::string name; std::string name;
std::string type; std::string type;
EnvInterfaceSharedPtr env_ptr; EnvInterfaceSharedPtr env_ptr;
}; };
class EnvManager : public rclcpp::Node class EnvManager : public rclcpp::Node
{ {
public: public:
// EnvManager(rclcpp::Executor::SharedPtr executor, // EnvManager(rclcpp::Executor::SharedPtr executor,
// const std::string & json_config_path, // const std::string & json_config_path,
// const std::string & node_name = "env_manager", // const std::string & node_name = "env_manager",
// const std::string & node_namespace = "", // const std::string & node_namespace = "",
// rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()
// .allow_undeclared_parameters(true) // .allow_undeclared_parameters(true)
// .automatically_declare_parameters_from_overrides(true)); // .automatically_declare_parameters_from_overrides(true));
EnvManager(rclcpp::Executor::SharedPtr executor, EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager",
const std::string & node_name = "env_manager", const std::string& node_namespace = "",
const std::string & node_namespace = "", rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()
rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true)
.allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true));
.automatically_declare_parameters_from_overrides(true)); virtual ~EnvManager() = default;
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); // TODO: Define the input data format
EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); // Set Target for RL enviroment
EnvStateReturnType unloadEnv(const std::string &env_name); EnvInterfaceSharedPtr setTarget();
EnvStateReturnType configureEnv(const std::string& env_name); EnvInterfaceSharedPtr switchTarget();
EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); EnvInterfaceSharedPtr unsetTarget();
// Load Constraints for RL enviroment
//TODO: Define the input data format EnvInterfaceSharedPtr loadConstraints();
// Set Target for RL enviroment EnvInterfaceSharedPtr switchConstraints();
EnvInterfaceSharedPtr setTarget(); EnvInterfaceSharedPtr unloadConstraints();
EnvInterfaceSharedPtr switchTarget();
EnvInterfaceSharedPtr unsetTarget();
// Load Constraints for RL enviroment
EnvInterfaceSharedPtr loadConstraints();
EnvInterfaceSharedPtr switchConstraints();
EnvInterfaceSharedPtr unloadConstraints();
protected: protected:
void initServices(); void initServices();
rclcpp::Node::SharedPtr getNode(); rclcpp::Node::SharedPtr getNode();
void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
private: private:
rclcpp::Node::SharedPtr m_node; rclcpp::Node::SharedPtr m_node;
std::mutex m_env_mutex; std::mutex m_env_mutex;
std::vector<EnvSpec> m_active_envs; std::vector<EnvSpec> m_active_envs;
rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr m_load_env_srv;
m_load_env_srv; rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr m_configure_env_srv;
rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr m_unload_env_srv;
m_configure_env_srv;
rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
m_unload_env_srv;
rclcpp::CallbackGroup::SharedPtr m_callback_group;
rclcpp::Executor::SharedPtr m_executor;
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
rclcpp::CallbackGroup::SharedPtr m_callback_group;
rclcpp::Executor::SharedPtr m_executor;
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
}; };
} // namespace env_manager } // namespace env_manager
#endif // __ENV_MANAGER_HPP__ #endif // __ENV_MANAGER_HPP__

269
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,22 +1,22 @@
#include "env_manager/env_manager.hpp" #include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp" #include "nlohmann/json.hpp"
static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "env_interface"; static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "env_interface::EnvInterface"; "env_interface";
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME =
"env_interface::EnvInterface";
namespace env_manager namespace env_manager {
{ EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
EnvManager::EnvManager( const std::string &node_name,
rclcpp::Executor::SharedPtr executor, const std::string &node_namespace,
const std::string & node_name, rclcpp::NodeOptions &options)
const std::string & node_namespace, : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
rclcpp::NodeOptions & options) m_loader(
: rclcpp::Node(node_name, node_namespace, options), std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
m_executor(executor), ENV_INTERFACE_BASE_CLASS_NAMESPACE,
m_loader(std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>( ENV_INTERFACE_BASE_CLASS_NAME)) {
ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) initServices();
{
initServices();
} }
// EnvManager::EnvManager( // EnvManager::EnvManager(
@ -24,7 +24,7 @@ EnvManager::EnvManager(
// const std::string & json_config_path, // const std::string & json_config_path,
// const std::string & node_name, // const std::string & node_name,
// const std::string & node_namespace, // const std::string & node_namespace,
// rclcpp::NodeOptions & options) // rclcpp::NodeOptions & options)
// : rclcpp::Node(node_name, node_namespace, options), // : rclcpp::Node(node_name, node_namespace, options),
// m_executor(executor), // m_executor(executor),
// m_loader(std::make_shared<pluginlib::ClassLoader<rclcpp_lifecycle::LifecycleNode>>( // m_loader(std::make_shared<pluginlib::ClassLoader<rclcpp_lifecycle::LifecycleNode>>(
@ -33,130 +33,111 @@ EnvManager::EnvManager(
// initServices(); // initServices();
// } // }
void EnvManager::initServices() {
using namespace std::placeholders;
m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv>(
void "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2));
EnvManager::initServices() m_configure_env_srv =
{ create_service<env_manager_interfaces::srv::ConfigureEnv>(
using namespace std::placeholders; "~/configure_env",
std::bind(&EnvManager::configureEnv_cb, this, _1, _2));
m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv> m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>(
("~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2));
m_configure_env_srv = create_service<env_manager_interfaces::srv::ConfigureEnv>
("~/configure_env", std::bind(&EnvManager::configureEnv_cb, this, _1, _2));
m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>
("~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2));
} }
rclcpp::Node::SharedPtr rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; }
EnvManager::getNode()
{
return m_node;
}
void void EnvManager::loadEnv_cb(
EnvManager::loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) {
{ std::lock_guard<std::mutex> lock(m_env_mutex);
std::lock_guard<std::mutex> lock(m_env_mutex); response->ok = loadEnv(request->name, request->type).get() != nullptr;
response->ok = loadEnv(request->name, request->type).get() != nullptr;
} }
void void EnvManager::configureEnv_cb(
EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
{ std::lock_guard<std::mutex> guard(m_env_mutex);
std::lock_guard<std::mutex> guard(m_env_mutex); response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
} }
void void EnvManager::unloadEnv_cb(
EnvManager::unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
{ std::lock_guard<std::mutex> lock(m_env_mutex);
std::lock_guard<std::mutex> lock(m_env_mutex); response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
} }
EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
std::lock_guard<std::mutex> 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 m_active_envs.push_back(enviroment);
EnvManager::addEnv(const EnvSpec& enviroment) m_executor->add_node(
{ enviroment.env_ptr->getNode()->get_node_base_interface());
std::lock_guard<std::mutex> lock(m_env_mutex); return enviroment.env_ptr;
// 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;
} }
EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name,
EnvInterfaceSharedPtr const std::string &env_class_type) {
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());
EnvInterfaceSharedPtr loaded_env = nullptr; try {
RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); if (m_loader->isClassAvailable(env_class_type)) {
try loaded_env = m_loader->createSharedInstance(env_class_type);
{
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;
} else { } else {
RCLCPP_ERROR(this->get_logger(), RCLCPP_ERROR(
"Environment '%s' not found in the active environments list.", this->get_logger(),
env_name.c_str()); "Loading enviroment is not available '%s' with class type '%s'",
return EnvStateReturnType::ERROR; 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 // EnvInterfaceSharedPtr
@ -167,27 +148,25 @@ EnvManager::unloadEnv(const std::string& env_name)
// return loadEnv(env_name, env_class_type); // return loadEnv(env_name, env_class_type);
// } // }
EnvStateReturnType EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
EnvManager::configureEnv(const std::string& env_name) std::lock_guard<std::mutex> guard(m_env_mutex);
{
std::lock_guard<std::mutex> guard(m_env_mutex); auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { [&env_name](const EnvSpec &env) { return env.name == env_name; });
return env.name == env_name;
}); if (it == m_active_envs.end()) {
RCLCPP_ERROR(this->get_logger(),
if (it == m_active_envs.end()) { "Environment '%s' not found in the active environments list.",
RCLCPP_ERROR(this->get_logger(), env_name.c_str());
"Environment '%s' not found in the active environments list.", return EnvStateReturnType::FAILURE;
env_name.c_str()); }
return EnvStateReturnType::FAILURE; it->env_ptr->configure();
} // it->env_ptr->getNode()->configure();
it->env_ptr->configure(); // it->env_ptr->activate();
// it->env_ptr->getNode()->configure(); it->env_ptr->getNode()->activate();
// it->env_ptr->activate();
it->env_ptr->getNode()->activate(); return EnvStateReturnType::SUCCESS;
return EnvStateReturnType::SUCCESS;
} }
} } // namespace env_manager

25
env_manager/env_manager/src/run_env_manager.cpp
View file

@ -1,22 +1,21 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "env_manager/env_manager.hpp" #include "env_manager/env_manager.hpp"
int main(int argc, char ** argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Executor> executor = std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
std::make_shared<rclcpp::executors::MultiThreadedExecutor>(); std::string manager_node_name = "env_manager";
std::string manager_node_name = "env_manager";
auto em = std::make_shared<env_manager::EnvManager>(executor, manager_node_name); auto em = std::make_shared<env_manager::EnvManager>(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->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
em->configureEnv("gz_enviroment"); em->configureEnv("gz_enviroment");
executor->add_node(em); executor->add_node(em);
executor->spin(); executor->spin();
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }

1
env_manager/gz_enviroment/CMakeLists.txt
View file

@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
ignition-msgs8 ignition-msgs8
ros_gz_bridge ros_gz_bridge
env_interface env_interface
ignition-gazebo6
) )
# find dependencies # find dependencies

45
env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp
View file

@ -1,6 +1,7 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp" // #include "ros_gz_bridge/convert.hpp"
#include <string>
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp> #include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp> #include <geometry_msgs/msg/transform_stamped.hpp>
@ -8,6 +9,10 @@
#include "gz/msgs/pose_v.pb.h" #include "gz/msgs/pose_v.pb.h"
#include "env_interface/env_interface.hpp" #include "env_interface/env_interface.hpp"
#include <gz/sim/components/Model.hh>
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
namespace gz_enviroment namespace gz_enviroment
{ {
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 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 class GzEnviroment : public env_interface::EnvInterface
{ {
public: public:
GzEnviroment();
GzEnviroment(); CallbackReturn on_init() override;
CallbackReturn on_init() override; CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
protected: protected:
void onGzPoseSub(const gz::msgs::Pose_V& pose_v);
void onGzPoseSub(const gz::msgs::Pose_V& pose_v); bool doGzSpawn();
bool doGzSpawn();
private: private:
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster; std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
std::shared_ptr<gz::transport::Node> m_gz_node; std::shared_ptr<gz::transport::Node> m_gz_node;
std::vector<std::string> m_follow_frames; std::vector<std::string> m_follow_frames;
std::string m_topic_name; std::string m_topic_name;
std::string m_service_spawn; std::string m_service_spawn;
std::string m_world_name;
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath, const std::string& name);
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath,
const std::string& name);
}; };
} } // namespace gz_enviroment

272
env_manager/gz_enviroment/src/gz_enviroment.cpp
View file

@ -2,173 +2,179 @@
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "ros_gz_bridge/convert.hpp" #include "ros_gz_bridge/convert.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include <memory>
#include <rclcpp/logging.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
namespace gz_enviroment { #include <gz/sim/components.hh>
#include <string>
GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {} namespace gz_enviroment
{
GzEnviroment::GzEnviroment() : env_interface::EnvInterface()
{
}
CallbackReturn GzEnviroment::on_init() CallbackReturn GzEnviroment::on_init()
{ {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
return CallbackReturn::SUCCESS; gz::sim::EntityComponentManager ecm;
}
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) { // Получение всех сущностей, которые представляют модели
// Configuration of parameters const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model());
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_gz_node = std::make_shared<gz::transport::Node>(); // Вывод списка моделей
m_topic_name = std::string("/world/") + getGzWorldName() +"/dynamic_pose/info"; for (const auto entity : modelEntities)
m_service_spawn = std::string("/world/") + getGzWorldName() + "/create"; {
const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
if (!doGzSpawn()) { if (modelName)
return CallbackReturn::ERROR; {
std::cout << "Model Name: " << modelName->Data() << std::endl;
} }
}
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { CallbackReturn GzEnviroment::on_configure(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<tf2_ros::TransformBroadcaster>(getNode());
return CallbackReturn::SUCCESS;
}
CallbackReturn
GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
{ {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); // Configuration of parameters
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_tf2_broadcaster.reset(); m_gz_node = std::make_shared<gz::transport::Node>();
m_gz_node.reset(); m_world_name = getGzWorldName();
m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
return CallbackReturn::SUCCESS; m_service_spawn = std::string("/world/") + m_world_name + "/create";
if (!doGzSpawn())
{
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
} }
CallbackReturn CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&)
GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
{ {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); // Setting up the subscribers and publishers
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
return CallbackReturn::SUCCESS; m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(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 // TODO: Check do this via EntityComponentManager
void void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
{ {
// TODO: Read from config // TODO: Read from config
m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "monkey"}; m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6", "monkey" };
for (const auto &it: pose_v.pose()) for (const auto& it : pose_v.pose())
{ {
if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end())
continue; continue;
geometry_msgs::msg::PoseStamped rpose;
ros_gz_bridge::convert_gz_to_ros(it, rpose);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = getNode()->get_clock()->now(); geometry_msgs::msg::PoseStamped rpose;
t.header.frame_id = "world"; ros_gz_bridge::convert_gz_to_ros(it, rpose);
t.child_frame_id = it.name();
t.transform.translation.x = rpose.pose.position.x; geometry_msgs::msg::TransformStamped t;
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.header.stamp = getNode()->get_clock()->now();
t.transform.rotation.y = rpose.pose.orientation.y; t.header.frame_id = "world";
t.transform.rotation.z = rpose.pose.orientation.z; t.child_frame_id = it.name();
t.transform.rotation.w = rpose.pose.orientation.w;
t.transform.translation.x = rpose.pose.position.x;
m_tf2_broadcaster->sendTransform(t); 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 std::string GzEnviroment::getGzWorldName()
GzEnviroment::getGzWorldName()
{ {
bool executed{false}; bool executed{ false };
bool result{false}; bool result{ false };
unsigned int timeout{5000}; unsigned int timeout{ 5000 };
std::string service{"/gazebo/worlds"}; std::string service{ "/gazebo/worlds" };
gz::msgs::StringMsg_V worlds_msg; gz::msgs::StringMsg_V worlds_msg;
while (rclcpp::ok() && !executed) { while (rclcpp::ok() && !executed)
RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); {
executed = m_gz_node->Request(service, timeout, worlds_msg, result); RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
} executed = m_gz_node->Request(service, timeout, worlds_msg, result);
}
if (!executed) { if (!executed)
RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); {
return ""; RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
} return "";
}
if (!result || worlds_msg.data().empty()) { if (!result || worlds_msg.data().empty())
RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); {
return ""; 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); RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str());
return worlds_msg.data(0);
} }
bool bool GzEnviroment::doGzSpawn()
GzEnviroment::doGzSpawn()
{ {
gz::msgs::EntityFactory req; gz::msgs::EntityFactory req;
gz::msgs::Boolean rep; gz::msgs::Boolean rep;
bool result; bool result;
unsigned int timeout = 5000; unsigned int timeout = 5000;
// TODO: From Config // TODO: From Config
std::vector<double> pps{1.0, 0.0, 1.0, 0.0, 0.0}; std::vector<double> pps{ 1.0, 0.0, 1.0, 0.0, 0.0 };
std::string mesh_file = "monkey.stl"; std::string mesh_file = "monkey.stl";
std::string mesh_name = "monkey"; std::string mesh_name = "monkey";
// ENDTODO // ENDTODO
std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name);
req.set_sdf(sdf_string); req.set_sdf(sdf_string);
bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
return executed; return executed;
} }
std::string std::string GzEnviroment::createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath,
GzEnviroment::createGzModelString( const std::string& name)
const std::vector<double>& pose,
const std::string& mesh_filepath,
const std::string& name
)
{ {
std::string model_template = std::string model_template = std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
std::string("<sdf version='1.7'>") + "<link name='link_" + name + "'>" + "<pose>" + std::to_string(pose[0]) + " " +
"<model name='"+ name +"'>" + std::to_string(pose[1]) + " " + std::to_string(pose[2]) + " " + std::to_string(pose[3]) +
"<link name='link_"+ name +"'>" + " " + std::to_string(pose[4]) + " " + std::to_string(pose[5]) + "</pose>" +
"<pose>" "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" + mesh_filepath +
+std::to_string(pose[0])+" " "</uri></mesh></geometry>" + "</visual>" + "<collision name='collision_" + name + "'>" +
+std::to_string(pose[1])+" " "<geometry><mesh><uri>file://" + mesh_filepath + "</uri></mesh></geometry>" +
+std::to_string(pose[2])+" " "</collision>" + "</link>" + "</model>" + "</sdf>";
+std::to_string(pose[3])+" " return model_template;
+std::to_string(pose[4])+" "
+std::to_string(pose[5])+"</pose>" +
"<visual name='visual_"+name+"'>" +
"<geometry><mesh><uri>file://"+mesh_filepath+"</uri></mesh></geometry>" +
"</visual>" +
"<collision name='collision_"+name+"'>" +
"<geometry><mesh><uri>file://"+mesh_filepath+"</uri></mesh></geometry>" +
"</collision>" +
"</link>" +
"</model>" +
"</sdf>";
return model_template;
} }
} } // namespace gz_enviroment
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface);
gz_enviroment::GzEnviroment, env_interface::EnvInterface
);

59
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
View file

@ -1,24 +1,25 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include <algorithm>
#include <cinttypes> #include <cinttypes>
#include <memory> #include <memory>
#include <algorithm>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject; using AddPlanningSceneObject =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr; rclcpp::Node::SharedPtr g_node = nullptr;
void handle_service( void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<AddPlanningSceneObject::Request> request, const std::shared_ptr<AddPlanningSceneObject::Request> request,
const std::shared_ptr<AddPlanningSceneObject::Response> response) const std::shared_ptr<AddPlanningSceneObject::Response> response) {
{ (void)request_header;
(void)request_header; // Create collision object for the robot to avoid
// Create collision object for the robot to avoid auto const collision_object = [frame_id = "world",
auto const collision_object = [frame_id = object_name = request->object_id,
"world", object_name=request->object_id, object_pose=request->object_pose] { object_pose = request->object_pose] {
moveit_msgs::msg::CollisionObject collision_object; moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = frame_id; collision_object.header.frame_id = frame_id;
collision_object.id = object_name; collision_object.id = object_name;
@ -40,32 +41,32 @@ void handle_service(
box_pose.orientation.y = object_pose[4]; box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5]; box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6]; box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive); collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose); collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD; collision_object.operation = collision_object.ADD;
return collision_object; return collision_object;
}(); }();
// Add the collision object to the scene // Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object); planning_scene_interface.applyCollisionObject(collision_object);
response->success = true; response->success = true;
} }
int main(int argc, char ** argv) int main(int argc, char **argv) {
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv); rclcpp::NodeOptions node_options;
rclcpp::NodeOptions node_options; node_options.allow_undeclared_parameters(true)
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); .automatically_declare_parameters_from_overrides(true);
g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); g_node =
auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service); rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
rclcpp::spin(g_node); auto server = g_node->create_service<AddPlanningSceneObject>(
rclcpp::shutdown(); "add_planing_scene_object_service", handle_service);
g_node = nullptr; rclcpp::spin(g_node);
return 0; rclcpp::shutdown();
} g_node = nullptr;
return 0;
}

324
rbs_skill_servers/src/assemble_state_server.cpp
View file

@ -1,18 +1,18 @@
#include <filesystem>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector> #include <vector>
#include <filesystem>
#include <tinyxml2.h>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <tinyxml2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp> #include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.h> #include <geometry_msgs/msg/twist.h>
#include <rbs_skill_interfaces/srv/assemble_state.hpp> #include <rbs_skill_interfaces/srv/assemble_state.hpp>
@ -24,12 +24,11 @@
#define ASSEMBLE_POSITION_OFFSET 0.5 #define ASSEMBLE_POSITION_OFFSET 0.5
#define ASSEMBLE_ORIENTATION_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" (package_dir) + "/models/" + (assemble_name) + "/model.sdf"
static inline geometry_msgs::msg::Pose get_pose( static inline geometry_msgs::msg::Pose
const std::vector<std::string> & tokens) get_pose(const std::vector<std::string> &tokens) {
{
geometry_msgs::msg::Pose p; geometry_msgs::msg::Pose p;
p.position.set__x(std::stod(tokens.at(0))); p.position.set__x(std::stod(tokens.at(0)));
p.position.set__y(std::stod(tokens.at(1))); p.position.set__y(std::stod(tokens.at(1)));
@ -41,23 +40,22 @@ static inline geometry_msgs::msg::Pose get_pose(
return p; return p;
} }
static inline geometry_msgs::msg::PoseStamped get_pose_stamped( static inline geometry_msgs::msg::PoseStamped
const std::string & pose_string) get_pose_stamped(const std::string &pose_string) {
{
std::stringstream ss(pose_string); std::stringstream ss(pose_string);
std::istream_iterator<std::string> begin(ss); std::istream_iterator<std::string> begin(ss);
std::istream_iterator<std::string> end; std::istream_iterator<std::string> end;
std::vector<std::string> tokens(begin, end); std::vector<std::string> tokens(begin, end);
geometry_msgs::msg::PoseStamped ps; geometry_msgs::msg::PoseStamped ps;
ps.set__pose(get_pose(tokens)); ps.set__pose(get_pose(tokens));
return ps; return ps;
} }
static std::vector<geometry_msgs::msg::PoseStamped> get_assemble_poses( static std::vector<geometry_msgs::msg::PoseStamped>
const std::string & assemble_name, const std::string & part_name, get_assemble_poses(const std::string &assemble_name,
const std::string& package_dir, const std::string & assemble_prefix) const std::string &part_name, const std::string &package_dir,
{ const std::string &assemble_prefix) {
std::vector<geometry_msgs::msg::PoseStamped> result; std::vector<geometry_msgs::msg::PoseStamped> result;
std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf"; std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf";
@ -66,12 +64,11 @@ static std::vector<geometry_msgs::msg::PoseStamped> get_assemble_poses(
auto model = doc.RootElement()->FirstChildElement("model"); auto model = doc.RootElement()->FirstChildElement("model");
auto joint = model->FirstChildElement("joint"); auto joint = model->FirstChildElement("joint");
while (joint) while (joint) {
{
auto frame_id = joint->FirstChildElement("child")->GetText(); auto frame_id = joint->FirstChildElement("child")->GetText();
if (frame_id != part_name) if (frame_id != part_name)
continue; 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); ps.header.set__frame_id(assemble_prefix + part_name);
result.push_back(ps); result.push_back(ps);
joint = joint->NextSiblingElement("joint"); joint = joint->NextSiblingElement("joint");
@ -80,164 +77,167 @@ static std::vector<geometry_msgs::msg::PoseStamped> get_assemble_poses(
return result; return result;
} }
class AssembleStateServer: public rclcpp::Node class AssembleStateServer : public rclcpp::Node {
{
public: public:
AssembleStateServer(rclcpp::NodeOptions options) AssembleStateServer(rclcpp::NodeOptions options)
: Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) : Node(SERVICE_NAME,
{ options.allow_undeclared_parameters(true)
assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); .automatically_declare_parameters_from_overrides(true)) {
assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock()); assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string();
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_); tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
service_ = create_service<rbs_skill_interfaces::srv::AssembleState>( tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, service_ = create_service<rbs_skill_interfaces::srv::AssembleState>(
std::placeholders::_1, std::placeholders::_2)); SERVICE_NAME, std::bind(&AssembleStateServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
void
callback(std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request>
request,
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response>
response) {
auto state = static_cast<AssembleReqState>(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( response->call_status = call_status;
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request> request, response->assemble_name = assemble_name;
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response> response) }
{
auto state = static_cast<AssembleReqState>(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;
}
private: private:
enum AssembleReqState enum AssembleReqState {
{ NONE = -1,
NONE=-1, INITIALIZE = 0,
INITIALIZE=0, VALIDATE = 1,
VALIDATE=1, COMPLETE = 2
COMPLETE=2 };
};
struct Assemble struct Assemble {
{ std::string part;
std::string part; std::string ws;
std::string ws; AssembleReqState state;
AssembleReqState state; std::vector<geometry_msgs::msg::PoseStamped> poses;
std::vector<geometry_msgs::msg::PoseStamped> poses; std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster; };
};
bool on_initialize(Assemble* assemble) bool on_initialize(Assemble *assemble) {
{ RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s",
RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str()); assemble->part.c_str());
try try {
{ assemble->tf2_broadcaster =
assemble->tf2_broadcaster = std::make_unique<tf2_ros::StaticTransformBroadcaster>(this); std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
} catch (const tf2::TransformException &ex){ } catch (const tf2::TransformException &ex) {
RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what()); RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s",
return false; ex.what());
} return false;
assemble->state = INITIALIZE; }
for (const auto& it: assemble->poses) assemble->state = INITIALIZE;
{ for (const auto &it : assemble->poses) {
geometry_msgs::msg::TransformStamped t; geometry_msgs::msg::TransformStamped t;
t.header.frame_id = assemble->ws; t.header.frame_id = assemble->ws;
t.child_frame_id = it.header.frame_id; t.child_frame_id = it.header.frame_id;
auto pose = it.pose; auto pose = it.pose;
t.transform.translation.x = pose.position.x; t.transform.translation.x = pose.position.x;
t.transform.translation.y = pose.position.y; t.transform.translation.y = pose.position.y;
t.transform.translation.z = pose.position.z; t.transform.translation.z = pose.position.z;
t.transform.rotation.x = pose.orientation.x; t.transform.rotation.x = pose.orientation.x;
t.transform.rotation.y = pose.orientation.y; t.transform.rotation.y = pose.orientation.y;
t.transform.rotation.z = pose.orientation.z; t.transform.rotation.z = pose.orientation.z;
t.transform.rotation.w = pose.orientation.w; t.transform.rotation.w = pose.orientation.w;
assemble->tf2_broadcaster->sendTransform(t); assemble->tf2_broadcaster->sendTransform(t);
}
return true;
} }
bool on_validate(Assemble* assemble) return true;
{ }
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;
RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate); bool on_validate(Assemble *assemble) {
assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE; 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) assemble->state = COMPLETE;
{
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; return true;
}
return true;
}
private: private:
std::map<std::string, Assemble> assembles_; std::map<std::string, Assemble> assembles_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_; std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_; std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster_; std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster_;
std::mutex mt; std::mutex mt;
rclcpp::Service<rbs_skill_interfaces::srv::AssembleState>::SharedPtr service_; rclcpp::Service<rbs_skill_interfaces::srv::AssembleState>::SharedPtr service_;
std::string assemble_dir_; std::string assemble_dir_;
std::string assemble_prefix_; std::string assemble_prefix_;
}; };
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"

155
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

@ -7,108 +7,113 @@
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
// action libs // action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "rbs_skill_interfaces/action/gripper_command.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 "sensor_msgs/msg/joint_state.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
namespace rbs_skill_actions { namespace rbs_skill_actions {
class GripperControlActionServer: public rclcpp::Node { class GripperControlActionServer : public rclcpp::Node {
public: public:
using GripperCommand = rbs_skill_interfaces::action::GripperCommand; using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true)) explicit GripperControlActionServer(rclcpp::NodeOptions options)
{ : Node("gripper_control_action_server",
this->actionServer_ = rclcpp_action::create_server<GripperCommand>( options.allow_undeclared_parameters(true)) {
this, this->actionServer_ = rclcpp_action::create_server<GripperCommand>(
"gripper_control", this, "gripper_control",
std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), std::bind(&GripperControlActionServer::goal_callback, this,
std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1), std::placeholders::_1, std::placeholders::_2),
std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1) std::bind(&GripperControlActionServer::cancel_callback, this,
); std::placeholders::_1),
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>("/gripper_controller/commands", 10); std::bind(&GripperControlActionServer::accepted_callback, this,
join_state_subscriber = this->create_subscription<sensor_msgs::msg::JointState>( std::placeholders::_1));
"/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1)); publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
} "/gripper_controller/commands", 10);
join_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&GripperControlActionServer::joint_state_callback, this,
std::placeholders::_1));
}
private:
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
join_state_subscriber;
double gripper_joint_state{1.0};
private: using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr join_state_subscriber;
double gripper_joint_state{1.0};
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>; 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) rclcpp_action::GoalResponse
{ goal_callback(const rclcpp_action::GoalUUID &uuid,
gripper_joint_state = msg.position.back(); std::shared_ptr<const GripperCommand::Goal> goal) {
}
rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal) { RCLCPP_INFO(this->get_logger(),
"goal request recieved for gripper [%.2f m]", goal->position);
RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position); (void)uuid;
(void)uuid; if (goal->position > 0.9 or goal->position < 0) {
if(goal->position > 0.9 or goal->position < 0) { return rclcpp_action::GoalResponse::REJECT;
return rclcpp_action::GoalResponse::REJECT; }
} return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) { rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle; (void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) { void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach(); std::thread{std::bind(&GripperControlActionServer::execute, this,
} std::placeholders::_1),
goal_handle}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle){ .detach();
}
const auto goal = goal_handle->get_goal(); void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
auto result = std::make_shared<GripperCommand::Result>();
auto feedback = std::make_shared<GripperCommand::Feedback>();
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);
std_msgs::msg::Float64MultiArray command; const auto goal = goal_handle->get_goal();
auto result = std::make_shared<GripperCommand::Result>();
auto feedback = std::make_shared<GripperCommand::Feedback>();
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); std_msgs::msg::Float64MultiArray command;
RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper");
publisher->publish(command);
std::this_thread::sleep_for(3s);
using namespace std::chrono_literals;
goal_handle->succeed(result); command.data.push_back(goal->position);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); 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); RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
// int main(int argc, char ** argv) { // int main(int argc, char ** argv) {
// rclcpp::init(argc, argv); // rclcpp::init(argc, argv);
// rbs_skill_actions::GripperControlActionServer server; // rbs_skill_actions::GripperControlActionServer server;
// rclcpp::spin(server); // rclcpp::spin(server);
// return 0; // return 0;
// } // }

266
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -7,9 +7,9 @@
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
// action libs // 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/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/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/quaternion.hpp"
@ -26,156 +26,156 @@
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
*/ */
// For Visualization // For Visualization
//#include <eigen_conversions/eigen_msg.h> // #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_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: 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)
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node)
{ : Node("move_cartesian_action_server"), node_(node) {
// using namespace std::placeholders; // using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>( // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(), // this->get_node_base_interface(),
// this->get_node_clock_interface(), // this->get_node_clock_interface(),
// this->get_node_logging_interface(), // this->get_node_logging_interface(),
// this->get_node_waitables_interface(), // this->get_node_waitables_interface(),
// "move_topose", // "move_topose",
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
} }
void init() void init() {
{
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
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)
);
} action_server_ = rclcpp_action::create_server<MoveitSendPose>(
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: private:
rclcpp::Node::SharedPtr node_; rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_; rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>; using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse goal_callback( rclcpp_action::GoalResponse
const rclcpp_action::GoalUUID & uuid, goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal) std::shared_ptr<const MoveitSendPose::Goal> goal) {
{ RCLCPP_INFO(
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", this->get_logger(),
goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, "%f, %f]",
goal->target_pose.orientation.w); goal->robot_name.c_str(), goal->target_pose.position.x,
(void)uuid; 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<ServerGoalHandle> 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<ServerGoalHandle> 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<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
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<geometry_msgs::msg::Pose> 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<ServerGoalHandle> goal_handle) if (goal_handle->is_canceling()) {
{ goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Received cancel request"); RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
(void)goal_handle; return;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> 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<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
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<geometry_msgs::msg::Pose> 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;
}
} }
}
}; // class MoveCartesianActionServer }; // class MoveCartesianActionServer
}// namespace rbs_skill_actions } // namespace rbs_skill_actions
int main(int argc, char ** argv) int main(int argc, char **argv) {
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv); rclcpp::NodeOptions node_options;
rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true);
node_options.automatically_declare_parameters_from_overrides(true); auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
rbs_skill_actions::MoveCartesianActionServer server(node); rbs_skill_actions::MoveCartesianActionServer server(node);
std::thread run_server([&server]() { std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3)); rclcpp::sleep_for(std::chrono::seconds(3));
server.init(); server.init();
}); });
rclcpp::spin(node); rclcpp::spin(node);
run_server.join(); run_server.join();
return 0; return 0;
} }

233
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -1,16 +1,15 @@
#include <cinttypes>
#include <functional> #include <functional>
#include <memory> #include <memory>
#include <thread> #include <thread>
#include <cinttypes>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp" #include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
// action libs // action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "rbs_skill_interfaces/action/moveit_send_joint_states.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/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/quaternion.hpp"
@ -26,143 +25,139 @@
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
*/ */
// For Visualization // For Visualization
//#include <eigen_conversions/eigen_msg.h> // #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_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: public:
using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates; using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node) explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node)
{ : Node("move_to_joint_states_action_server"), node_(node) {
// using namespace std::placeholders; // using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>( // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(), // this->get_node_base_interface(),
// this->get_node_clock_interface(), // this->get_node_clock_interface(),
// this->get_node_logging_interface(), // this->get_node_logging_interface(),
// this->get_node_waitables_interface(), // this->get_node_waitables_interface(),
// "move_topose", // "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
} }
void init() void init() {
{
action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
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)
);
} action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
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: private:
rclcpp::Node::SharedPtr node_; rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_; rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendJointStates>; using ServerGoalHandle =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse goal_callback( rclcpp_action::GoalResponse
const rclcpp_action::GoalUUID & uuid, goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal) std::shared_ptr<const MoveitSendJointStates::Goal> goal) {
{ RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]",
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str()); goal->robot_name.c_str());
(void)uuid; (void)uuid;
if (false) { if (false) {
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
} }
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) }
{
RCLCPP_INFO(this->get_logger(), "Received cancel request"); rclcpp_action::CancelResponse
(void)goal_handle; cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
return rclcpp_action::CancelResponse::ACCEPT; RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1),
goal_handle)
.detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
std::vector<double> 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<ServerGoalHandle> goal_handle) move_group_interface.setJointValueTarget(goal->joint_values);
{ move_group_interface.setMaxVelocityScalingFactor(
using namespace std::placeholders; goal->joints_velocity_scaling_factor);
std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach(); 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<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); if (goal_handle->is_canceling()) {
move_group_interface.startStateMonitor(); goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
std::vector<double> joint_states = goal->joint_values; return;
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");
} }
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToJointStateActionServer }; // class MoveToJointStateActionServer
}// namespace rbs_skill_actions } // namespace rbs_skill_actions
int main(int argc, char ** argv) int main(int argc, char **argv) {
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv); rclcpp::NodeOptions node_options;
rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true);
node_options.automatically_declare_parameters_from_overrides(true); auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node); rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() { std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3)); rclcpp::sleep_for(std::chrono::seconds(3));
server.init(); server.init();
}); });
rclcpp::spin(node); rclcpp::spin(node);
run_server.join(); run_server.join();
return 0; return 0;
} }

249
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -7,9 +7,9 @@
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
// action libs // 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/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/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/quaternion.hpp"
@ -18,8 +18,8 @@
// moveit libs // moveit libs
#include "moveit/move_group_interface/move_group_interface.h" #include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_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/planning_scene_interface/planning_scene_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
/* /*
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
@ -27,148 +27,149 @@
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
*/ */
// For Visualization // For Visualization
//#include <eigen_conversions/eigen_msg.h> // #include <eigen_conversions/eigen_msg.h>
#include <moveit_msgs/msg/display_robot_state.hpp> #include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp" #include <moveit_msgs/msg/display_robot_state.hpp>
namespace rbs_skill_actions namespace rbs_skill_actions {
{
class MoveToPoseActionServer : public rclcpp::Node class MoveToPoseActionServer : public rclcpp::Node {
{
public: 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)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node) explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node)
{ : Node("move_topose_action_server"), node_(node) {
// using namespace std::placeholders; // using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>( // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(), // this->get_node_base_interface(),
// this->get_node_clock_interface(), // this->get_node_clock_interface(),
// this->get_node_logging_interface(), // this->get_node_logging_interface(),
// this->get_node_waitables_interface(), // this->get_node_waitables_interface(),
// "move_topose", // "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
} }
void init() void init() {
{
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
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)
);
} action_server_ = rclcpp_action::create_server<MoveitSendPose>(
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: private:
rclcpp::Node::SharedPtr node_; rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_; rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>; using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse goal_callback( rclcpp_action::GoalResponse
const rclcpp_action::GoalUUID & uuid, goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal) std::shared_ptr<const MoveitSendPose::Goal> goal) {
{ RCLCPP_INFO(
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", this->get_logger(),
goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, "%f, %f]",
goal->target_pose.orientation.w); goal->robot_name.c_str(), goal->target_pose.position.x,
(void)uuid; goal->target_pose.position.y, goal->target_pose.position.z,
if (false) { goal->target_pose.orientation.x, goal->target_pose.orientation.y,
return rclcpp_action::GoalResponse::REJECT; goal->target_pose.orientation.z, goal->target_pose.orientation.w);
} (void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; if (false) {
return rclcpp_action::GoalResponse::REJECT;
} }
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> 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<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
move_group_interface.setPoseTarget(goal->target_pose); rclcpp_action::CancelResponse
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration); RCLCPP_INFO(this->get_logger(), "Received cancel request");
moveit::planning_interface::MoveGroupInterface::Plan plan; (void)goal_handle;
moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan); return rclcpp_action::CancelResponse::ACCEPT;
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{ void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code)); using namespace std::placeholders;
goal_handle->abort(result); std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1),
} goal_handle)
.detach();
}
if (goal_handle->is_canceling()) { void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Executing action goal");
RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); const auto goal = goal_handle->get_goal();
return; auto result = std::make_shared<MoveitSendPose::Result>();
}
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 }; // class MoveToPoseActionServer
}// namespace rbs_skill_actions } // namespace rbs_skill_actions
int main(int argc, char ** argv) int main(int argc, char **argv) {
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv); rclcpp::NodeOptions node_options;
rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true);
node_options.automatically_declare_parameters_from_overrides(true); auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
rbs_skill_actions::MoveToPoseActionServer server(node); rbs_skill_actions::MoveToPoseActionServer server(node);
std::thread run_server([&server]() { std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3)); rclcpp::sleep_for(std::chrono::seconds(3));
server.init(); server.init();
}); });
rclcpp::spin(node); rclcpp::spin(node);
run_server.join(); run_server.join();
return 0; return 0;
} }

325
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

@ -1,20 +1,20 @@
#include <filesystem>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
#include <filesystem>
#include <tinyxml2.h>
#include <geometric_shapes/mesh_operations.h> #include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h> #include <geometric_shapes/shape_operations.h>
#include <sdf/sdf.hh>
#include <sdf/World.hh> #include <sdf/World.hh>
#include <sdf/sdf.hh>
#include <tinyxml2.h>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/collision_object.hpp>
#include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp" #include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp"
@ -22,8 +22,8 @@
#define INIT_SCENE_PARAM_NAME "init_scene" #define INIT_SCENE_PARAM_NAME "init_scene"
#define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths" #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; geometry_msgs::msg::Pose result;
auto position = pose.Pos(); auto position = pose.Pos();
result.position.set__x(position.X()); 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; return result;
} }
static std::string get_correct_mesh_path( static std::string
const std::string& uri, const std::vector<std::string> &resources) get_correct_mesh_path(const std::string &uri,
{ const std::vector<std::string> &resources) {
std::string result = ""; std::string result = "";
std::regex reg(R"((?:model|package)(?:\:\/)(.*))"); std::regex reg(R"((?:model|package)(?:\:\/)(.*))");
std::smatch m; std::smatch m;
if (std::regex_match(uri, m, reg)) if (std::regex_match(uri, m, reg)) {
{
std::string rel_path = m[1]; std::string rel_path = m[1];
std::for_each(resources.begin(), resources.end(), std::for_each(resources.begin(), resources.end(),
[&result, &rel_path](const std::string& res){ [&result, &rel_path](const std::string &res) {
if (result.empty()) if (result.empty())
result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result; result = std::filesystem::exists(res + rel_path)
}); ? std::string(res + rel_path)
: result;
});
} }
return "file://" + result; return "file://" + result;
} }
static moveit_msgs::msg::CollisionObject static moveit_msgs::msg::CollisionObject
get_object(const sdf::Model *model, const std::vector<std::string> &resources) get_object(const sdf::Model *model, const std::vector<std::string> &resources) {
{
moveit_msgs::msg::CollisionObject obj; moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = "world"; obj.header.frame_id = "world";
obj.id = model->Name(); obj.id = model->Name();
obj.pose = pose_from_pose3d(model->RawPose()); obj.pose = pose_from_pose3d(model->RawPose());
size_t link_count = model->LinkCount(); 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 link = model->LinkByIndex(i);
auto collision = link->CollisionByIndex(0); auto collision = link->CollisionByIndex(0);
auto link_pose = pose_from_pose3d(link->RawPose()); auto link_pose = pose_from_pose3d(link->RawPose());
auto geometry = collision->Geom(); auto geometry = collision->Geom();
switch(geometry->Type()) switch (geometry->Type()) {
{ case sdf::GeometryType::MESH: {
case sdf::GeometryType::MESH: auto path =
{ get_correct_mesh_path(geometry->MeshShape()->Uri(), resources);
auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); shapes::Mesh *m = shapes::createMeshFromResource(
shapes::Mesh *m = shapes::createMeshFromResource( !path.empty() ? path : geometry->MeshShape()->Uri());
!path.empty()? path: geometry->MeshShape()->Uri()); auto scale = geometry->MeshShape()->Scale().X();
auto scale = geometry->MeshShape()->Scale().X(); m->scale(scale);
m->scale(scale);
Eigen::Vector3d centroid = {0, 0, 0}; Eigen::Vector3d centroid = {0, 0, 0};
for (size_t i = 0; i < 3 * m->vertex_count; i += 3) for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
{ const auto x = m->vertices[i];
const auto x = m->vertices[i]; const auto y = m->vertices[i + 1];
const auto y = m->vertices[i+1]; const auto z = m->vertices[i + 2];
const auto z = m->vertices[i+2];
centroid.x() += x*(1-scale); centroid.x() += x * (1 - scale);
centroid.y() += y*(1-scale); centroid.y() += y * (1 - scale);
centroid.z() += z*(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<shape_msgs::msg::Mesh>(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:
{ centroid = centroid / m->vertex_count;
auto sdf_plane = geometry->PlaneShape(); for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
shape_msgs::msg::Plane plane; m->vertices[i] -= centroid.x();
auto normal = sdf_plane->Normal(); m->vertices[i + 1] -= centroid.y();
plane.coef[0] = normal.X(); m->vertices[i + 2] -= centroid.z();
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: shape_msgs::msg::Mesh mesh;
case sdf::GeometryType::SPHERE: shapes::ShapeMsg m_msg;
case sdf::GeometryType::HEIGHTMAP: shapes::constructMsgFromShape(m, m_msg);
case sdf::GeometryType::CAPSULE: mesh = boost::get<shape_msgs::msg::Mesh>(m_msg);
case sdf::GeometryType::ELLIPSOID: obj.meshes.push_back(mesh);
case sdf::GeometryType::POLYLINE: obj.mesh_poses.push_back(link_pose);
break; 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; return obj;
} }
static std::vector<moveit_msgs::msg::CollisionObject> static std::vector<moveit_msgs::msg::CollisionObject>
get_objects(const sdf::World *world, const std::string &model_resources) get_objects(const sdf::World *world, const std::string &model_resources) {
{
std::vector<std::string> resources; std::vector<std::string> resources;
std::regex reg("\\:+"); std::regex reg("\\:+");
std::sregex_token_iterator begin( std::sregex_token_iterator begin(model_resources.begin(),
model_resources.begin(), model_resources.end(), reg, -1), end; model_resources.end(), reg, -1),
end;
std::copy(++begin, end, std::back_inserter(resources)); std::copy(++begin, end, std::back_inserter(resources));
std::vector<moveit_msgs::msg::CollisionObject> result; std::vector<moveit_msgs::msg::CollisionObject> result;
auto models_count = world->ModelCount(); auto models_count = world->ModelCount();
for (size_t i = 0; i < models_count; ++i) for (size_t i = 0; i < models_count; ++i) {
{ try {
try{
auto model = world->ModelByIndex(i); auto model = world->ModelByIndex(i);
result.push_back(get_object(model, resources)); result.push_back(get_object(model, resources));
} catch (std::exception &ex){ } catch (std::exception &ex) {
std::cerr << ex.what() << std::endl; std::cerr << ex.what() << std::endl;
} }
} }
@ -167,81 +163,80 @@ get_objects(const sdf::World *world, const std::string &model_resources)
return result; return result;
} }
class UpdatePlanningSceneServer: public rclcpp::Node class UpdatePlanningSceneServer : public rclcpp::Node {
{
public: public:
UpdatePlanningSceneServer(rclcpp::NodeOptions options) UpdatePlanningSceneServer(rclcpp::NodeOptions options)
: Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) : Node("update_planning_scene_node",
{ options.allow_undeclared_parameters(true)
std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); .automatically_declare_parameters_from_overrides(true)) {
std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); 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()) if (!scene.empty()) {
{ RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str());
RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); init_scene(scene, model_resources);
init_scene(scene, model_resources); }
} service_ =
service_ = create_service<rbs_skill_interfaces::srv::PlanningSceneObjectState>( create_service<rbs_skill_interfaces::srv::PlanningSceneObjectState>(
SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this, SERVICE_NAME,
std::placeholders::_1, std::placeholders::_2)); 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<UpdatePlanningSceneRequestState>(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) response->call_status = planning_scene_.applyCollisionObject(obj);
{ }
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<UpdatePlanningSceneRequestState>(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);
}
private: private:
enum UpdatePlanningSceneRequestState enum UpdatePlanningSceneRequestState { ADD = 0, REMOVE = 1, UPDATE = 2 };
{
ADD=0,
REMOVE=1,
UPDATE=2
};
rclcpp::Service<rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_; rclcpp::Service<
moveit::planning_interface::PlanningSceneInterface planning_scene_; rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_;
moveit::planning_interface::PlanningSceneInterface planning_scene_;
}; };
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"

188
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -1,4 +1,4 @@
//#include "rbs_skill_servers" // #include "rbs_skill_servers"
#include "rbs_skill_servers/pick_place_pose_loader.hpp" #include "rbs_skill_servers/pick_place_pose_loader.hpp"
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer; using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
@ -6,105 +6,107 @@ using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
using namespace std::chrono_literals; using namespace std::chrono_literals;
// rclcpp::Node::SharedPtr g_node = nullptr; // rclcpp::Node::SharedPtr g_node = nullptr;
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options) GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) : Node("grasp_place_pose_loader",
{ options.allow_undeclared_parameters(true)
tf_buffer_ = .automatically_declare_parameters_from_overrides(true)) {
std::make_unique<tf2_ros::Buffer>(this->get_clock()); tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service", srv_ = create_service<GetGraspPlacePoseService>(
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2)); "/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this,
std::placeholders::_1, std::placeholders::_2));
} }
void void GetGraspPlacePoseServer::handle_server(
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response) request,
{ rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name response) {
try { std::string rrr =
tf_data = tf_buffer_->lookupTransform( "ASSEMBLE_" + request->object_name; // TODO: replace with better name
"world", rrr.c_str(), try {
tf2::TimePointZero); tf_data =
} catch (const tf2::TransformException & ex) { tf_buffer_->lookupTransform("world", rrr.c_str(), tf2::TimePointZero);
RCLCPP_ERROR( } catch (const tf2::TransformException &ex) {
this->get_logger(), "Could not transform %s to %s: %s", RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
rrr.c_str(), "world", ex.what()); rrr.c_str(), "world", ex.what());
return; return;
} }
// TODO: Here need to check the parameter // TODO: Here need to check the parameter
// We can use another parameter library from PickNik // We can use another parameter library from PickNik
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array(); grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", .as_double_array();
request->grasp_direction.x, RCLCPP_INFO(this->get_logger(),
request->grasp_direction.y, "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.z); request->grasp_direction.x, request->grasp_direction.y,
RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", request->grasp_direction.z);
request->place_direction.x, RCLCPP_INFO(this->get_logger(),
request->place_direction.y, "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->place_direction.z); request->place_direction.x, request->place_direction.y,
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); request->place_direction.z);
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl; Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl; // std::cout << "grasp_point = " << std::endl << grasp_pose.translation() <<
Eigen::Vector3d vec_grasp(0.15,0.15,0.02); // std::endl << grasp_pose.linear() << std::endl; std::cout << "place_pose = "
Eigen::Vector3d vec_place(0,0,0.15); // << std::endl << place_pose.translation() << std::endl <<
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp); // place_pose.linear() << std::endl;
response->place = collect_pose(place_pose, request->place_direction, vec_place); 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<geometry_msgs::msg::Pose> std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
GetGraspPlacePoseServer::collect_pose( const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction,
const Eigen::Affine3d pose, const Eigen::Vector3d scale_vec) {
const geometry_msgs::msg::Vector3 direction, std::vector<geometry_msgs::msg::Pose> pose_v_;
const Eigen::Vector3d scale_vec) pose_v_.push_back(tf2::toMsg(pose));
{ Eigen::Vector3d posedir;
std::vector<geometry_msgs::msg::Pose> pose_v_; Eigen::Affine3d pose_ = pose;
pose_v_.push_back(tf2::toMsg(pose)); tf2::fromMsg(direction, posedir);
Eigen::Vector3d posedir; Eigen::Matrix3d Ixy =
Eigen::Affine3d pose_ = pose; Eigen::Matrix3d::Zero(); // posedir * posedir.transpose();
tf2::fromMsg(direction, posedir); Ixy.diagonal() = posedir;
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose(); Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
Ixy.diagonal() = posedir; Iz.diagonal() = Eigen::Vector3d::UnitZ();
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
Iz.diagonal() = Eigen::Vector3d::UnitZ();
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0] if (posedir.cwiseEqual(Eigen::Vector3d::UnitX())
{ .all()) // IF Direction == [1,0,0]
std::cout << "\n TBD" << std::endl; {
} std::cout << "\n TBD" << std::endl;
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0] } else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY())
{ .all()) // IF Direction == [0,1,0]
// Gp -- grasp point frame {
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y // Gp -- grasp point frame
pose_v_.push_back(tf2::toMsg(pose_)); pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
pose_.pretranslate(Iz * scale_vec); // Gp-y + z pose_v_.push_back(tf2::toMsg(pose_));
pose_v_.push_back(tf2::toMsg(pose_)); pose_.pretranslate(Iz * scale_vec); // Gp-y + z
pose_.pretranslate(Ixy * scale_vec); // Gp+z pose_v_.push_back(tf2::toMsg(pose_));
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] } 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 pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
} pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
else } else {
{ std::cout << "\n TBD" << std::endl;
std::cout << "\n TBD" << std::endl; }
} return pose_v_;
return pose_v_;
} }
Eigen::Affine3d Eigen::Affine3d
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose) GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose) {
{ Eigen::Vector3d point(
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data()); std::vector<double>(pose.begin(), pose.begin() + 3).data());
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data()); Eigen::Quaterniond quat(
Eigen::Affine3d aff; std::vector<double>(pose.begin() + 3, pose.end()).data());
aff.translation() = point; Eigen::Affine3d aff;
aff.linear() = quat.toRotationMatrix(); aff.translation() = point;
return aff; aff.linear() = quat.toRotationMatrix();
return aff;
} }