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
{
class EnvInterface : public EnvInterfaceBase
{
public:
EnvInterface() = default;
virtual ~EnvInterface() = default;
};
} // namespace env_interface
class EnvInterface : public EnvInterfaceBase
{
public:
EnvInterface() = default;
virtual ~EnvInterface() = default;
};
} // namespace env_interface

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

@ -1,38 +1,40 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include <cstddef>
#include <cstdint>
namespace env_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// template <typename A_space, typename S_space>
class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
EnvInterfaceBase() = default;
EnvInterfaceBase() = default;
virtual ~EnvInterfaceBase() = default;
virtual ~EnvInterfaceBase() = default;
virtual CallbackReturn init(
const std::string& t_env_name,
const std::string & t_namespace = "",
const rclcpp::NodeOptions & t_node_options = rclcpp::NodeOptions()
);
virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "",
const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions());
const rclcpp_lifecycle::State& configure();
const rclcpp_lifecycle::State& configure();
virtual CallbackReturn on_init() = 0;
virtual CallbackReturn on_init() = 0;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode();
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode() const;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode();
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:
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
{
CallbackReturn
EnvInterfaceBase::init(
const std::string& t_env_name,
const std::string & t_namespace,
const rclcpp::NodeOptions & t_node_options)
CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace,
const rclcpp::NodeOptions& t_node_options)
{
m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
t_env_name, t_namespace, t_node_options, false);
m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(t_env_name, t_namespace, t_node_options, false);
if (on_init() == CallbackReturn::FAILURE)
{
return CallbackReturn::FAILURE;
}
if (on_init() == CallbackReturn::FAILURE)
{
return CallbackReturn::FAILURE;
}
m_node->register_on_configure(
std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1)
);
m_node->register_on_activate(
std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1)
);
m_node->register_on_cleanup(
std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1)
);
m_node->register_on_deactivate(
std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1)
);
m_node->register_on_error(
std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1)
);
m_node->register_on_shutdown(
std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1)
);
m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1));
m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1));
m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1));
m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1));
m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1));
m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1));
return CallbackReturn::SUCCESS;
return CallbackReturn::SUCCESS;
}
const rclcpp_lifecycle::State&
EnvInterfaceBase::configure()
const rclcpp_lifecycle::State& EnvInterfaceBase::configure()
{
return getNode()->configure();
return getNode()->configure();
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
EnvInterfaceBase::getNode()
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> EnvInterfaceBase::getNode()
{
if (!m_node.get())
{
throw std::runtime_error("Lifecycle node hasn't been initialized yet");
}
return m_node;
if (!m_node.get())
{
throw std::runtime_error("Lifecycle node hasn't been initialized yet");
}
return m_node;
}
const rclcpp_lifecycle::State & EnvInterfaceBase::getState() const
const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const
{
return m_node->get_current_state();
return m_node->get_current_state();
}
} // namespace env_interface
} // namespace env_interface

111
env_manager/env_manager/include/env_manager/env_manager.hpp
View file

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

261
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,22 +1,22 @@
#include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp"
static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "env_interface";
static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "env_interface::EnvInterface";
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
"env_interface";
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME =
"env_interface::EnvInterface";
namespace env_manager
{
EnvManager::EnvManager(
rclcpp::Executor::SharedPtr executor,
const std::string & node_name,
const std::string & node_namespace,
rclcpp::NodeOptions & options)
: rclcpp::Node(node_name, node_namespace, options),
m_executor(executor),
m_loader(std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME))
{
initServices();
namespace env_manager {
EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
const std::string &node_name,
const std::string &node_namespace,
rclcpp::NodeOptions &options)
: rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
m_loader(
std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
ENV_INTERFACE_BASE_CLASS_NAMESPACE,
ENV_INTERFACE_BASE_CLASS_NAME)) {
initServices();
}
// EnvManager::EnvManager(
@ -33,130 +33,111 @@ EnvManager::EnvManager(
// initServices();
// }
void EnvManager::initServices() {
using namespace std::placeholders;
void
EnvManager::initServices()
{
using namespace std::placeholders;
m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv>
("~/load_env", std::bind(&EnvManager::loadEnv_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));
m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv>(
"~/load_env", std::bind(&EnvManager::loadEnv_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
EnvManager::getNode()
{
return m_node;
}
rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; }
void
EnvManager::loadEnv_cb(
void EnvManager::loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response)
{
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = loadEnv(request->name, request->type).get() != nullptr;
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = loadEnv(request->name, request->type).get() != nullptr;
}
void
EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response)
{
std::lock_guard<std::mutex> guard(m_env_mutex);
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
void EnvManager::configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> guard(m_env_mutex);
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
}
void
EnvManager::unloadEnv_cb(
void EnvManager::unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response)
{
std::lock_guard<std::mutex> lock(m_env_mutex);
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
std::lock_guard<std::mutex> lock(m_env_mutex);
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
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;
}
m_active_envs.push_back(enviroment);
m_executor->add_node(enviroment.env_ptr->getNode()->get_node_base_interface());
return enviroment.env_ptr;
m_active_envs.push_back(enviroment);
m_executor->add_node(
enviroment.env_ptr->getNode()->get_node_base_interface());
return enviroment.env_ptr;
}
EnvInterfaceSharedPtr
EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type)
{
EnvInterfaceSharedPtr loaded_env = nullptr;
RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str());
try
{
if (m_loader->isClassAvailable(env_class_type))
{
loaded_env = m_loader->createSharedInstance(env_class_type);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s' with class type '%s'",
env_name.c_str(), env_class_type.c_str());
RCLCPP_INFO(this->get_logger(), "Available classes");
for (const auto& available_class : m_loader->getDeclaredClasses())
{
RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str());
}
return nullptr;
}
}
catch (const pluginlib::PluginlibException& e)
{
RCLCPP_ERROR(
get_logger(), "Error happened during creation of enviroment '%s' with type '%s':\n%s",
env_name.c_str(), env_class_type.c_str(), e.what());
return nullptr;
}
EnvSpec enviroment;
enviroment.name = env_name;
enviroment.type = env_class_type;
enviroment.env_ptr = loaded_env;
return addEnv(enviroment);
}
EnvStateReturnType
EnvManager::unloadEnv(const std::string& env_name)
{
auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) {
return env.name == env_name;
});
if (it != m_active_envs.end()) {
m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
m_active_envs.erase(it);
return EnvStateReturnType::SUCCESS;
EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name,
const std::string &env_class_type) {
EnvInterfaceSharedPtr loaded_env = nullptr;
RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str());
try {
if (m_loader->isClassAvailable(env_class_type)) {
loaded_env = m_loader->createSharedInstance(env_class_type);
} else {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::ERROR;
RCLCPP_ERROR(
this->get_logger(),
"Loading enviroment is not available '%s' with class type '%s'",
env_name.c_str(), env_class_type.c_str());
RCLCPP_INFO(this->get_logger(), "Available classes");
for (const auto &available_class : m_loader->getDeclaredClasses()) {
RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str());
}
return nullptr;
}
return EnvStateReturnType::FAILURE;
} catch (const pluginlib::PluginlibException &e) {
RCLCPP_ERROR(
get_logger(),
"Error happened during creation of enviroment '%s' with type '%s':\n%s",
env_name.c_str(), env_class_type.c_str(), e.what());
return nullptr;
}
EnvSpec enviroment;
enviroment.name = env_name;
enviroment.type = env_class_type;
enviroment.env_ptr = loaded_env;
return addEnv(enviroment);
}
EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
[&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it != m_active_envs.end()) {
m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
m_active_envs.erase(it);
return EnvStateReturnType::SUCCESS;
} else {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::ERROR;
}
return EnvStateReturnType::FAILURE;
}
// EnvInterfaceSharedPtr
@ -167,27 +148,25 @@ EnvManager::unloadEnv(const std::string& env_name)
// return loadEnv(env_name, env_class_type);
// }
EnvStateReturnType
EnvManager::configureEnv(const std::string& env_name)
{
std::lock_guard<std::mutex> guard(m_env_mutex);
EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
std::lock_guard<std::mutex> guard(m_env_mutex);
auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) {
return env.name == env_name;
});
auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
[&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it == m_active_envs.end()) {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::FAILURE;
}
it->env_ptr->configure();
// it->env_ptr->getNode()->configure();
// it->env_ptr->activate();
it->env_ptr->getNode()->activate();
if (it == m_active_envs.end()) {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::FAILURE;
}
it->env_ptr->configure();
// it->env_ptr->getNode()->configure();
// it->env_ptr->activate();
it->env_ptr->getNode()->activate();
return EnvStateReturnType::SUCCESS;
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 "env_manager/env_manager.hpp"
int main(int argc, char ** argv)
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
std::string manager_node_name = "env_manager";
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
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->configureEnv("gz_enviroment");
em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
em->configureEnv("gz_enviroment");
executor->add_node(em);
executor->spin();
rclcpp::shutdown();
return 0;
executor->add_node(em);
executor->spin();
rclcpp::shutdown();
return 0;
}

1
env_manager/gz_enviroment/CMakeLists.txt
View file

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

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

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

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

318
rbs_skill_servers/src/assemble_state_server.cpp
View file

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

153
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

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

262
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

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

229
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -1,16 +1,15 @@
#include <cinttypes>
#include <functional>
#include <memory>
#include <thread>
#include <cinttypes>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
@ -26,143 +25,139 @@
#include <tf2_ros/transform_listener.h>
*/
// 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_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
namespace rbs_skill_actions
{
namespace rbs_skill_actions {
class MoveToJointStateActionServer : public rclcpp::Node
{
class MoveToJointStateActionServer : public rclcpp::Node {
public:
using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates;
using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node)
{
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
// explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_to_joint_states_action_server"), node_(node) {
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
void init()
{
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:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
using ServerGoalHandle =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str());
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal) {
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]",
goal->robot_name.c_str());
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::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(&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);
}
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;
move_group_interface.setJointValueTarget(goal->joint_values);
move_group_interface.setMaxVelocityScalingFactor(
goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(
goal->joints_acceleration_scaling_factor);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) ==
moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach();
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
void execute(const std::shared_ptr<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);
}
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
}// namespace rbs_skill_actions
} // namespace rbs_skill_actions
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
rclcpp::spin(node);
run_server.join();
return 0;
return 0;
}

251
rbs_skill_servers/src/move_topose_action_server.cpp
View file

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

317
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

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

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