diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp deleted file mode 100644 index 7b1421b..0000000 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -// action libs -#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" -#include "geometry_msgs/msg/transform.hpp" - -// 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/trajectory_processing/time_optimal_trajectory_generation.h" - -/* -#include -#include -#include -*/ -// For Visualization -// #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" - -namespace rbs_skill_actions { - -class MoveCartesianActionServer : public rclcpp::Node { -public: - 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)); - } - - 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_; - - 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; - - 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 start_pose = current_pose.pose; - geometry_msgs::msg::Pose target_pose = goal->target_pose; - // target_pose.position = goal->target_pose.position; - // int num_waypoints = 100; - // for (int i = 1; i <= num_waypoints; ++i) { - // geometry_msgs::msg::Pose intermediate_pose; - // double fraction = static_cast(i) / (num_waypoints + 1); - // - // intermediate_pose.position.x = - // start_pose.position.x + - // fraction * (target_pose.position.x - start_pose.position.x); - // intermediate_pose.position.y = - // start_pose.position.y + - // fraction * (target_pose.position.y - start_pose.position.y); - // intermediate_pose.position.z = - // start_pose.position.z + - // fraction * (target_pose.position.z - start_pose.position.z); - // - // intermediate_pose.orientation = start_pose.orientation; - // - // waypoints.push_back(intermediate_pose); - // } - 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); - - 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); - - robot_trajectory::RobotTrajectory rt( - move_group_interface.getCurrentState()->getRobotModel(), - goal->robot_name); - - rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory); - - trajectory_processing::TimeOptimalTrajectoryGeneration tp; - - bool su = tp.computeTimeStamps(rt); - - rt.getRobotTrajectoryMsg(trajectory); - - moveit::planning_interface::MoveGroupInterface::Plan plan; - plan.trajectory = trajectory; - - if (fraction > 0) { - RCLCPP_INFO(this->get_logger(), "Planning success"); - 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) { - 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 - -} // 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); - - 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(); - - return 0; -} diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp deleted file mode 100644 index 351069e..0000000 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -// action libs -#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" -#include "geometry_msgs/msg/transform.hpp" - -// moveit libs -#include "moveit/move_group_interface/move_group_interface.h" -#include "moveit/planning_interface/planning_interface.h" -#include "moveit/planning_scene_interface/planning_scene_interface.h" -#include "moveit/robot_model_loader/robot_model_loader.h" - -/* -#include -#include -#include -*/ -// For Visualization -// #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 - -namespace rbs_skill_actions { - -class MoveToPoseActionServer : public rclcpp::Node { -public: - 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)); - } - - 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_; - - 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::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(); - - 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) { - 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) { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - - } else { - RCLCPP_WARN_STREAM(this->get_logger(), - "Failed to generate plan because " - << moveit::core::errorCodeToString(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 - -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(); - }); - - rclcpp::spin(node); - run_server.join(); - - return 0; -} diff --git a/rbs_skill_servers/src/mtp_moveit.cpp b/rbs_skill_servers/src/mtp_moveit.cpp index 7b07c9e..ba8e503 100644 --- a/rbs_skill_servers/src/mtp_moveit.cpp +++ b/rbs_skill_servers/src/mtp_moveit.cpp @@ -50,7 +50,7 @@ private: } else { RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " - << error_code_to_string(plan_err_code)); + << moveit::core::errorCodeToString(plan_err_code)); m_current_goal_handle->abort(m_current_result); }