95 lines
3.4 KiB
C++
95 lines
3.4 KiB
C++
|
|
||
|
|
#include <memory>
|
||
|
|
#include <string>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include "rclcpp/rclcpp.hpp"
|
||
|
|
#include "rclcpp_action/rclcpp_action.hpp"
|
||
|
|
#include "rclcpp_components/register_node_macro.hpp"
|
||
|
|
#include "std_msgs/msg/float64_multi_array.hpp"
|
||
|
|
#include "robossembler_interfaces/action/gripper_command.hpp"
|
||
|
|
|
||
|
|
using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
|
||
|
|
|
||
|
|
namespace robossembler_servers
|
||
|
|
{
|
||
|
|
class GripperCmd : public rclcpp::Node
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
|
||
|
|
explicit GripperCmd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
|
||
|
|
: Node("gripper_cmd", options)
|
||
|
|
{
|
||
|
|
using namespace std::placeholders;
|
||
|
|
this->action_server_ = rclcpp_action::create_server<GripperCmdAction>(
|
||
|
|
this,
|
||
|
|
"/gripper_cmd",
|
||
|
|
std::bind(&GripperCmd::goal_callback, this, _1, _2),
|
||
|
|
std::bind(&GripperCmd::cancel_callback, this, _1),
|
||
|
|
std::bind(&GripperCmd::accepted_callback, this, _1));
|
||
|
|
|
||
|
|
this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
|
||
|
|
("/rasmt_hand_controller/command", 10);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
|
||
|
|
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
|
||
|
|
|
||
|
|
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
|
||
|
|
|
||
|
|
rclcpp_action::GoalResponse goal_callback(
|
||
|
|
const rclcpp_action::GoalUUID & uuid,
|
||
|
|
std::shared_ptr<const GripperCmdAction::Goal> goal)
|
||
|
|
{
|
||
|
|
RCLCPP_INFO(this->get_logger(), "Received goal request [%.3f]", goal->value);
|
||
|
|
(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(&robossembler_servers::GripperCmd::execute, this, _1), goal_handle).detach();
|
||
|
|
}
|
||
|
|
|
||
|
|
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
|
||
|
|
{
|
||
|
|
RCLCPP_INFO(this->get_logger(), "Executing goal");
|
||
|
|
const auto goal = goal_handle->get_goal();
|
||
|
|
auto feedback = std::make_shared<GripperCmdAction::Feedback>();
|
||
|
|
auto result = std::make_shared<GripperCmdAction::Result>();
|
||
|
|
|
||
|
|
if (goal_handle->is_canceling()) {
|
||
|
|
goal_handle->canceled(result);
|
||
|
|
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
std_msgs::msg::Float64MultiArray commands;
|
||
|
|
RCLCPP_INFO(this->get_logger(), "Initialize commands");
|
||
|
|
commands.data.push_back(goal->value);
|
||
|
|
commands.data.push_back(goal->value);
|
||
|
|
//commands.data[0] = goal->value;
|
||
|
|
//commands.data[1] = goal->value;
|
||
|
|
RCLCPP_INFO(this->get_logger(), "Publish commands");
|
||
|
|
this->gripper_cmd_publisher_->publish(commands);
|
||
|
|
|
||
|
|
goal_handle->succeed(result);
|
||
|
|
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
} // namespace robossembler_servers
|
||
|
|
|
||
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_servers::GripperCmd)
|