2023-03-31 20:24:56 +00:00
|
|
|
#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"
|
2023-11-09 15:39:19 +03:00
|
|
|
#include "rclcpp_action/rclcpp_action.hpp"
|
2023-03-31 20:24:56 +00:00
|
|
|
|
|
|
|
#include "sensor_msgs/msg/joint_state.hpp"
|
2023-11-09 15:39:19 +03:00
|
|
|
#include "std_msgs/msg/float64_multi_array.hpp"
|
2023-03-31 20:24:56 +00:00
|
|
|
|
|
|
|
namespace rbs_skill_actions {
|
|
|
|
|
2023-11-09 15:39:19 +03:00
|
|
|
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>(
|
2024-04-18 13:29:36 +00:00
|
|
|
"~/gripper_controller/commands", 10);
|
2023-11-09 15:39:19 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
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(),
|
2023-12-14 12:00:35 +03:00
|
|
|
"goal request recieved for gripper [%.6f m]", goal->position);
|
2023-11-09 15:39:19 +03:00
|
|
|
(void)uuid;
|
2023-12-14 12:00:35 +03:00
|
|
|
// if (goal->position > 0.008 or goal->position < 0) {
|
|
|
|
// return rclcpp_action::GoalResponse::REJECT;
|
|
|
|
// }
|
2023-11-09 15:39:19 +03:00
|
|
|
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
|
2023-03-31 20:24:56 +00:00
|
|
|
|
|
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
|