from stash
This commit is contained in:
parent
d72c06efea
commit
76cd4319eb
8 changed files with 828 additions and 839 deletions
|
@ -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;
|
||||||
}
|
}
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
|
@ -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;
|
||||||
}
|
}
|
|
@ -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;
|
||||||
}
|
}
|
|
@ -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"
|
||||||
|
|
|
@ -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();
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue