Bill Finger
a7b7225dd1
- 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.
98 lines
3.3 KiB
C++
98 lines
3.3 KiB
C++
#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/gripper_command.hpp"
|
|
#include "rclcpp_action/rclcpp_action.hpp"
|
|
|
|
#include "sensor_msgs/msg/joint_state.hpp"
|
|
#include "std_msgs/msg/float64_multi_array.hpp"
|
|
|
|
namespace rbs_skill_actions {
|
|
|
|
class GripperControlActionServer : public rclcpp::Node {
|
|
|
|
public:
|
|
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
|
|
explicit GripperControlActionServer(rclcpp::NodeOptions options)
|
|
: Node("gripper_control_action_server",
|
|
options.allow_undeclared_parameters(true)) {
|
|
this->actionServer_ = rclcpp_action::create_server<GripperCommand>(
|
|
this, "gripper_control",
|
|
std::bind(&GripperControlActionServer::goal_callback, this,
|
|
std::placeholders::_1, std::placeholders::_2),
|
|
std::bind(&GripperControlActionServer::cancel_callback, this,
|
|
std::placeholders::_1),
|
|
std::bind(&GripperControlActionServer::accepted_callback, this,
|
|
std::placeholders::_1));
|
|
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
|
|
"~/gripper_controller/commands", 10);
|
|
}
|
|
|
|
private:
|
|
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
|
|
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
|
|
|
|
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
|
|
|
|
rclcpp_action::GoalResponse
|
|
goal_callback(const rclcpp_action::GoalUUID &uuid,
|
|
std::shared_ptr<const GripperCommand::Goal> goal) {
|
|
|
|
RCLCPP_INFO(this->get_logger(),
|
|
"goal request recieved for gripper [%.6f m]", goal->position);
|
|
(void)uuid;
|
|
// if (goal->position > 0.008 or goal->position < 0) {
|
|
// 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 request to cancel goal");
|
|
(void)goal_handle;
|
|
return rclcpp_action::CancelResponse::ACCEPT;
|
|
}
|
|
|
|
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
|
std::thread{std::bind(&GripperControlActionServer::execute, this,
|
|
std::placeholders::_1),
|
|
goal_handle}
|
|
.detach();
|
|
}
|
|
|
|
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
|
|
|
const auto goal = goal_handle->get_goal();
|
|
auto result = std::make_shared<GripperCommand::Result>();
|
|
auto feedback = std::make_shared<GripperCommand::Feedback>();
|
|
|
|
if (goal_handle->is_canceling()) {
|
|
goal_handle->canceled(result);
|
|
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
|
|
return;
|
|
}
|
|
|
|
std_msgs::msg::Float64MultiArray command;
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
command.data.push_back(goal->position);
|
|
RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper");
|
|
publisher->publish(command);
|
|
std::this_thread::sleep_for(3s);
|
|
|
|
goal_handle->succeed(result);
|
|
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
|
|
}
|
|
};
|
|
} // namespace rbs_skill_actions
|
|
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
|