robossembler/runtime
8
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

Migration to BT.cpp v4 & new BT executor

Browse source
This commit is contained in:
Ilya Uraev 2024-07-04 12:11:07 +00:00 • committed by Igor Brylyov
parent b58307dea1
commit 2de37b027b
69 changed files with 843 additions and 2065 deletions
Download patch file Download diff file Expand all files Collapse all files

72
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
View file

@ -1,72 +0,0 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include <algorithm>
#include <cinttypes>
#include <memory>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rclcpp/rclcpp.hpp"
using AddPlanningSceneObject =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr;
void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<AddPlanningSceneObject::Request> request,
const std::shared_ptr<AddPlanningSceneObject::Response> response) {
(void)request_header;
// Create collision object for the robot to avoid
auto const collision_object = [frame_id = "world",
object_name = request->object_id,
object_pose = request->object_pose] {
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = frame_id;
collision_object.id = object_name;
shape_msgs::msg::SolidPrimitive primitive;
// Define the size of the box in meters
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.05;
primitive.dimensions[primitive.BOX_Y] = 0.05;
primitive.dimensions[primitive.BOX_Z] = 0.05;
// Define the pose of the box (relative to the frame_id)
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = object_pose[0];
box_pose.position.y = object_pose[1];
box_pose.position.z = object_pose[2];
box_pose.orientation.x = object_pose[3];
box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}();
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
response->success = true;
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true);
g_node =
rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<AddPlanningSceneObject>(
"add_planing_scene_object_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}

245
rbs_skill_servers/src/assemble_state_server.cpp
View file

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

1
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -179,6 +179,7 @@ private:
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {

1
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -133,6 +133,7 @@ private:
moveit::core::MoveItErrorCode move_err_code =
move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {

244
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

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