robossembler/runtime
8
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

refactor(rbs_skill_servers): update action configuration, dependencies, and skills

Browse source 
- Removed `assembly_config_service.py` node from launch configuration.
- Added default `goal.duration` setting to `MoveToPose` and `MoveToPoseArray`.
- Replaced `timeout_seconds` with `duration` in action definitions for `MoveitSendJointStates` and `MoveitSendPose`.
- Removed dependencies on TinyXML2 and Gazebo/SDFormat, adding `controller_manager_msgs` and `control_msgs` to CMake configuration.
- Added new action servers `cartesian_move_to_pose` and `move_to_joint_states`, registering them in CMakeLists file.
- Introduced `SkillBase`, a template class for managing action-based skills, providing essential ROS 2 action server support and functionalities for handling goals, cancels, accepted actions, and controller management.
- Implemented methods to load, configure, and switch required controllers with conflict detection for active controllers, along with parameter checking and asynchronous handling for required parameters.
- Enhanced error handling for missing controllers, parameters, and resource conflicts.
- Updated `skills.launch.py` to utilize `ComposableNodeContainer` for skill nodes, incorporating `MoveToJointStateActionServer` and `CartesianMoveToPose` as composable nodes.
- Changed the executable name in `cartesian_move_to_pose_action_server` node configuration.
- Added `cartesian_move_to_pose.cpp`, implementing the `CartesianMoveToPose` action server, including trajectory interpolation, pose adjustment, and controller management.
- Updated `package.xml` to include `rclcpp_components` dependency.
- Refactored `MoveToJointStateActionServer` to extend `SkillBase`, leveraging `FollowJointTrajectory` for joint trajectory execution, while removing redundant code and dependencies.
- Implemented trajectory generation based on initial and target joint positions with parameterized interpolation for smoother execution, enhancing joint state handling to dynamically align current and target joint values.
This commit is contained in:
Ilya Uraev 2024-10-30 17:49:03 +03:00
parent 4dccc2b74c
commit a7b7225dd1
24 changed files with 1864 additions and 1204 deletions
Download patch file Download diff file Expand all files Collapse all files

0
rbs_skill_servers/src/gripper_control_action_server.cpp → rbs_skill_servers/src/gripper_command.cpp
View file

221
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -1,221 +0,0 @@
#include <functional>
#include <memory>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#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/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>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#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<MoveitSendPose>(
// 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<MoveitSendPose>(
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<MoveitSendPose>::SharedPtr action_server_;
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(&MoveCartesianActionServer::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>();
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<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);
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;
}

164
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -1,164 +0,0 @@
#include <cinttypes>
#include <functional>
#include <memory>
#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_joint_states.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 <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#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 MoveToJointStateActionServer : public rclcpp::Node {
public:
using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
// explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node)
: Node("move_to_joint_states_action_server"), node_(node) {
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// 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<MoveitSendJointStates>(
node_->get_node_base_interface(), node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "move_to_joint_states",
std::bind(&MoveToJointStateActionServer::goal_callback, this,
std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToJointStateActionServer::cancel_callback, this,
std::placeholders::_1),
std::bind(&MoveToJointStateActionServer::accepted_callback, this,
std::placeholders::_1));
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal) {
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]",
goal->robot_name.c_str());
(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<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(&MoveToJointStateActionServer::execute, this, _1),
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<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(
node_, goal->robot_name);
move_group_interface.startStateMonitor();
std::vector<double> joint_states = goal->joint_values;
for (auto &joint : joint_states) {
RCLCPP_INFO(this->get_logger(), "Joint value %f", joint);
}
move_group_interface.setJointValueTarget(goal->joint_values);
move_group_interface.setMaxVelocityScalingFactor(
goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(
goal->joints_acceleration_scaling_factor);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) ==
moveit::core::MoveItErrorCode::SUCCESS);
if (success) {
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
// move_group_interface.move();
} else {
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToJointStateActionServer
} // 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_to_joint_states", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

176
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -1,176 +0,0 @@
#include <functional>
#include <memory>
#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/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 <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
// #include <eigen_conversions/eigen_msg.h>
#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 <moveit_msgs/msg/display_robot_state.hpp>
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<MoveitSendPose>(
// 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<MoveitSendPose>(
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<MoveitSendPose>::SharedPtr action_server_;
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;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
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(&MoveToPoseActionServer::execute, this, _1),
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>();
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 "
<< error_code_to_string(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;
}

155
rbs_skill_servers/src/mtjs_jtc.cpp Normal file
View file

@ -0,0 +1,155 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_joint_states__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendJointStates =
rbs_skill_interfaces::action::MoveitSendJointStates;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToJointStateActionServer : public SkillBase<MoveitSendJointStates> {
public:
explicit MoveToJointStateActionServer(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendJointStates>("mtjs_jtc", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToJointStateActionServer::jointStateCallback, this,
std::placeholders::_1),
s_options);
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
}
trajectory_goal.trajectory.points = generate_trajectory(
m_current_joint_positions, m_current_goal->joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
// RCLCPP_INFO(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_mame_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
if (index_it != m_joint_mame_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
t * (target_joint_values[j] - start_joint_values[j]);
point.positions.push_back(position);
}
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(
rbs_skill_actions::MoveToJointStateActionServer);

183
rbs_skill_servers/src/mtp_cart.cpp Normal file
View file

@ -0,0 +1,183 @@
#include "Eigen/Dense"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/logging.hpp>
// Action libs
#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;
using GoalHandleMoveitSendPose =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
class CartesianMoveToPose : public SkillBase<MoveitSendPose> {
public:
explicit CartesianMoveToPose(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_cart", options), m_current_step(0) {
// Initialize publisher for target poses
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::PublisherOptions p_options;
p_options.callback_group = cbg;
m_publisher = this->create_publisher<geometry_msgs::msg::PoseStamped>(
"cartesian_motion_controller/target_frame", 10, p_options);
m_current_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_current_pose_subscriber =
this->create_subscription<geometry_msgs::msg::PoseStamped>(
"cartesian_motion_controller/current_pose", 10,
std::bind(&CartesianMoveToPose::on_pose_callback, this,
std::placeholders::_1),
s_options);
}
protected:
std::string requiredActionController() override {
return "cartesian_motion_controller";
}
std::vector<std::string> requiredParameters() override {
return {"end_effector_link", "robot_base_link"};
}
void executeAction() override {
m_base_link = this->get_parameter("robot_base_link").as_string();
m_trajectory =
interpolate(m_current_pose->pose, m_current_goal->target_pose);
m_current_step = 0;
m_total_time = m_current_goal->duration;
m_step_time =
m_total_time / static_cast<double>(m_trajectory.size()) * 1000.0;
m_trajectory_timer = this->create_wall_timer(
std::chrono::milliseconds(static_cast<int>(m_step_time)),
[this]() { adjust_and_publish_pose(); });
}
private:
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_publisher;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
m_current_pose_subscriber;
geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose;
std::string m_base_link;
double m_threshold_distance = 0.02;
std::vector<geometry_msgs::msg::Pose> m_trajectory;
size_t m_current_step;
rclcpp::TimerBase::SharedPtr m_trajectory_timer;
double m_total_time;
double m_step_time;
void adjust_and_publish_pose() {
if (m_current_step < m_trajectory.size()) {
publish_pose(m_trajectory[m_current_step++]);
} else {
double distance =
calculate_distance(m_current_pose->pose, m_current_goal->target_pose);
if (distance <= m_threshold_distance) {
m_current_result->success = true;
m_current_goal_handle->succeed(m_current_result);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
m_trajectory_timer->cancel();
} else {
publish_pose(m_trajectory.back());
}
}
}
double calculate_distance(const geometry_msgs::msg::Pose &p1,
const geometry_msgs::msg::Pose &p2) {
Eigen::Vector3d pos1(p1.position.x, p1.position.y, p1.position.z);
Eigen::Vector3d pos2(p2.position.x, p2.position.y, p2.position.z);
double position_distance = (pos1 - pos2).norm();
Eigen::Quaterniond orient1 = normalize_orientation(p1.orientation);
Eigen::Quaterniond orient2 = normalize_orientation(p2.orientation);
return position_distance + orient1.angularDistance(orient2);
}
Eigen::Quaterniond
normalize_orientation(const geometry_msgs::msg::Quaternion &q) {
Eigen::Quaterniond orientation(q.w, q.x, q.y, q.z);
orientation.normalize();
if (orientation.norm() == 0) {
RCLCPP_FATAL(this->get_logger(),
"Quaternion has zero norm; normalization failed.");
throw std::runtime_error("Quaternion normalization failed.");
}
return orientation;
}
std::vector<geometry_msgs::msg::Pose>
interpolate(const geometry_msgs::msg::Pose &start_pose,
const geometry_msgs::msg::Pose &end_pose) {
std::vector<geometry_msgs::msg::Pose> trajectory;
Eigen::Vector3d start_position(start_pose.position.x, start_pose.position.y,
start_pose.position.z);
Eigen::Vector3d end_position(end_pose.position.x, end_pose.position.y,
end_pose.position.z);
Eigen::Quaterniond start_orientation =
normalize_orientation(start_pose.orientation);
Eigen::Quaterniond end_orientation =
normalize_orientation(end_pose.orientation);
double distance = (end_position - start_position).norm();
int num_steps = static_cast<int>(distance / 0.01);
for (int i = 0; i <= num_steps; ++i) {
double t = static_cast<double>(i) / num_steps;
Eigen::Vector3d interpolated_position =
start_position + t * (end_position - start_position);
Eigen::Quaterniond interpolated_orientation =
start_orientation.slerp(t, end_orientation);
geometry_msgs::msg::Pose interpolated_pose;
interpolated_pose.position.x = interpolated_position.x();
interpolated_pose.position.y = interpolated_position.y();
interpolated_pose.position.z = interpolated_position.z();
interpolated_pose.orientation.x = interpolated_orientation.x();
interpolated_pose.orientation.y = interpolated_orientation.y();
interpolated_pose.orientation.z = interpolated_orientation.z();
interpolated_pose.orientation.w = interpolated_orientation.w();
trajectory.push_back(interpolated_pose);
}
return trajectory;
}
void publish_pose(const geometry_msgs::msg::Pose &pose) {
geometry_msgs::msg::PoseStamped tp;
tp.pose = pose;
tp.header.stamp = this->now();
tp.header.frame_id = m_base_link;
m_publisher->publish(tp);
}
void on_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
m_current_pose = msg;
}
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::CartesianMoveToPose);

224
rbs_skill_servers/src/mtp_jtc.cpp Normal file
View file

@ -0,0 +1,224 @@
#include "Eigen/Dense"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "kdl/chainiksolverpos_lma.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <cstddef>
#include <iterator>
#include <kdl/chain.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToPose : public SkillBase<MoveitSendPose> {
public:
explicit MoveToPose(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_jtc", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToPose::jointStateCallback, this,
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
}
std::vector<double> target_joint_values;
solveIK(target_joint_values);
trajectory_goal.trajectory.points =
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
RCLCPP_WARN(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_mame_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_mame_to_index.find(m_joint_names[j]);
if (index_it != m_joint_mame_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
t * (target_joint_values[j] - start_joint_values[j]);
point.positions.push_back(position);
}
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
void solveIK(std::vector<double> &out) {
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
}
KDL::JntArray q_out(m_joint_names.size());
Eigen::Affine3d target_pose;
Eigen::fromMsg(m_current_goal->target_pose, target_pose);
KDL::Frame target_pose_kdl(
KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
KDL::Vector(target_pose.translation().x(),
target_pose.translation().y(),
target_pose.translation().z()));
const std::string m_base_link = "base_link";
const std::string m_ee_link = "gripper_grasp_point";
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to parse KDL tree from robot description.");
return;
}
KDL::Chain kdl_chain;
if (!kdl_tree.getChain(m_base_link, m_ee_link, kdl_chain)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to get KDL chain from base to end-effector.");
return;
}
auto ik_solver =
std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_chain, 1e-5, 500);
if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
out.resize(q_out.rows());
for (size_t i = 0; i < out.size(); i++) {
out[i] = q_out(i);
}
} else {
RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
}
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPose);

314
rbs_skill_servers/src/mtp_jtc_cart.cpp Normal file
View file

@ -0,0 +1,314 @@
#include "Eigen/Dense"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "kdl/chainfksolverpos_recursive.hpp"
#include "kdl/chainiksolverpos_lma.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <Eigen/src/Geometry/Transform.h>
#include <cstddef>
#include <iterator>
#include <kdl/chain.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <memory>
#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp_action/client.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <vector>
namespace rbs_skill_actions {
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
using GoalHandleMoveitSendJointStates =
rclcpp_action::ServerGoalHandle<MoveitSendPose>;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using FollowJointTrajectoryGoalHandle =
rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MoveToPoseJtcCartesian : public SkillBase<MoveitSendPose> {
public:
explicit MoveToPoseJtcCartesian(
const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
: SkillBase<MoveitSendPose>("mtp_jtc_cart", options) {
m_joint_trajectory_client =
rclcpp_action::create_client<FollowJointTrajectory>(
this, "/joint_trajectory_controller/follow_joint_trajectory");
auto cbg =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions s_options;
s_options.callback_group = cbg;
m_joint_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&MoveToPoseJtcCartesian::jointStateCallback, this,
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to obtain KDL tree from the robot description.");
throw std::runtime_error("KDL Tree initialization failed");
}
if (!kdl_tree.getChain("base_link", "gripper_grasp_point", m_kdl_chain)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to obtain KDL chain from base to end-effector.");
throw std::runtime_error("KDL Chain initialization failed");
}
}
protected:
std::string requiredActionController() override {
return "joint_trajectory_controller";
}
std::vector<std::string> requiredParameters() override { return {"joints"}; }
void executeAction() override {
RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto trajectory_goal = FollowJointTrajectory::Goal();
auto joints = this->get_parameters(requiredParameters());
m_joint_names = joints.at(0).as_string_array();
trajectory_goal.trajectory.joint_names = m_joint_names;
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
std::vector<double> target_joint_values;
Eigen::Affine3d target_pose;
Eigen::fromMsg(m_current_goal->target_pose, target_pose);
if (!solveIK(target_pose, target_joint_values)) {
RCLCPP_ERROR(this->get_logger(),
"IK solution not found for goal position");
m_current_goal_handle->abort(m_current_result);
}
trajectory_goal.trajectory.points =
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
[this](const FollowJointTrajectoryGoalHandle::WrappedResult
&wrapped_result) {
if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
m_current_goal_handle->succeed(m_current_result);
} else {
RCLCPP_ERROR(this->get_logger(), "Goal failed");
m_current_goal_handle->abort(m_current_result);
}
};
m_joint_trajectory_client->async_send_goal(trajectory_goal,
send_goal_options);
}
private:
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
if (m_joint_names.empty()) {
return;
}
if (m_joint_name_to_index.empty()) {
m_joint_name_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto it =
std::find(msg->name.begin(), msg->name.end(), m_joint_names[j]);
if (it != msg->name.end()) {
size_t index = std::distance(msg->name.begin(), it);
m_joint_name_to_index[m_joint_names[j]] = index;
}
}
}
if (m_current_joint_positions.size() != m_joint_names.size()) {
m_current_joint_positions.resize(m_joint_names.size(), 0.0);
}
for (size_t j = 0; j < m_joint_names.size(); ++j) {
auto index_it = m_joint_name_to_index.find(m_joint_names[j]);
if (index_it != m_joint_name_to_index.end()) {
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
generate_trajectory(const std::vector<double> &start_joint_values,
const std::vector<double> &target_joint_values,
const double duration) {
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
KDL::Frame start_pose_kdl, target_pose_kdl;
if (!getEndEffectorPose(start_joint_values, start_pose_kdl) ||
!getEndEffectorPose(target_joint_values, target_pose_kdl)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to get initial or target end-effector pose");
return points;
}
Eigen::Affine3d start_pose = KDLFrameToEigen(start_pose_kdl);
Eigen::Affine3d target_pose = KDLFrameToEigen(target_pose_kdl);
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
Eigen::Vector3d start_translation = start_pose.translation();
Eigen::Vector3d target_translation = target_pose.translation();
Eigen::Vector3d interpolated_translation =
(1.0 - t) * start_translation + t * target_translation;
Eigen::Translation3d interpolated_translation_transform(
interpolated_translation);
Eigen::Quaterniond start_quaternion(start_pose.rotation());
Eigen::Quaterniond target_quaternion(target_pose.rotation());
Eigen::Quaterniond interpolated_quaternion =
start_quaternion.slerp(t, target_quaternion);
Eigen::Affine3d interpolated_pose =
interpolated_translation_transform * interpolated_quaternion;
std::vector<double> ik_joint_values;
if (!solveIK(interpolated_pose, ik_joint_values)) {
RCLCPP_ERROR(this->get_logger(), "IK solution not found for point %d",
i);
return {};
}
point.positions = ik_joint_values;
point.time_from_start = rclcpp::Duration::from_seconds(t * duration);
points.push_back(point);
}
return points;
}
bool getEndEffectorPose(const std::vector<double> &joint_positions,
KDL::Frame &end_effector_pose) {
if (joint_positions.size() != m_joint_names.size()) {
RCLCPP_ERROR(this->get_logger(), "Mismatch between provided joint "
"positions and expected joint names.");
RCLCPP_ERROR(this->get_logger(), "Joint pos size: %zu",
joint_positions.size());
RCLCPP_ERROR(this->get_logger(), "Joint names size: %zu",
m_joint_names.size());
return false;
}
std::unordered_set<std::string> available_joints(m_joint_names.begin(),
m_joint_names.end());
if (available_joints.size() != m_joint_names.size()) {
RCLCPP_ERROR(this->get_logger(),
"Duplicate joints detected in joint names list.");
return false;
}
KDL::JntArray q_in(joint_positions.size());
for (size_t i = 0; i < joint_positions.size(); ++i) {
q_in(i) = joint_positions[i];
}
KDL::ChainFkSolverPos_recursive fk_solver(m_kdl_chain);
if (fk_solver.JntToCart(q_in, end_effector_pose) < 0) {
RCLCPP_ERROR(this->get_logger(),
"Failed to compute FK for the specified joint positions.");
return false;
}
return true;
}
Eigen::Affine3d KDLFrameToEigen(const KDL::Frame &frame) {
Eigen::Affine3d transform;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
transform(i, j) = frame.M(i, j);
}
transform(i, 3) = frame.p[i];
}
transform(3, 3) = 1.0;
return transform;
}
bool solveIK(const Eigen::Affine3d &target_pose, std::vector<double> &out) {
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
}
KDL::JntArray q_out(m_joint_names.size());
KDL::Frame target_pose_kdl(
KDL::Rotation(target_pose(0, 0), target_pose(0, 1), target_pose(0, 2),
target_pose(1, 0), target_pose(1, 1), target_pose(1, 2),
target_pose(2, 0), target_pose(2, 1), target_pose(2, 2)),
KDL::Vector(target_pose.translation().x(),
target_pose.translation().y(),
target_pose.translation().z()));
auto ik_solver =
std::make_unique<KDL::ChainIkSolverPos_LMA>(m_kdl_chain, 1e-5, 500);
if (ik_solver->CartToJnt(q_in, target_pose_kdl, q_out) >= 0) {
out.resize(q_out.rows());
for (size_t i = 0; i < out.size(); i++) {
out[i] = q_out(i);
}
return true;
} else {
RCLCPP_ERROR(this->get_logger(), "IK solution not found.");
return false;
}
}
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr
m_joint_trajectory_client;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
m_joint_state_subscriber;
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_name_to_index;
std::vector<std::string> m_joint_names;
KDL::Chain m_kdl_chain;
};
} // namespace rbs_skill_actions
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveToPoseJtcCartesian);

68
rbs_skill_servers/src/mtp_moveit.cpp Normal file
View file

@ -0,0 +1,68 @@
#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 "
<< error_code_to_string(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);

95
rbs_skill_servers/src/mtp_moveit_cart.cpp Normal file
View file

@ -0,0 +1,95 @@
#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);

1
rbs_skill_servers/src/moveit_servo_skill_server.cpp → rbs_skill_servers/src/mtp_servo.cpp
View file

@ -1,3 +1,4 @@
// FIXME: This code currently not used
#include <functional>
#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>