robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

refactor(MoveitMtp): MoveGroupInterface as shared_ptr

Browse source
This commit is contained in:
Ilya Uraev 2025-03-10 13:48:25 +03:00
parent 276f59f434
commit 9bc4774e8c
2 changed files with 57 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

51
rbs_skill_servers/src/mtp_moveit.cpp
View file

@ -1,6 +1,9 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <memory>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/utils/moveit_error_code.hpp>
namespace rbs_skill_actions {
@ -9,52 +12,64 @@ 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) {}
: SkillBase("mtp_moveit", options) {
this->declare_parameter("base_link", "");
this->declare_parameter("ee_link", "");
}
private:
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
move_group_ =
std::make_shared<moveit::planning_interface::MoveGroupInterface>(
this->shared_from_this(), m_current_goal->robot_name);
moveit::planning_interface::MoveGroupInterface move_group_interface(
this->shared_from_this(), m_current_goal->robot_name);
move_group_interface.startStateMonitor();
move_group_->startStateMonitor();
move_group_interface.setPoseTarget(m_current_goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(
geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.pose = m_current_goal->target_pose;
pose_stamped.header.frame_id = this->get_parameter("base_link").as_string();
pose_stamped.header.stamp = this->get_clock()->now();
move_group_->setPoseTarget(pose_stamped);
move_group_->setMaxVelocityScalingFactor(
m_current_goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(
move_group_->setMaxAccelerationScalingFactor(
m_current_goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::core::MoveItErrorCode plan_err_code =
move_group_interface.plan(plan);
moveit::core::MoveItErrorCode plan_err_code = move_group_->plan(plan);
if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
move_group_interface.plan(plan);
move_group_->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);
moveit::core::MoveItErrorCode move_err_code = move_group_->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_ERROR_STREAM(
this->get_logger(),
"Failed to move because:"
<< moveit::core::errorCodeToString(move_err_code));
}
} else {
RCLCPP_WARN_STREAM(this->get_logger(),
"Failed to generate plan because "
"Failed to generate plan because: "
<< moveit::core::errorCodeToString(plan_err_code));
m_current_result->success = false;
m_current_goal_handle->abort(m_current_result);
}
if (m_current_goal_handle->is_canceling()) {
m_current_result->success = false;
m_current_goal_handle->canceled(m_current_result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;

42
rbs_skill_servers/src/mtp_moveit_cart.cpp
View file

@ -11,10 +11,12 @@ using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
class MoveitMtpCart : public SkillBase<MoveitSendPose> {
public:
explicit MoveitMtpCart(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
explicit MoveitMtpCart(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase("mtp_moveit_cart", options) {}
private:
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
@ -22,15 +24,15 @@ private:
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_ =
std::make_shared<moveit::planning_interface::MoveGroupInterface>(
this->shared_from_this(), m_current_goal->robot_name);
move_group_->startStateMonitor();
moveit::core::RobotState start_state(
*move_group_interface.getCurrentState());
moveit::core::RobotState start_state(*move_group_->getCurrentState());
std::vector<geometry_msgs::msg::Pose> waypoints;
auto current_pose = move_group_interface.getCurrentPose();
auto current_pose = move_group_->getCurrentPose();
geometry_msgs::msg::Pose target_pose = m_current_goal->target_pose;
waypoints.push_back(target_pose);
@ -39,22 +41,22 @@ private:
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);
double fraction =
move_group_->computeCartesianPath(waypoints, eef_step, trajectory);
robot_trajectory::RobotTrajectory rt(
move_group_interface.getCurrentState()->getRobotModel(),
move_group_->getCurrentState()->getRobotModel(),
m_current_goal->robot_name);
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
bool su = tp.computeTimeStamps(rt);
if (!su) {
RCLCPP_ERROR(this->get_logger(), "Failed to compute timestamp of trajectory");
RCLCPP_ERROR(this->get_logger(),
"Failed to compute timestamp of trajectory");
m_current_goal_handle->abort(m_current_result);
}
@ -65,23 +67,27 @@ private:
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) {
moveit::core::MoveItErrorCode move_err_code = move_group_->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 (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
} else {
RCLCPP_ERROR_STREAM(
this->get_logger(),
"Failed to move because:"
<< moveit::core::errorCodeToString(move_err_code));
}
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
m_current_result->success = false;
m_current_goal_handle->abort(m_current_result);
}
if (m_current_goal_handle->is_canceling()) {
m_current_result->success = false;
m_current_goal_handle->canceled(m_current_result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;