runtime/rbs_skill_servers/src/gripper_command.cpp

99 lines
3.3 KiB
C++
Raw Normal View History

#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"
#include "sensor_msgs/msg/joint_state.hpp"
2023-11-09 15:39:19 +03:00
#include "std_msgs/msg/float64_multi_array.hpp"
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(),
"goal request recieved for gripper [%.6f m]", goal->position);
2023-11-09 15:39:19 +03:00
(void)uuid;
// 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
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);