95 lines
3.5 KiB
C++
95 lines
3.5 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"
|
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
|
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
|
|
#include <rclcpp/logging.hpp>
|
|
|
|
namespace rbs_skill_actions {
|
|
|
|
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
|
|
|
|
class MoveitMtpCart : public SkillBase<MoveitSendPose> {
|
|
public:
|
|
explicit MoveitMtpCart(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
|
|
: SkillBase("mtp_moveit_cart", 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();
|
|
|
|
moveit::core::RobotState start_state(
|
|
*move_group_interface.getCurrentState());
|
|
|
|
std::vector<geometry_msgs::msg::Pose> waypoints;
|
|
auto current_pose = move_group_interface.getCurrentPose();
|
|
geometry_msgs::msg::Pose target_pose = m_current_goal->target_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(),
|
|
m_current_goal->robot_name);
|
|
|
|
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
|
|
|
|
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
|
|
|
|
bool su = tp.computeTimeStamps(rt);
|
|
if (!su) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to compute timestamp of trajectory");
|
|
m_current_goal_handle->abort(m_current_result);
|
|
}
|
|
|
|
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) {
|
|
m_current_result->success = true;
|
|
m_current_goal_handle->succeed(m_current_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");
|
|
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::MoveitMtpCart);
|