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

Added rbs_gym package for RL & multi-robot launch setup

Browse source
This commit is contained in:
Ilya Uraev 2024-07-04 11:38:08 +00:00 • committed by Igor Brylyov
parent f92670cd0d
commit b58307dea1
103 changed files with 15170 additions and 653 deletions
Download patch file Download diff file Expand all files Collapse all files

8
rbs_skill_servers/CMakeLists.txt
View file

@ -34,6 +34,7 @@ find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package(Eigen3 3.3 REQUIRED)
find_package(rbs_utils REQUIRED)
find_package(moveit_servo REQUIRED)
# Default to Fortress
set(SDF_VER 12)
@ -79,6 +80,7 @@ set(deps
moveit_ros_planning
moveit_ros_planning_interface
moveit_msgs
moveit_servo
geometry_msgs
tf2_ros
rclcpp_components
@ -133,6 +135,11 @@ add_executable(move_cartesian_path_action_server
src/move_cartesian_path_action_server.cpp)
ament_target_dependencies(move_cartesian_path_action_server ${deps})
add_executable(servo_action_server
src/moveit_servo_skill_server.cpp)
ament_target_dependencies(servo_action_server ${deps})
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@ -143,6 +150,7 @@ install(
pick_place_pose_loader
move_to_joint_states_action_server
move_cartesian_path_action_server
servo_action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME})

24
rbs_skill_servers/launch/skills.launch.py
View file

@ -26,6 +26,10 @@ def launch_setup(context, *args, **kwargs):
"rbs_skill_servers", "config/gripperPositions.yaml"
)
kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
@ -38,12 +42,6 @@ def launch_setup(context, *args, **kwargs):
]
)
move_to_pose = Node(
package="rbs_skill_servers",
executable="move_to_pose.py",
namespace=namespace
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
@ -68,9 +66,17 @@ def launch_setup(context, *args, **kwargs):
{"use_sim_time": use_sim_time},
]
)
cartesian_move_to_pose_action_server = Node(
package="rbs_skill_servers",
executable="move_to_pose.py",
namespace=namespace,
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace}
]
)
# FIXME: The name of this node, "move_topose,"
# is intended to be different from the actual MoveToPose node.
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
@ -95,7 +101,7 @@ def launch_setup(context, *args, **kwargs):
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
move_to_pose,
cartesian_move_to_pose_action_server,
# grasp_pose_loader
]
return nodes_to_start

1
rbs_skill_servers/package.xml
View file

@ -13,6 +13,7 @@
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_servo</depend>
<depend>moveit_msgs</depend>
<depend>tf2_ros</depend>
<depend>rclcpp_action</depend>

145
rbs_skill_servers/scripts/move_to_pose.py
View file

@ -5,35 +5,40 @@ from rclpy.node import Node
import numpy as np
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
import math
from geometry_msgs.msg import Pose, PoseStamped
from rbs_skill_interfaces.action import MoveitSendPose
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
class PoseSubscriber(Node):
def __init__(self, parent=None):
super().__init__('pose_subscriber')
self.parent = parent
self._sub = self.create_subscription(PoseStamped,
"/cartesian_motion_controller/current_pose",
self.parent.on_pose_callback, 1,
callback_group=self.parent._callback_group)
self.get_logger().info('PoseSubscriber node initialized')
class CartesianMoveToPose(Node):
def __init__(self):
super().__init__('cartesian_move_to_pose')
super().__init__('cartesian_move_to_pose') # pyright: ignore[]
self.declare_parameter("base_link", "base_link")
self.declare_parameter("robot_name", "")
self._callback_group = ReentrantCallbackGroup()
self._action_server = ActionServer(
self,
MoveitSendPose,
'cartesian_move_to_pose',
self.execute_callback, callback_group=self._callback_group)
# for multirobot setup where each robot name is a namespace
self.robot_name: str = self.get_parameter("robot_name").get_parameter_value().string_value
self.robot_name = self.robot_name.lstrip('/').rstrip('/')
self.robot_name = f"/{self.robot_name}" if self.robot_name else ""
self._pub = self.create_publisher(PoseStamped,
"/cartesian_motion_controller/target_frame", 1,
f"{self.robot_name}/cartesian_motion_controller/target_frame", 1,
callback_group=self._callback_group)
self.current_pose = None
self.goal_tolerance = 0.05
self.max_speed = 0.1
self.max_acceleration = 0.05
self.base_link = self.get_parameter("base_link").get_parameter_value().string_value
def on_pose_callback(self, msg: PoseStamped):
if isinstance(msg, PoseStamped):
@ -41,13 +46,22 @@ class CartesianMoveToPose(Node):
def execute_callback(self, goal_handle):
self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
tp = PoseStamped()
tp.pose = goal_handle.request.target_pose
tp.header.stamp = self.get_clock().now().to_msg()
tp.header.frame_id = "base_link"
target_pose = goal_handle.request.target_pose
start_pose = self.current_pose.pose if self.current_pose else None
while self.get_distance_to_target(tp.pose) >= self.goal_tolerance:
if start_pose is None:
self.get_logger().error("Current pose is not available")
goal_handle.abort()
return MoveitSendPose.Result()
trajectory = self.generate_trajectory(start_pose, target_pose)
for point in trajectory:
tp = PoseStamped()
tp.pose = point
tp.header.stamp = self.get_clock().now().to_msg()
tp.header.frame_id = self.base_link
self._pub.publish(tp)
rclpy.spin_once(self, timeout_sec=0.1)
goal_handle.succeed()
@ -55,6 +69,77 @@ class CartesianMoveToPose(Node):
result.success = True
return result
def generate_trajectory(self, start_pose, target_pose):
start_position = np.array([
start_pose.position.x,
start_pose.position.y,
start_pose.position.z
])
target_position = np.array([
target_pose.position.x,
target_pose.position.y,
target_pose.position.z
])
start_orientation = R.from_quat([
start_pose.orientation.x,
start_pose.orientation.y,
start_pose.orientation.z,
start_pose.orientation.w
])
target_orientation = R.from_quat([
target_pose.orientation.x,
target_pose.orientation.y,
target_pose.orientation.z,
target_pose.orientation.w
])
distance = np.linalg.norm(target_position - start_position)
max_speed = self.max_speed
max_acceleration = self.max_acceleration
t_acc = max_speed / max_acceleration
d_acc = 0.5 * max_acceleration * t_acc**2
if distance < 2 * d_acc:
t_acc = math.sqrt(distance / max_acceleration)
t_flat = 0
else:
t_flat = (distance - 2 * d_acc) / max_speed
total_time = 2 * t_acc + t_flat
num_points = int(total_time * 10)
trajectory = []
times = np.linspace(0, total_time, num_points + 1)
key_rots = R.from_quat([start_orientation.as_quat(), target_orientation.as_quat()])
slerp = Slerp([0, total_time], key_rots)
for t in times:
if t < t_acc:
fraction = 0.5 * max_acceleration * t**2 / distance
elif t < t_acc + t_flat:
fraction = (d_acc + max_speed * (t - t_acc)) / distance
else:
t_decel = t - t_acc - t_flat
fraction = (d_acc + max_speed * t_flat + 0.5 * max_acceleration * t_decel**2) / distance
intermediate_position = start_position + fraction * (target_position - start_position)
intermediate_orientation = slerp([t])[0]
intermediate_pose = Pose()
intermediate_pose.position.x = intermediate_position[0]
intermediate_pose.position.y = intermediate_position[1]
intermediate_pose.position.z = intermediate_position[2]
intermediate_orientation_quat = intermediate_orientation.as_quat()
intermediate_pose.orientation.x = intermediate_orientation_quat[0]
intermediate_pose.orientation.y = intermediate_orientation_quat[1]
intermediate_pose.orientation.z = intermediate_orientation_quat[2]
intermediate_pose.orientation.w = intermediate_orientation_quat[3]
trajectory.append(intermediate_pose)
return trajectory
def get_distance_to_target(self, target_pose: Pose):
if self.current_pose is None or self.current_pose.pose is None:
self.get_logger().warn("Current pose is not available")
@ -65,36 +150,40 @@ class CartesianMoveToPose(Node):
current_position = np.array([
current_pose.position.x,
current_pose.position.y,
current_pose.position.z,
current_pose.orientation.x,
current_pose.orientation.y,
current_pose.orientation.z
current_pose.position.z
])
target_position = np.array([
target_pose.position.x,
target_pose.position.y,
target_pose.position.z,
target_pose.orientation.x,
target_pose.orientation.y,
target_pose.orientation.z
target_pose.position.z
])
# Проверка на наличие значений в массивах координат
if np.any(np.isnan(current_position)) or np.any(np.isnan(target_position)):
self.get_logger().error("Invalid coordinates")
return None
# Вычисляем расстояние между текущей и целевой позициями
distance = np.linalg.norm(current_position - target_position)
return distance
class PoseSubscriber(Node):
def __init__(self, parent: CartesianMoveToPose, robot_name: str):
super().__init__('pose_subscriber') # pyright: ignore[]
self.parent = parent
self._sub = self.create_subscription(PoseStamped,
f"{robot_name}/cartesian_motion_controller/current_pose",
self.parent.on_pose_callback, 1,
callback_group=self.parent._callback_group)
self.get_logger().info('PoseSubscriber node initialized')
def main(args=None):
rclpy.init(args=args)
cartesian_move_to_pose = CartesianMoveToPose()
pose_subscriber = PoseSubscriber(parent=cartesian_move_to_pose)
pose_subscriber = PoseSubscriber(parent=cartesian_move_to_pose,
robot_name=cartesian_move_to_pose.robot_name)
executor = MultiThreadedExecutor()
executor.add_node(cartesian_move_to_pose)

40
rbs_skill_servers/scripts/test_cartesian_controller.py
View file

@ -2,30 +2,36 @@
import rclpy
from rclpy.node import Node
import argparse
from geometry_msgs.msg import PoseStamped
class CartesianControllerPublisher(Node):
def __init__(self, robot_name: str):
def __init__(self, robot_name: str, poses: dict):
super().__init__("cartesian_controller_pose_publisher")
self.publisher_ = self.create_publisher(
PoseStamped,
"/" + robot_name + "/cartesian_motion_controller/target_frame", 10)
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.robot_name = robot_name
self.poses = poses
def timer_callback(self):
pose = self.poses.get(self.robot_name, {
'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
})
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "base_link"
msg.pose.position.x = 0.7
msg.pose.position.y = 0.0
msg.pose.position.z = 0.45
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.707
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 0.707
msg.pose.position.x = pose['position']['x']
msg.pose.position.y = pose['position']['y']
msg.pose.position.z = pose['position']['z']
msg.pose.orientation.x = pose['orientation']['x']
msg.pose.orientation.y = pose['orientation']['y']
msg.pose.orientation.z = pose['orientation']['z']
msg.pose.orientation.w = pose['orientation']['w']
self.publisher_.publish(msg)
@ -36,7 +42,21 @@ def main(args=None):
parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
args = parser.parse_args()
minimal_publisher = CartesianControllerPublisher(args.robot_name)
# Define poses for each robot
poses = {
'arm2': {
'position': {'x': -0.3, 'y': 0.0, 'z': 0.45},
'orientation': {'x': 0.0, 'y': -0.707, 'z': 0.0, 'w': 0.707}
},
'arm1': {
'position': {'x': 0.3, 'y': 0.0, 'z': 0.45},
'orientation': {'x': 0.0, 'y': 0.707, 'z': 0.0, 'w': 0.707}
}
# Add more robots and their poses as needed
}
minimal_publisher = CartesianControllerPublisher(args.robot_name, poses)
rclpy.spin(minimal_publisher)

47
rbs_skill_servers/scripts/test_cartesian_controller_single.py Normal file
View file

@ -0,0 +1,47 @@
import rclpy
from rclpy.node import Node
import argparse
from geometry_msgs.msg import PoseStamped
class CartesianControllerPublisher(Node):
def __init__(self, robot_name: str):
super().__init__("cartesian_controller_pose_publisher")
self.publisher_ = self.create_publisher(
PoseStamped,
"/cartesian_motion_controller/target_frame", 10)
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "base_link"
msg.pose.position.x = 0.2
msg.pose.position.y = 0.2
msg.pose.position.z = 0.2
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 1.0
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 0.0
self.publisher_.publish(msg)
def main(args=None):
rclpy.init(args=args)
parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
args = parser.parse_args()
minimal_publisher = CartesianControllerPublisher(args.robot_name)
rclpy.spin(minimal_publisher)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

43
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -1,5 +1,7 @@
#include <functional>
#include <memory>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <thread>
#include "rclcpp/rclcpp.hpp"
@ -19,6 +21,7 @@
#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 <tf2/LinearMath/Quaternion.h>
@ -122,23 +125,59 @@ private:
std::vector<geometry_msgs::msg::Pose> 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<double>(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);
// 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);
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(trajectory);
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");

232
rbs_skill_servers/src/moveit_servo_skill_server.cpp Normal file
View file

@ -0,0 +1,232 @@
#include <functional>
#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <memory>
#include <rclcpp/publisher.hpp>
#include <rclcpp/qos.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <thread>
#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_servo/make_shared_from_pool.h>
#include <moveit_servo/pose_tracking.h>
#include <moveit_servo/servo.h>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/status_codes.h>
namespace rbs_skill_actions {
class StatusMonitor {
public:
StatusMonitor(const rclcpp::Node::SharedPtr &node, const std::string &topic)
: m_node(node) {
sub_ = node->create_subscription<std_msgs::msg::Int8>(
topic, rclcpp::SystemDefaultsQoS(),
[this](const std_msgs::msg::Int8::ConstSharedPtr &msg) {
return statusCB(msg);
});
}
private:
rclcpp::Node::SharedPtr m_node;
void statusCB(const std_msgs::msg::Int8::ConstSharedPtr &msg) {
moveit_servo::StatusCode latest_status =
static_cast<moveit_servo::StatusCode>(msg->data);
if (latest_status != status_) {
status_ = latest_status;
const auto &status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_);
RCLCPP_INFO_STREAM(m_node->get_logger(), "Servo status: " << status_str);
}
}
moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID;
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr sub_;
};
class MoveServoActionServer : public rclcpp::Node {
public:
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
// explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveServoActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_servo_action_server"), m_node(node) {
auto servo_parameters =
moveit_servo::ServoParameters::makeServoParameters(node);
if (servo_parameters == nullptr) {
RCLCPP_FATAL(node->get_logger(), "Could not get servo parameters!");
// exit(EXIT_FAILURE);
}
// Load the planning scene monitor
m_planning_scene_monitor =
std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
node, "robot_description");
if (!m_planning_scene_monitor->getPlanningScene()) {
RCLCPP_ERROR_STREAM(node->get_logger(),
"Error in setting up the PlanningSceneMonitor.");
}
m_planning_scene_monitor->providePlanningSceneService();
m_planning_scene_monitor->startSceneMonitor();
m_planning_scene_monitor->startWorldGeometryMonitor(
planning_scene_monitor::PlanningSceneMonitor::
DEFAULT_COLLISION_OBJECT_TOPIC,
planning_scene_monitor::PlanningSceneMonitor::
DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
false /* skip octomap monitor */);
m_planning_scene_monitor->startStateMonitor("/joint_states");
m_planning_scene_monitor->startPublishingPlanningScene(
planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);
// Wait for Planning Scene Monitor to setup
if (!m_planning_scene_monitor->waitForCurrentRobotState(
node->now(), 5.0 /* seconds */)) {
RCLCPP_ERROR_STREAM(
node->get_logger(),
"Error waiting for current robot state in PlanningSceneMonitor.");
}
// Create the pose tracker
m_tracker = std::make_shared<moveit_servo::PoseTracking>(
node, servo_parameters, m_planning_scene_monitor);
m_status_monitor = std::make_shared<StatusMonitor>(node, servo_parameters->status_topic);
}
void init() {
m_action_server = rclcpp_action::create_server<MoveitSendPose>(
m_node->get_node_base_interface(), m_node->get_node_clock_interface(),
m_node->get_node_logging_interface(),
m_node->get_node_waitables_interface(), "move_servo",
std::bind(&MoveServoActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveServoActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveServoActionServer::accepted_callback, this,
std::placeholders::_1));
m_pose_pub = m_node->create_publisher<geometry_msgs::msg::PoseStamped>("target_pose", rclcpp::SystemDefaultsQoS());
}
private:
rclcpp::Node::SharedPtr m_node;
rclcpp_action::Server<MoveitSendPose>::SharedPtr m_action_server;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_pose_pub;
planning_scene_monitor::PlanningSceneMonitorPtr m_planning_scene_monitor;
moveit_servo::PoseTrackingPtr m_tracker;
std::shared_ptr<StatusMonitor> m_status_monitor;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendPose::Goal> 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<ServerGoalHandle> 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<ServerGoalHandle> goal_handle) {
using namespace std::placeholders;
std::thread(std::bind(&MoveServoActionServer::execute, this, _1),
goal_handle)
.detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
geometry_msgs::msg::TransformStamped current_ee_pose;
m_tracker->getCommandFrameTransform(current_ee_pose);
// Convert it to a Pose
geometry_msgs::msg::PoseStamped target_pose;
target_pose.header.frame_id = current_ee_pose.header.frame_id;
target_pose.pose = goal->target_pose;
// target_pose.pose.position.y = current_ee_pose.transform.translation.y;
// target_pose.pose.position.z = current_ee_pose.transform.translation.z;
// target_pose.pose.orientation = current_ee_pose.transform.rotation;
// target_pose.pose.position.x += 0.1;
m_tracker->resetTargetPose();
target_pose.header.stamp = m_node->now();
m_pose_pub->publish(target_pose);
Eigen::Vector3d lin_tol{ 0.001, 0.001, 0.001 };
double rot_tol = 0.01;
// Run the pose tracking
moveit_servo::PoseTrackingStatusCode tracking_status =
m_tracker->moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */);
if (tracking_status == moveit_servo::PoseTrackingStatusCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Pose tracking succeeded.");
} else {
result->success = false;
goal_handle->abort(result);
RCLCPP_INFO(this->get_logger(), "Pose tracking failed with status: %d", static_cast<int>(tracking_status));
}
RCLCPP_INFO_STREAM(m_node->get_logger(), "Pose tracker exited with status: "
<< moveit_servo::POSE_TRACKING_STATUS_CODE_MAP.at(tracking_status));
}
}; // 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);
node_options.allow_undeclared_parameters();
auto node = rclcpp::Node::make_shared("move_servo", "", node_options);
rbs_skill_actions::MoveServoActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}