From 76cd4319ebf29400dba8a252427a1170950fb3d6 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 5 Dec 2023 22:37:28 +0300 Subject: [PATCH] from stash --- .../add_planning_scene_objects_service.cpp | 57 +-- .../src/assemble_state_server.cpp | 324 ++++++++--------- .../src/gripper_control_action_server.cpp | 153 +++++---- .../src/move_cartesian_path_action_server.cpp | 264 +++++++------- .../move_to_joint_states_action_server.cpp | 231 ++++++------- .../src/move_topose_action_server.cpp | 247 ++++++------- .../src/moveit_update_planning_scene.cpp | 325 +++++++++--------- .../src/task_planner_controller.cpp | 66 ++-- 8 files changed, 828 insertions(+), 839 deletions(-) diff --git a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp index 3ecb596..aaf0a88 100644 --- a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp +++ b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp @@ -1,24 +1,25 @@ +#include "moveit/move_group_interface/move_group_interface.h" +#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" +#include #include #include -#include #include -#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" -#include "moveit/move_group_interface/move_group_interface.h" #include "rclcpp/rclcpp.hpp" -using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject; +using AddPlanningSceneObject = + rbs_skill_interfaces::srv::AddPlanningSceneObject; rclcpp::Node::SharedPtr g_node = nullptr; void handle_service( const std::shared_ptr request_header, const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - // Create collision object for the robot to avoid - auto const collision_object = [frame_id = - "world", object_name=request->object_id, object_pose=request->object_pose] { + const std::shared_ptr response) { + (void)request_header; + // Create collision object for the robot to avoid + auto const collision_object = [frame_id = "world", + object_name = request->object_id, + object_pose = request->object_pose] { moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = frame_id; collision_object.id = object_name; @@ -40,32 +41,32 @@ void handle_service( box_pose.orientation.y = object_pose[4]; box_pose.orientation.z = object_pose[5]; box_pose.orientation.w = object_pose[6]; - - collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; return collision_object; - }(); + }(); - // Add the collision object to the scene - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - planning_scene_interface.applyCollisionObject(collision_object); + // Add the collision object to the scene + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + planning_scene_interface.applyCollisionObject(collision_object); - response->success = true; + response->success = true; } -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); - g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); - auto server = g_node->create_service("add_planing_scene_object_service", handle_service); - rclcpp::spin(g_node); - rclcpp::shutdown(); - g_node = nullptr; - return 0; +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + g_node = + rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); + auto server = g_node->create_service( + "add_planing_scene_object_service", handle_service); + rclcpp::spin(g_node); + rclcpp::shutdown(); + g_node = nullptr; + return 0; } \ No newline at end of file diff --git a/rbs_skill_servers/src/assemble_state_server.cpp b/rbs_skill_servers/src/assemble_state_server.cpp index d359528..25ec2c5 100644 --- a/rbs_skill_servers/src/assemble_state_server.cpp +++ b/rbs_skill_servers/src/assemble_state_server.cpp @@ -1,18 +1,18 @@ +#include #include #include #include #include -#include -#include #include +#include -#include -#include #include +#include +#include -#include #include +#include #include #include @@ -24,12 +24,11 @@ #define ASSEMBLE_POSITION_OFFSET 0.5 #define ASSEMBLE_ORIENTATION_OFFSET 0.5 -#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ +#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ (package_dir) + "/models/" + (assemble_name) + "/model.sdf" -static inline geometry_msgs::msg::Pose get_pose( - const std::vector & tokens) -{ +static inline geometry_msgs::msg::Pose +get_pose(const std::vector &tokens) { geometry_msgs::msg::Pose p; p.position.set__x(std::stod(tokens.at(0))); p.position.set__y(std::stod(tokens.at(1))); @@ -41,23 +40,22 @@ static inline geometry_msgs::msg::Pose get_pose( return p; } -static inline geometry_msgs::msg::PoseStamped get_pose_stamped( - const std::string & pose_string) -{ +static inline geometry_msgs::msg::PoseStamped +get_pose_stamped(const std::string &pose_string) { std::stringstream ss(pose_string); std::istream_iterator begin(ss); std::istream_iterator end; std::vector tokens(begin, end); - + geometry_msgs::msg::PoseStamped ps; ps.set__pose(get_pose(tokens)); return ps; } -static std::vector get_assemble_poses( - const std::string & assemble_name, const std::string & part_name, - const std::string& package_dir, const std::string & assemble_prefix) -{ +static std::vector +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 result; std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf"; @@ -66,12 +64,11 @@ static std::vector get_assemble_poses( auto model = doc.RootElement()->FirstChildElement("model"); auto joint = model->FirstChildElement("joint"); - while (joint) - { + while (joint) { auto frame_id = joint->FirstChildElement("child")->GetText(); if (frame_id != part_name) continue; - auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); + auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); ps.header.set__frame_id(assemble_prefix + part_name); result.push_back(ps); joint = joint->NextSiblingElement("joint"); @@ -80,164 +77,167 @@ static std::vector get_assemble_poses( return result; } -class AssembleStateServer: public rclcpp::Node -{ +class AssembleStateServer : public rclcpp::Node { public: - AssembleStateServer(rclcpp::NodeOptions options) - : Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); - assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); - tf_buffer_ = std::make_unique(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - service_ = create_service( - SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + AssembleStateServer(rclcpp::NodeOptions options) + : Node(SERVICE_NAME, + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); + assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + service_ = create_service( + SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + void + callback(std::shared_ptr + request, + std::shared_ptr + response) { + auto state = static_cast(request->req_kind); + bool call_status = false; + std::string assemble_name = request->assemble_name; + std::string part_name = request->part_name; + auto assemble = assembles_.find(request->assemble_name); + if (assemble == assembles_.end()) + assembles_.insert(std::make_pair( + request->assemble_name, + Assemble{.part = request->part_name, + .ws = request->workspace, + .state = NONE, + .poses = + get_assemble_poses(assemble_name, part_name, + assemble_dir_, assemble_prefix_)})); + RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", + assemble_name.c_str()); + switch (state) { + case NONE: + call_status = false; + break; + case INITIALIZE: + call_status = (assembles_.at(assemble_name).state == NONE) && + on_initialize(&assembles_.at(assemble_name)); + break; + case VALIDATE: + response->validate_status = + (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && + on_validate(&assembles_.at(assemble_name)); + break; + case COMPLETE: + call_status = (assembles_.at(assemble_name).state == VALIDATE) && + on_complete(&assembles_.at(assemble_name)); + if (call_status) + assembles_.erase(assemble_name); + break; } - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - bool call_status = false; - std::string assemble_name = request->assemble_name; - std::string part_name = request->part_name; - auto assemble = assembles_.find(request->assemble_name); - if (assemble == assembles_.end()) - assembles_.insert(std::make_pair(request->assemble_name, Assemble { - .part=request->part_name, - .ws=request->workspace, - .state=NONE, - .poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_) - })); - RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str()); - switch(state) - { - case NONE: - call_status = false; - break; - case INITIALIZE: - call_status = (assembles_.at(assemble_name).state == NONE) && - on_initialize(&assembles_.at(assemble_name)); - break; - case VALIDATE: - response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && - on_validate(&assembles_.at(assemble_name)); - break; - case COMPLETE: - call_status = (assembles_.at(assemble_name).state == VALIDATE) && - on_complete(&assembles_.at(assemble_name)); - if (call_status) - assembles_.erase(assemble_name); - break; - } + response->call_status = call_status; + response->assemble_name = assemble_name; + } - response->call_status = call_status; - response->assemble_name = assemble_name; - } - private: - enum AssembleReqState - { - NONE=-1, - INITIALIZE=0, - VALIDATE=1, - COMPLETE=2 - }; + enum AssembleReqState { + NONE = -1, + INITIALIZE = 0, + VALIDATE = 1, + COMPLETE = 2 + }; - struct Assemble - { - std::string part; - std::string ws; - AssembleReqState state; - std::vector poses; - std::unique_ptr tf2_broadcaster; - }; + struct Assemble { + std::string part; + std::string ws; + AssembleReqState state; + std::vector poses; + std::unique_ptr 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(this); - } catch (const tf2::TransformException &ex){ - RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what()); - return false; - } - assemble->state = INITIALIZE; - for (const auto& it: assemble->poses) - { - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = assemble->ws; - t.child_frame_id = it.header.frame_id; - auto pose = it.pose; - t.transform.translation.x = pose.position.x; - t.transform.translation.y = pose.position.y; - t.transform.translation.z = pose.position.z; - t.transform.rotation.x = pose.orientation.x; - t.transform.rotation.y = pose.orientation.y; - t.transform.rotation.z = pose.orientation.z; - t.transform.rotation.w = pose.orientation.w; - assemble->tf2_broadcaster->sendTransform(t); - } - - return true; + bool on_initialize(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster = + std::make_unique(this); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", + ex.what()); + return false; + } + assemble->state = INITIALIZE; + for (const auto &it : assemble->poses) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = assemble->ws; + t.child_frame_id = it.header.frame_id; + auto pose = it.pose; + t.transform.translation.x = pose.position.x; + t.transform.translation.y = pose.position.y; + t.transform.translation.z = pose.position.z; + t.transform.rotation.x = pose.orientation.x; + t.transform.rotation.y = pose.orientation.y; + t.transform.rotation.z = pose.orientation.z; + t.transform.rotation.w = pose.orientation.w; + assemble->tf2_broadcaster->sendTransform(t); } - bool on_validate(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str()); - std::string frame_from = assemble_prefix_ + assemble->part; - std::string frame_to = assemble->part; - geometry_msgs::msg::TransformStamped ts; - try - { - ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); - } catch (const tf2::TransformException &ex) { - return false; - } - auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; - auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; + return true; + } - RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate); - assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE; + bool on_validate(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", + assemble->part.c_str()); + std::string frame_from = assemble_prefix_ + assemble->part; + std::string frame_to = assemble->part; + geometry_msgs::msg::TransformStamped ts; + try { + ts = + tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + return false; + } + auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; + auto orient_validate = + ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; - return assemble->state == VALIDATE; + RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", + pos_validate, orient_validate); + assemble->state = (pos_validate && orient_validate) ? VALIDATE : INITIALIZE; + + return assemble->state == VALIDATE; + } + + bool on_complete(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster.reset(); + assemble->tf2_broadcaster = NULL; + assemble->poses.clear(); + } catch (const std::exception &ex) { + RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", + ex.what()); + return false; } - bool on_complete(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str()); - try - { - assemble->tf2_broadcaster.reset(); - assemble->tf2_broadcaster = NULL; - assemble->poses.clear(); - } catch (const std::exception &ex) { - RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what()); - return false; - } + assemble->state = COMPLETE; - assemble->state = COMPLETE; + return true; + } - return true; - } - - private: - std::map assembles_; - std::shared_ptr tf_listener_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf2_broadcaster_; - std::mutex mt; - rclcpp::Service::SharedPtr service_; - std::string assemble_dir_; - std::string assemble_prefix_; + std::map assembles_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf2_broadcaster_; + std::mutex mt; + rclcpp::Service::SharedPtr service_; + std::string assemble_dir_; + std::string assemble_prefix_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_skill_servers/src/gripper_control_action_server.cpp b/rbs_skill_servers/src/gripper_control_action_server.cpp index 8c6f682..9f68288 100644 --- a/rbs_skill_servers/src/gripper_control_action_server.cpp +++ b/rbs_skill_servers/src/gripper_control_action_server.cpp @@ -7,105 +7,110 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/gripper_command.hpp" +#include "rclcpp_action/rclcpp_action.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" #include "sensor_msgs/msg/joint_state.hpp" - - +#include "std_msgs/msg/float64_multi_array.hpp" namespace rbs_skill_actions { - class GripperControlActionServer: public rclcpp::Node { +class GripperControlActionServer : public rclcpp::Node { - public: - using GripperCommand = rbs_skill_interfaces::action::GripperCommand; - explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true)) - { - this->actionServer_ = rclcpp_action::create_server( - this, - "gripper_control", - std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1) - ); - publisher = this->create_publisher("/gripper_controller/commands", 10); - join_state_subscriber = this->create_subscription( - "/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1)); - } +public: + using GripperCommand = rbs_skill_interfaces::action::GripperCommand; + explicit GripperControlActionServer(rclcpp::NodeOptions options) + : Node("gripper_control_action_server", + options.allow_undeclared_parameters(true)) { + this->actionServer_ = rclcpp_action::create_server( + this, "gripper_control", + std::bind(&GripperControlActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperControlActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&GripperControlActionServer::accepted_callback, this, + std::placeholders::_1)); + publisher = this->create_publisher( + "/gripper_controller/commands", 10); + join_state_subscriber = + this->create_subscription( + "/joint_states", 10, + std::bind(&GripperControlActionServer::joint_state_callback, this, + std::placeholders::_1)); + } +private: + rclcpp_action::Server::SharedPtr actionServer_; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr + join_state_subscriber; + double gripper_joint_state{1.0}; - private: - - rclcpp_action::Server::SharedPtr actionServer_; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr join_state_subscriber; - double gripper_joint_state{1.0}; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + void joint_state_callback(const sensor_msgs::msg::JointState &msg) { + gripper_joint_state = msg.position.back(); + } - void joint_state_callback(const sensor_msgs::msg::JointState & msg) - { - gripper_joint_state = msg.position.back(); - } + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { - rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - - RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position); - (void)uuid; - if(goal->position > 0.9 or goal->position < 0) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } + RCLCPP_INFO(this->get_logger(), + "goal request recieved for gripper [%.2f m]", goal->position); + (void)uuid; + if (goal->position > 0.9 or goal->position < 0) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) { + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - void accepted_callback(const std::shared_ptr goal_handle) { - std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach(); - } - - void execute(const std::shared_ptr goal_handle){ + void accepted_callback(const std::shared_ptr goal_handle) { + std::thread{std::bind(&GripperControlActionServer::execute, this, + std::placeholders::_1), + goal_handle} + .detach(); + } - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - auto feedback = std::make_shared(); - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); - return; - } - RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state); + void execute(const std::shared_ptr goal_handle) { - std_msgs::msg::Float64MultiArray command; + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + auto feedback = std::make_shared(); - using namespace std::chrono_literals; + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); + return; + } + RCLCPP_INFO(this->get_logger(), "Current gripper state %f", + gripper_joint_state); - command.data.push_back(goal->position); - RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper"); - publisher->publish(command); - std::this_thread::sleep_for(3s); + std_msgs::msg::Float64MultiArray command; + using namespace std::chrono_literals; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); - - } - }; -} - + command.data.push_back(goal->position); + RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper"); + publisher->publish(command); + std::this_thread::sleep_for(3s); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); + } +}; +} // namespace rbs_skill_actions RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer); - // int main(int argc, char ** argv) { // rclcpp::init(argc, argv); // rbs_skill_actions::GripperControlActionServer server; diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp index c3deb63..1709f0f 100644 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,156 +26,156 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveCartesianActionServer : public rclcpp::Node -{ +class MoveCartesianActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), - // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); - } + // explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_cartesian_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), + // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_cartesian", - std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_cartesian", + std::bind(&MoveCartesianActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveCartesianActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveCartesianActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr 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 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 goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + 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 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 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 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 goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - 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 waypoints; - auto current_pose = move_group_interface.getCurrentPose(); - //waypoints.push_back(current_pose.pose); - geometry_msgs::msg::Pose target_pose = goal->target_pose; - //target_pose.position = goal->target_pose.position; - waypoints.push_back(target_pose); - - RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", - target_pose.position.x, target_pose.position.y, target_pose.position.z); - //waypoints.push_back(start_pose.pose); - moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; - const double eef_step = 0.001; - double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - if(fraction>0) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory); - if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - }else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - goal_handle->abort(result); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + } }; // class MoveCartesianActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); - rbs_skill_actions::MoveCartesianActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveCartesianActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; + return 0; } \ No newline at end of file diff --git a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp index 9f79472..2f61ed9 100644 --- a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp +++ b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp @@ -1,16 +1,15 @@ +#include #include #include #include -#include - #include "rclcpp/rclcpp.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,143 +25,139 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToJointStateActionServer : public rclcpp::Node -{ +class MoveToJointStateActionServer : public rclcpp::Node { public: - using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates; + using MoveitSendJointStates = + rbs_skill_interfaces::action::MoveitSendJointStates; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_to_joint_states_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_to_joint_states", - std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_to_joint_states", + std::bind(&MoveToJointStateActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToJointStateActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToJointStateActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = + rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str()); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", + goal->robot_name.c_str()); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr 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 goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), + goal_handle) + .detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + std::vector 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 goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach(); + + move_group_interface.setJointValueTarget(goal->joint_values); + move_group_interface.setMaxVelocityScalingFactor( + goal->joints_velocity_scaling_factor); + move_group_interface.setMaxAccelerationScalingFactor( + goal->joints_acceleration_scaling_factor); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_interface.plan(plan) == + moveit::core::MoveItErrorCode::SUCCESS); + if (success) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + move_group_interface.execute(plan); + // move_group_interface.move(); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - std::vector joint_states = goal->joint_values; - for (auto &joint : joint_states) - { - RCLCPP_INFO(this->get_logger(), "Joint value %f", joint); - } - - - move_group_interface.setJointValueTarget(goal->joint_values); - move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor); - move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor); - - - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); - if(success) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - move_group_interface.execute(plan); - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } - - - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } }; // class MoveToJointStateActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToJointStateActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToJointStateActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; + return 0; } \ No newline at end of file diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp index 4bf83ad..4ed59bc 100644 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ b/rbs_skill_servers/src/move_topose_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -18,8 +18,8 @@ // moveit libs #include "moveit/move_group_interface/move_group_interface.h" #include "moveit/planning_interface/planning_interface.h" -#include "moveit/robot_model_loader/robot_model_loader.h" #include "moveit/planning_scene_interface/planning_scene_interface.h" +#include "moveit/robot_model_loader/robot_model_loader.h" /* #include @@ -27,148 +27,149 @@ #include */ // For Visualization -//#include -#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" +#include -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToPoseActionServer : public rclcpp::Node -{ +class MoveToPoseActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_topose_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_topose", - std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_topose", + std::bind(&MoveToPoseActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToPoseActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToPoseActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr 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 goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - move_group_interface.setPoseTarget(goal->target_pose); - move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); - move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration); - moveit::planning_interface::MoveGroupInterface::Plan plan; - moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan); - if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){ - move_group_interface.plan(plan); - } - if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - //move_group_interface.execute(plan); - moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan); - if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - }else{ - RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code)); - goal_handle->abort(result); - } + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), + goal_handle) + .detach(); + } - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + move_group_interface.setPoseTarget(goal->target_pose); + move_group_interface.setMaxVelocityScalingFactor( + goal->end_effector_velocity); + move_group_interface.setMaxAccelerationScalingFactor( + goal->end_effector_acceleration); + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveit::core::MoveItErrorCode plan_err_code = + move_group_interface.plan(plan); + if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) { + move_group_interface.plan(plan); } + if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + // move_group_interface.execute(plan); + moveit::core::MoveItErrorCode move_err_code = + move_group_interface.execute(plan); + if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "Failure in move:"); + } + + } else { + RCLCPP_WARN_STREAM(this->get_logger(), + "Failed to generate plan because " + << error_code_to_string(plan_err_code)); + goal_handle->abort(result); + } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; + } + } }; // class MoveToPoseActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToPoseActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToPoseActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; + return 0; } \ No newline at end of file diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp index 366bf35..8840992 100644 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -1,20 +1,20 @@ +#include #include #include -#include -#include #include #include -#include #include +#include +#include #include -#include -#include #include +#include +#include -#include #include +#include #include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp" @@ -22,8 +22,8 @@ #define INIT_SCENE_PARAM_NAME "init_scene" #define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths" -static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &pose) -{ +static geometry_msgs::msg::Pose +pose_from_pose3d(const ignition::math::Pose3d &pose) { geometry_msgs::msg::Pose result; auto position = pose.Pos(); result.position.set__x(position.X()); @@ -37,129 +37,125 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p return result; } -static std::string get_correct_mesh_path( - const std::string& uri, const std::vector &resources) -{ +static std::string +get_correct_mesh_path(const std::string &uri, + const std::vector &resources) { std::string result = ""; std::regex reg(R"((?:model|package)(?:\:\/)(.*))"); std::smatch m; - if (std::regex_match(uri, m, reg)) - { + if (std::regex_match(uri, m, reg)) { std::string rel_path = m[1]; - std::for_each(resources.begin(), resources.end(), - [&result, &rel_path](const std::string& res){ - if (result.empty()) - result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result; - }); + std::for_each(resources.begin(), resources.end(), + [&result, &rel_path](const std::string &res) { + if (result.empty()) + result = std::filesystem::exists(res + rel_path) + ? std::string(res + rel_path) + : result; + }); } - return "file://" + result; + return "file://" + result; } -static moveit_msgs::msg::CollisionObject -get_object(const sdf::Model *model, const std::vector &resources) -{ +static moveit_msgs::msg::CollisionObject +get_object(const sdf::Model *model, const std::vector &resources) { moveit_msgs::msg::CollisionObject obj; obj.header.frame_id = "world"; obj.id = model->Name(); obj.pose = pose_from_pose3d(model->RawPose()); size_t link_count = model->LinkCount(); - for (size_t i = 0; i < link_count; ++i) - { + for (size_t i = 0; i < link_count; ++i) { auto link = model->LinkByIndex(i); auto collision = link->CollisionByIndex(0); auto link_pose = pose_from_pose3d(link->RawPose()); auto geometry = collision->Geom(); - switch(geometry->Type()) - { - case sdf::GeometryType::MESH: - { - auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); - shapes::Mesh *m = shapes::createMeshFromResource( - !path.empty()? path: geometry->MeshShape()->Uri()); - auto scale = geometry->MeshShape()->Scale().X(); - m->scale(scale); + switch (geometry->Type()) { + case sdf::GeometryType::MESH: { + auto path = + get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); + shapes::Mesh *m = shapes::createMeshFromResource( + !path.empty() ? path : geometry->MeshShape()->Uri()); + auto scale = geometry->MeshShape()->Scale().X(); + m->scale(scale); - Eigen::Vector3d centroid = {0, 0, 0}; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) - { - const auto x = m->vertices[i]; - const auto y = m->vertices[i+1]; - const auto z = m->vertices[i+2]; + Eigen::Vector3d centroid = {0, 0, 0}; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + const auto x = m->vertices[i]; + const auto y = m->vertices[i + 1]; + const auto z = m->vertices[i + 2]; - centroid.x() += x*(1-scale); - centroid.y() += y*(1-scale); - centroid.z() += z*(1-scale); - } - - centroid = centroid / m->vertex_count; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { - m->vertices[i] -= centroid.x(); - m->vertices[i + 1] -= centroid.y(); - m->vertices[i + 2] -= centroid.z(); - } - - shape_msgs::msg::Mesh mesh; - shapes::ShapeMsg m_msg; - shapes::constructMsgFromShape(m, m_msg); - mesh = boost::get(m_msg); - obj.meshes.push_back(mesh); - obj.mesh_poses.push_back(link_pose); - break; - } case sdf::GeometryType::BOX: { - auto sdf_box = geometry->BoxShape(); - shape_msgs::msg::SolidPrimitive box; - box.type = shape_msgs::msg::SolidPrimitive::BOX; - auto sdf_box_size = sdf_box->Size(); - box.dimensions.push_back(sdf_box_size.X()); - box.dimensions.push_back(sdf_box_size.Y()); - box.dimensions.push_back(sdf_box_size.Z()); - obj.primitives.push_back(box); - obj.primitive_poses.push_back(link_pose); - break; + centroid.x() += x * (1 - scale); + centroid.y() += y * (1 - scale); + centroid.z() += z * (1 - scale); } - case sdf::GeometryType::PLANE: - { - auto sdf_plane = geometry->PlaneShape(); - shape_msgs::msg::Plane plane; - auto normal = sdf_plane->Normal(); - plane.coef[0] = normal.X(); - plane.coef[1] = normal.Y(); - plane.coef[2] = normal.Z(); - obj.planes.push_back(plane); - obj.plane_poses.push_back(link_pose); - break; + + centroid = centroid / m->vertex_count; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + m->vertices[i] -= centroid.x(); + m->vertices[i + 1] -= centroid.y(); + m->vertices[i + 2] -= centroid.z(); } - case sdf::GeometryType::EMPTY: - case sdf::GeometryType::CYLINDER: - case sdf::GeometryType::SPHERE: - case sdf::GeometryType::HEIGHTMAP: - case sdf::GeometryType::CAPSULE: - case sdf::GeometryType::ELLIPSOID: - case sdf::GeometryType::POLYLINE: - break; + + shape_msgs::msg::Mesh mesh; + shapes::ShapeMsg m_msg; + shapes::constructMsgFromShape(m, m_msg); + mesh = boost::get(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 -get_objects(const sdf::World *world, const std::string &model_resources) -{ +static std::vector +get_objects(const sdf::World *world, const std::string &model_resources) { std::vector resources; std::regex reg("\\:+"); - std::sregex_token_iterator begin( - model_resources.begin(), model_resources.end(), reg, -1), end; + std::sregex_token_iterator begin(model_resources.begin(), + model_resources.end(), reg, -1), + end; std::copy(++begin, end, std::back_inserter(resources)); std::vector result; auto models_count = world->ModelCount(); - for (size_t i = 0; i < models_count; ++i) - { - try{ + for (size_t i = 0; i < models_count; ++i) { + try { auto model = world->ModelByIndex(i); result.push_back(get_object(model, resources)); - } catch (std::exception &ex){ + } catch (std::exception &ex) { std::cerr << ex.what() << std::endl; } } @@ -167,81 +163,80 @@ get_objects(const sdf::World *world, const std::string &model_resources) return result; } -class UpdatePlanningSceneServer: public rclcpp::Node -{ +class UpdatePlanningSceneServer : public rclcpp::Node { public: - UpdatePlanningSceneServer(rclcpp::NodeOptions options) - : Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); - std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); + UpdatePlanningSceneServer(rclcpp::NodeOptions options) + : Node("update_planning_scene_node", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); + std::string model_resources = + get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); - if (!scene.empty()) - { - RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); - init_scene(scene, model_resources); - } - service_ = create_service( - SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + if (!scene.empty()) { + RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); + init_scene(scene, model_resources); + } + service_ = + create_service( + 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(request->req_kind); + moveit_msgs::msg::CollisionObject obj; + obj.id = request->frame_name; + switch (state) { + case ADD: + case UPDATE: + obj.meshes.resize(1); + obj.mesh_poses.resize(1); + obj.operation = state == ADD ? moveit_msgs::msg::CollisionObject::ADD + : moveit_msgs::msg::CollisionObject::MOVE; + obj.meshes.at(0) = std::move(request->mesh); + obj.mesh_poses.at(0) = std::move(request->pose.pose); + break; + case REMOVE: + obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; + break; } - bool init_scene(const std::string &init_scene, const std::string &model_resources) - { - sdf::Root root; - sdf::ParserConfig config; - config.AddURIPath("package://", model_resources); - config.AddURIPath("model://", model_resources); - sdf::Errors errors = root.Load(init_scene, config); - if (!errors.empty()) - { - for (auto const &e: errors) - RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str()); - return false; - } - auto world = root.WorldByIndex(0); - auto objects = get_objects(world, model_resources); - planning_scene_.applyCollisionObjects(objects); - return true; - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - moveit_msgs::msg::CollisionObject obj; - obj.id = request->frame_name; - switch(state) - { - case ADD: - case UPDATE: - obj.meshes.resize(1); - obj.mesh_poses.resize(1); - obj.operation = state == ADD? - moveit_msgs::msg::CollisionObject::ADD: - moveit_msgs::msg::CollisionObject::MOVE; - obj.meshes.at(0) = std::move(request->mesh); - obj.mesh_poses.at(0) = std::move(request->pose.pose); - break; - case REMOVE: - obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; - break; - } - - response->call_status = planning_scene_.applyCollisionObject(obj); - } + response->call_status = planning_scene_.applyCollisionObject(obj); + } private: - enum UpdatePlanningSceneRequestState - { - ADD=0, - REMOVE=1, - UPDATE=2 - }; + enum UpdatePlanningSceneRequestState { ADD = 0, REMOVE = 1, UPDATE = 2 }; - rclcpp::Service::SharedPtr service_; - moveit::planning_interface::PlanningSceneInterface planning_scene_; + rclcpp::Service< + rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_; + moveit::planning_interface::PlanningSceneInterface planning_scene_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_task_planner/src/task_planner_controller.cpp b/rbs_task_planner/src/task_planner_controller.cpp index 168fc27..e0603cf 100644 --- a/rbs_task_planner/src/task_planner_controller.cpp +++ b/rbs_task_planner/src/task_planner_controller.cpp @@ -13,47 +13,39 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -class TaskPlannerController : public rclcpp::Node -{ - public: - TaskPlannerController() : rclcpp::Node("task_planner_controller") - { - domain_expert_ = std::make_shared(); - planner_client_ = std::make_shared(); - problem_expert_ = std::make_shared(); - executor_client_ = std::make_shared(); - run_plan(); +class TaskPlannerController : public rclcpp::Node { +public: + TaskPlannerController() : rclcpp::Node("task_planner_controller") { + domain_expert_ = std::make_shared(); + planner_client_ = std::make_shared(); + problem_expert_ = std::make_shared(); + executor_client_ = std::make_shared(); + run_plan(); + } + + void run_plan() { + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + + auto plan = planner_client_->getPlan(domain, problem); + if (!plan.has_value()) { + RCLCPP_ERROR(this->get_logger(), "Could not find plan to reach goal %s", + parser::pddl::toString(problem_expert_->getGoal()).c_str()); + } else { + if (executor_client_->start_plan_execution(plan.value())) { + RCLCPP_INFO(this->get_logger(), "Execute plan..."); + } } + } - void run_plan() - { - auto domain = domain_expert_->getDomain(); - auto problem = problem_expert_->getProblem(); - auto plan = planner_client_->getPlan(domain, problem); - if(!plan.has_value()) - { - RCLCPP_ERROR(this->get_logger(), "Could not find plan to reach goal %s", - parser::pddl::toString(problem_expert_->getGoal()).c_str()); - } - else - { - if (executor_client_->start_plan_execution(plan.value())) - { - RCLCPP_INFO(this->get_logger(), "Execute plan..."); - } - } - } - - private: - std::shared_ptr domain_expert_; - std::shared_ptr planner_client_; - std::shared_ptr problem_expert_; - std::shared_ptr executor_client_; - +private: + std::shared_ptr domain_expert_; + std::shared_ptr planner_client_; + std::shared_ptr problem_expert_; + std::shared_ptr executor_client_; }; -int main(int argc, char * argv[]) -{ +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown();