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

324
rbs_skill_servers/src/assemble_state_server.cpp
View file

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

153
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

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

264
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -7,9 +7,9 @@
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
// action libs // action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/quaternion.hpp"
@ -26,156 +26,156 @@
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
*/ */
// For Visualization // For Visualization
//#include <eigen_conversions/eigen_msg.h> // #include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/action/move_group.hpp"
#include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
namespace rbs_skill_actions namespace rbs_skill_actions {
{
class MoveCartesianActionServer : public rclcpp::Node class MoveCartesianActionServer : public rclcpp::Node {
{
public: public:
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
//explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) // explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node)
{ : Node("move_cartesian_action_server"), node_(node) {
// using namespace std::placeholders; // using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>( // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(), // this->get_node_base_interface(),
// this->get_node_clock_interface(), // this->get_node_clock_interface(),
// this->get_node_logging_interface(), // this->get_node_logging_interface(),
// this->get_node_waitables_interface(), // this->get_node_waitables_interface(),
// "move_topose", // "move_topose",
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
} }
void init() void init() {
{
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(),
node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(),
"move_cartesian",
std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1)
);
} action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(), node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "move_cartesian",
std::bind(&MoveCartesianActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveCartesianActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveCartesianActionServer::accepted_callback, this,
std::placeholders::_1));
}
private: private:
rclcpp::Node::SharedPtr node_; rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_; rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>; using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse goal_callback( rclcpp_action::GoalResponse
const rclcpp_action::GoalUUID & uuid, goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal) std::shared_ptr<const MoveitSendPose::Goal> goal) {
{ RCLCPP_INFO(
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", this->get_logger(),
goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, "%f, %f]",
goal->target_pose.orientation.w); goal->robot_name.c_str(), goal->target_pose.position.x,
(void)uuid; goal->target_pose.position.y, goal->target_pose.position.z,
goal->target_pose.orientation.x, goal->target_pose.orientation.y,
goal->target_pose.orientation.z, goal->target_pose.orientation.w);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1),
goal_handle)
.detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(
*move_group_interface.getCurrentState());
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
// waypoints.push_back(current_pose.pose);
geometry_msgs::msg::Pose target_pose = goal->target_pose;
// target_pose.position = goal->target_pose.position;
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
target_pose.position.x, target_pose.position.y,
target_pose.position.z);
// waypoints.push_back(start_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(
waypoints, eef_step, jump_threshold, trajectory);
if (fraction > 0) {
RCLCPP_INFO(this->get_logger(), "Planning success");
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(trajectory);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
goal_handle->abort(result);
} }
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) if (goal_handle->is_canceling()) {
{ goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Received cancel request"); RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
(void)goal_handle; return;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
//waypoints.push_back(current_pose.pose);
geometry_msgs::msg::Pose target_pose = goal->target_pose;
//target_pose.position = goal->target_pose.position;
waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
target_pose.position.x, target_pose.position.y, target_pose.position.z);
//waypoints.push_back(start_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
if(fraction>0)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE)
{
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
//move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
goal_handle->abort(result);
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
} }
}
}; // class MoveCartesianActionServer }; // class MoveCartesianActionServer
}// namespace rbs_skill_actions } // namespace rbs_skill_actions
int main(int argc, char ** argv) int main(int argc, char **argv) {
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv); rclcpp::NodeOptions node_options;
rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true);
node_options.automatically_declare_parameters_from_overrides(true); auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
rbs_skill_actions::MoveCartesianActionServer server(node); rbs_skill_actions::MoveCartesianActionServer server(node);
std::thread run_server([&server]() { std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3)); rclcpp::sleep_for(std::chrono::seconds(3));
server.init(); server.init();
}); });
rclcpp::spin(node); rclcpp::spin(node);
run_server.join(); run_server.join();
return 0; return 0;
} }

231
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

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

247
rbs_skill_servers/src/move_topose_action_server.cpp
View file

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

325
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

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

66
rbs_task_planner/src/task_planner_controller.cpp
View file

@ -13,47 +13,39 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_action/rclcpp_action.hpp"
class TaskPlannerController : public rclcpp::Node class TaskPlannerController : public rclcpp::Node {
{ public:
public: TaskPlannerController() : rclcpp::Node("task_planner_controller") {
TaskPlannerController() : rclcpp::Node("task_planner_controller") domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
{ planner_client_ = std::make_shared<plansys2::PlannerClient>();
domain_expert_ = std::make_shared<plansys2::DomainExpertClient>(); problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>(); executor_client_ = std::make_shared<plansys2::ExecutorClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>(); run_plan();
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() private:
{ std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
auto domain = domain_expert_->getDomain(); std::shared_ptr<plansys2::PlannerClient> planner_client_;
auto problem = problem_expert_->getProblem(); std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
auto plan = planner_client_->getPlan(domain, problem); std::shared_ptr<plansys2::ExecutorClient> executor_client_;
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_;
}; };
int main(int argc, char * argv[]) int main(int argc, char *argv[]) {
{
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<TaskPlannerController>()); rclcpp::spin(std::make_shared<TaskPlannerController>());
rclcpp::shutdown(); rclcpp::shutdown();