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

from stash

Browse source
This commit is contained in:
Ilya Uraev 2023-12-05 22:37:28 +03:00
parent d72c06efea
commit 76cd4319eb
8 changed files with 828 additions and 839 deletions
Download patch file Download diff file Expand all files Collapse all files

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;
@ -40,32 +41,32 @@ void handle_service(
box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}();
}();
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
response->success = true;
response->success = true;
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<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;
}

324
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,23 +40,22 @@ static inline geometry_msgs::msg::Pose get_pose(
return p;
}
static inline geometry_msgs::msg::PoseStamped get_pose_stamped(
const std::string & pose_string)
{
static inline geometry_msgs::msg::PoseStamped
get_pose_stamped(const std::string &pose_string) {
std::stringstream ss(pose_string);
std::istream_iterator<std::string> begin(ss);
std::istream_iterator<std::string> end;
std::vector<std::string> tokens(begin, end);
geometry_msgs::msg::PoseStamped ps;
ps.set__pose(get_pose(tokens));
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,12 +64,11 @@ 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;
auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText());
auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText());
ps.header.set__frame_id(assemble_prefix + part_name);
result.push_back(ps);
joint = joint->NextSiblingElement("joint");
@ -80,164 +77,167 @@ static std::vector<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>(
SERVICE_NAME, std::bind(&AssembleStateServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
AssembleStateServer(rclcpp::NodeOptions options)
: Node(SERVICE_NAME,
options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) {
assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string();
assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string();
tf_buffer_ = std::make_unique<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));
}
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;
assemble->state = COMPLETE;
return true;
}
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:
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr join_state_subscriber;
double gripper_joint_state{1.0};
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
void joint_state_callback(const sensor_msgs::msg::JointState &msg) {
gripper_joint_state = msg.position.back();
}
void joint_state_callback(const sensor_msgs::msg::JointState & msg)
{
gripper_joint_state = msg.position.back();
}
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const GripperCommand::Goal> goal) {
rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal) {
RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position);
(void)uuid;
if(goal->position > 0.9 or goal->position < 0) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
RCLCPP_INFO(this->get_logger(),
"goal request recieved for gripper [%.2f m]", goal->position);
(void)uuid;
if (goal->position > 0.9 or goal->position < 0) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
}
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>();
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state);
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
std_msgs::msg::Float64MultiArray command;
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<GripperCommand::Result>();
auto feedback = std::make_shared<GripperCommand::Feedback>();
using namespace std::chrono_literals;
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(), "Current gripper state %f",
gripper_joint_state);
command.data.push_back(goal->position);
RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper");
publisher->publish(command);
std::this_thread::sleep_for(3s);
std_msgs::msg::Float64MultiArray command;
using namespace std::chrono_literals;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
}
};
}
command.data.push_back(goal->position);
RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper");
publisher->publish(command);
std::this_thread::sleep_for(3s);
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
}
};
} // namespace rbs_skill_actions
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
// int main(int argc, char ** argv) {
// rclcpp::init(argc, argv);
// rbs_skill_actions::GripperControlActionServer server;

264
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()
{
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)
);
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));
}
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;
}

231
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()
{
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)
);
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));
}
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;
}
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;
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);
}
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();
move_group_interface.setJointValueTarget(goal->joint_values);
move_group_interface.setMaxVelocityScalingFactor(
goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(
goal->joints_acceleration_scaling_factor);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) ==
moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
void execute(const std::shared_ptr<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");
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToJointStateActionServer
}// namespace rbs_skill_actions
} // namespace rbs_skill_actions
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
rclcpp::spin(node);
run_server.join();
return 0;
return 0;
}

247
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()
{
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)
);
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));
}
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;
}
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();
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan);
if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){
move_group_interface.plan(plan);
}
if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
//move_group_interface.execute(plan);
moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE)
{
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
}else{
RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code));
goal_handle->abort(result);
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1),
goal_handle)
.detach();
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
void execute(const std::shared_ptr<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;
}
}
}; // 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;
}

325
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,129 +37,125 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p
return result;
}
static std::string get_correct_mesh_path(
const std::string& uri, const std::vector<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;
});
std::for_each(resources.begin(), resources.end(),
[&result, &rel_path](const std::string &res) {
if (result.empty())
result = std::filesystem::exists(res + rel_path)
? std::string(res + rel_path)
: result;
});
}
return "file://" + result;
return "file://" + result;
}
static moveit_msgs::msg::CollisionObject
get_object(const sdf::Model *model, const std::vector<std::string> &resources)
{
static moveit_msgs::msg::CollisionObject
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;
}
}
return obj;
}
static std::vector<moveit_msgs::msg::CollisionObject>
get_objects(const sdf::World *world, const std::string &model_resources)
{
static std::vector<moveit_msgs::msg::CollisionObject>
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"

66
rbs_task_planner/src/task_planner_controller.cpp
View file

@ -13,47 +13,39 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
class TaskPlannerController : public rclcpp::Node
{
public:
TaskPlannerController() : rclcpp::Node("task_planner_controller")
{
domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
run_plan();
class TaskPlannerController : public rclcpp::Node {
public:
TaskPlannerController() : rclcpp::Node("task_planner_controller") {
domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
run_plan();
}
void run_plan() {
auto domain = domain_expert_->getDomain();
auto problem = problem_expert_->getProblem();
auto plan = planner_client_->getPlan(domain, problem);
if (!plan.has_value()) {
RCLCPP_ERROR(this->get_logger(), "Could not find plan to reach goal %s",
parser::pddl::toString(problem_expert_->getGoal()).c_str());
} else {
if (executor_client_->start_plan_execution(plan.value())) {
RCLCPP_INFO(this->get_logger(), "Execute plan...");
}
}
}
void run_plan()
{
auto domain = domain_expert_->getDomain();
auto problem = problem_expert_->getProblem();
auto plan = planner_client_->getPlan(domain, problem);
if(!plan.has_value())
{
RCLCPP_ERROR(this->get_logger(), "Could not find plan to reach goal %s",
parser::pddl::toString(problem_expert_->getGoal()).c_str());
}
else
{
if (executor_client_->start_plan_execution(plan.value()))
{
RCLCPP_INFO(this->get_logger(), "Execute plan...");
}
}
}
private:
std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
std::shared_ptr<plansys2::PlannerClient> planner_client_;
std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
std::shared_ptr<plansys2::ExecutorClient> executor_client_;
private:
std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
std::shared_ptr<plansys2::PlannerClient> planner_client_;
std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
std::shared_ptr<plansys2::ExecutorClient> executor_client_;
};
int main(int argc, char * argv[])
{
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<TaskPlannerController>());
rclcpp::shutdown();