Add minimal pick-and-place case with world embedded

rbs_skill_servers/src/gripper_control_action_server.cpp

@@ -0,0 +1,114 @@
+#include <functional>
+#include <memory>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+// action libs
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rbs_skill_interfaces/action/gripper_command.hpp"
+
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "sensor_msgs/msg/joint_state.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);
+            join_state_subscriber = this->create_subscription<sensor_msgs::msg::JointState>(
+                "/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1));
+        }
+
+
+    private:
+        
+        rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
+        rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
+        rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr join_state_subscriber;
+        double gripper_joint_state{1.0};
+
+        using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
+
+        void joint_state_callback(const sensor_msgs::msg::JointState & msg)
+        {
+            gripper_joint_state = msg.position.back();
+        }
+
+        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 [%.2f m]", goal->position);
+            (void)uuid;
+            if(goal->position > 0.9 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;
+            }
+            RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state);
+
+            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");
+
+        }
+    };
+}
+
+
+
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
+
+
+// int main(int argc, char ** argv) {
+//     rclcpp::init(argc, argv);
+//     rbs_skill_actions::GripperControlActionServer server;
+//     rclcpp::spin(server);
+//     return 0;
+// }