Loading files
00f4070e1This file is pinned to the selected release. Download the original to open it in your design tool.
#include <array>
#include <cmath>
#include <limits>
#include <vector>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <kalman_interfaces/action/arm_goto_joint_pose.hpp>
#include <kalman_interfaces/msg/arm_values.hpp>
#include <rclcpp/create_timer.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <urdf/model.h>
#include <std_msgs/msg/string.hpp>
namespace kalman_arm2 {
template <typename T> auto &ith_joint_val(T &msg, size_t i) {
if (i < 6) {
return msg.joints.at(i);
} else {
return msg.jaw;
}
}
class GotoJointPose : public rclcpp::Node {
public:
// ROS interfaces
rclcpp::Subscription<kalman_interfaces::msg::ArmValues>::SharedPtr
joint_pos_sub;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr
robot_description_sub;
rclcpp::Publisher<kalman_interfaces::msg::ArmValues>::SharedPtr
joint_vel_pub;
rclcpp::TimerBase::SharedPtr timer;
using ArmGotoJointPose = kalman_interfaces::action::ArmGotoJointPose;
using GoalHandleArmGotoJointPose =
rclcpp_action::ServerGoalHandle<ArmGotoJointPose>;
rclcpp_action::Server<ArmGotoJointPose>::SharedPtr action_server_;
// TF2
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
// Parameters
float max_joint_vel;
double update_rate;
float max_error;
std::vector<double> joint_limit_margins;
// State
kalman_interfaces::msg::ArmValues current_pos;
std::array<double, 7> joint_lower_limits;
std::array<double, 7> joint_upper_limits;
bool joint_limits_ready = false;
std::shared_ptr<GoalHandleArmGotoJointPose> current_gh;
GotoJointPose(const rclcpp::NodeOptions &options)
: Node("goto_joint_pose", options) {
// Parameters
this->declare_parameter<float>("max_joint_vel", 0.5);
this->declare_parameter<double>("update_rate", 10.0);
this->declare_parameter<float>("max_error", 0.1);
this->declare_parameter<std::vector<double>>(
"joint_limit_margins", std::vector<double>(6, 0.1)
);
this->get_parameter("max_joint_vel", max_joint_vel);
this->get_parameter("update_rate", update_rate);
this->get_parameter("max_error", max_error);
this->get_parameter("joint_limit_margins", joint_limit_margins);
if (joint_limit_margins.size() != 6) {
RCLCPP_WARN(
get_logger(),
"joint_limit_margins has %zu entries; expected 6. Using 0.1 rad.",
joint_limit_margins.size()
);
joint_limit_margins.assign(6, 0.1);
}
joint_lower_limits.fill(-std::numeric_limits<double>::infinity());
joint_upper_limits.fill(std::numeric_limits<double>::infinity());
// Publishers & subscribers
joint_vel_pub = create_publisher<kalman_interfaces::msg::ArmValues>(
"target_vel", 10
);
joint_pos_sub = create_subscription<kalman_interfaces::msg::ArmValues>(
"current_pos",
10,
std::bind(
&GotoJointPose::on_joint_positions, this, std::placeholders::_1
)
);
rclcpp::QoS robot_description_qos(1);
robot_description_qos.transient_local();
robot_description_qos.reliable();
robot_description_sub = create_subscription<std_msgs::msg::String>(
"/robot_description",
robot_description_qos,
std::bind(
&GotoJointPose::on_robot_description,
this,
std::placeholders::_1
)
);
// Create timer for periodic computation
auto timer_period = std::chrono::duration<double>(1.0 / update_rate);
timer = create_wall_timer(
timer_period, std::bind(&GotoJointPose::timer_cb, this)
);
using namespace std::placeholders;
action_server_ = rclcpp_action::create_server<ArmGotoJointPose>(
this,
"goto_pose",
std::bind(&GotoJointPose::handle_goal, this, _1, _2),
std::bind(&GotoJointPose::handle_cancel, this, _1),
std::bind(&GotoJointPose::handle_accepted, this, _1)
);
}
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const ArmGotoJointPose::Goal> goal
) {
(void)uuid;
if (!joint_limits_ready) {
RCLCPP_WARN(
get_logger(),
"Rejecting goal: joint limits are not available yet"
);
return rclcpp_action::GoalResponse::REJECT;
}
for (size_t i = 0; i < 7; ++i) {
if ((goal->ignore_mask & (1 << i)) != 0) {
continue;
}
const double target = ith_joint_val(goal->target_pos, i);
if (!std::isfinite(target) || target < joint_lower_limits[i] ||
target > joint_upper_limits[i]) {
RCLCPP_WARN(
get_logger(),
"Rejecting goal: joint %zu target %.3f is outside [%.3f, %.3f]",
i + 1,
target,
joint_lower_limits[i],
joint_upper_limits[i]
);
return rclcpp_action::GoalResponse::REJECT;
}
}
RCLCPP_INFO(get_logger(), "Accepted goal within joint limits");
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
handle_cancel(const std::shared_ptr<GoalHandleArmGotoJointPose> gh) {
RCLCPP_INFO(get_logger(), "Received request to cancel goal");
if (current_gh == gh) {
publish_zero_velocity();
RCLCPP_INFO(get_logger(), "Goal cancelled");
} else {
RCLCPP_WARN(
get_logger(),
"Cancel request for a goal that is not currently active"
);
}
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleArmGotoJointPose> gh) {
RCLCPP_INFO(this->get_logger(), "Accepted goal");
current_gh = gh;
}
void publish_zero_velocity() {
auto vel_msg = kalman_interfaces::msg::ArmValues();
vel_msg.header.stamp = now();
vel_msg.joints.fill(0.0);
vel_msg.jaw = 0.0;
joint_vel_pub->publish(vel_msg);
}
void
on_joint_positions(const kalman_interfaces::msg::ArmValues::SharedPtr msg) {
current_pos = *msg;
}
void on_robot_description(const std_msgs::msg::String::SharedPtr msg) {
urdf::Model model;
if (!model.initString(msg->data)) {
RCLCPP_ERROR(get_logger(), "Failed to parse /robot_description");
return;
}
const std::array<std::string, 7> names = {
"arm_joint_1", "arm_joint_2", "arm_joint_3", "arm_joint_4",
"arm_joint_5", "arm_joint_6", "arm_joint_jaw"
};
for (size_t i = 0; i < names.size(); ++i) {
const auto joint = model.getJoint(names[i]);
if (!joint || !joint->limits) {
RCLCPP_ERROR(
get_logger(), "No URDF limits found for %s", names[i].c_str()
);
return;
}
const double margin = i < 6 ? joint_limit_margins[i] : 0.0;
joint_lower_limits[i] = joint->limits->lower + margin;
joint_upper_limits[i] = joint->limits->upper - margin;
if (joint_lower_limits[i] > joint_upper_limits[i]) {
RCLCPP_ERROR(
get_logger(),
"Limit margin leaves no range for %s",
names[i].c_str()
);
return;
}
}
joint_limits_ready = true;
RCLCPP_INFO(get_logger(), "Loaded joint limits from /robot_description");
}
void timer_cb() {
if (!current_gh) {
return;
}
if (current_gh->is_canceling()) {
publish_zero_velocity();
current_gh->canceled(std::make_shared<ArmGotoJointPose::Result>());
current_gh.reset();
RCLCPP_INFO(get_logger(), "Goal transitioned to canceled");
return;
}
auto vel_msg = kalman_interfaces::msg::ArmValues();
vel_msg.header.stamp = now();
vel_msg.joints.fill(0.0);
vel_msg.jaw = 0.0;
uint8_t mask = current_gh->get_goal()->ignore_mask;
bool all_done = true;
for (size_t i = 0; i < 7; i++) {
// mask check
if ((mask & (1 << i))) {
continue;
}
auto &target_pos = current_gh->get_goal()->target_pos;
// error
const float current = ith_joint_val(current_pos, i);
const float target = ith_joint_val(target_pos, i);
const float error = target - current;
if (std::abs(error) > max_error) {
all_done = false;
// velocity
double vel = std::min(max_joint_vel, std::abs(error * 1.0f));
vel = std::copysign(vel, error);
ith_joint_val(vel_msg, i) = vel;
}
}
joint_vel_pub->publish(vel_msg);
// Decide if done
if (all_done) {
publish_zero_velocity();
current_gh->succeed(std::make_shared<ArmGotoJointPose::Result>());
RCLCPP_INFO(get_logger(), "Goal succeeded");
current_gh.reset();
} else {
auto fb = std::make_shared<ArmGotoJointPose::Feedback>();
fb->current_pos = current_pos;
current_gh->publish_feedback(fb);
}
}
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::GotoJointPose)