68 lines
2.6 KiB
C++
68 lines
2.6 KiB
C++
#include "moveit/move_group_interface/move_group_interface.h"
|
|
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
|
|
#include "rbs_skill_servers/base_skill.hpp"
|
|
|
|
namespace rbs_skill_actions {
|
|
|
|
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
|
|
|
|
class MoveitMtp : public SkillBase<MoveitSendPose> {
|
|
public:
|
|
explicit MoveitMtp(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
|
|
: SkillBase("mtp_moveit", options) {}
|
|
|
|
private:
|
|
std::string requiredActionController() override {
|
|
return "joint_trajectory_controller";
|
|
}
|
|
|
|
void executeAction() override {
|
|
RCLCPP_INFO(this->get_logger(), "Executing action goal");
|
|
|
|
moveit::planning_interface::MoveGroupInterface move_group_interface(
|
|
this->shared_from_this(), m_current_goal->robot_name);
|
|
move_group_interface.startStateMonitor();
|
|
|
|
move_group_interface.setPoseTarget(m_current_goal->target_pose);
|
|
move_group_interface.setMaxVelocityScalingFactor(
|
|
m_current_goal->end_effector_velocity);
|
|
move_group_interface.setMaxAccelerationScalingFactor(
|
|
m_current_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) {
|
|
m_current_result->success = true;
|
|
m_current_goal_handle->succeed(m_current_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));
|
|
m_current_goal_handle->abort(m_current_result);
|
|
}
|
|
|
|
if (m_current_goal_handle->is_canceling()) {
|
|
m_current_goal_handle->canceled(m_current_result);
|
|
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
|
|
return;
|
|
}
|
|
}
|
|
}; // class MoveToPoseActionServer
|
|
|
|
} // namespace rbs_skill_actions
|
|
|
|
#include "rclcpp_components/register_node_macro.hpp"
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtp);
|