Migrate to Gazebo Fortress

rbs_skill_servers/src/move_topose_action_server.cpp

@@ -0,0 +1,163 @@
+#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/msg/action_feedback_status_constants.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/transform.hpp"
+
+// moveit libs
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "moveit/planning_interface/planning_interface.h"
+#include "moveit/robot_model_loader/robot_model_loader.h"
+
+/*
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+*/
+// For Visualization
+//#include <eigen_conversions/eigen_msg.h>
+#include "moveit_msgs/msg/display_robot_state.hpp"
+#include "moveit_msgs/msg/display_trajectory.hpp"
+#include "moveit_msgs/msg/robot_trajectory.hpp"
+#include "moveit_msgs/action/move_group.hpp"
+
+namespace rbs_skill_actions
+{
+
+class MoveToPoseActionServer : public rclcpp::Node
+{
+public:
+    using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+
+    //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
+    explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node)
+    {
+        // using namespace std::placeholders;
+        // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
+        //     this->get_node_base_interface(),
+        //     this->get_node_clock_interface(),
+        //     this->get_node_logging_interface(),
+        //     this->get_node_waitables_interface(),
+        //     "move_topose",
+        //     std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
+        //     std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
+        //     std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
+    }
+
+    void init()
+    {
+        
+        action_server_ = rclcpp_action::create_server<MoveitSendPose>(
+            node_->get_node_base_interface(),
+            node_->get_node_clock_interface(),
+            node_->get_node_logging_interface(),
+            node_->get_node_waitables_interface(),
+            "move_topose",
+            std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
+            std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1),
+            std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1)
+        );
+
+    }
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
+
+    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
+
+    rclcpp_action::GoalResponse goal_callback(
+        const rclcpp_action::GoalUUID & uuid,
+        std::shared_ptr<const MoveitSendPose::Goal> goal)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", 
+            goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z,
+            goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, 
+            goal->target_pose.orientation.w);
+        (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(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach();
+    }
+    
+    void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
+    {
+        RCLCPP_INFO(this->get_logger(), "Executing action goal");
+        const auto goal = goal_handle->get_goal();
+        auto result = std::make_shared<MoveitSendPose::Result>();
+        
+        moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
+        
+        move_group_interface.setStartState(*move_group_interface.getCurrentState());
+        move_group_interface.setPoseTarget(goal->target_pose);
+        move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
+        move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
+
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
+        if(success)
+        {
+            RCLCPP_INFO(this->get_logger(), "Planning success");
+            //move_group_interface.execute(plan);
+            move_group_interface.move();
+        }else{
+            RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
+        }
+
+        if (goal_handle->is_canceling()) {
+            goal_handle->canceled(result);
+            RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
+            return;
+        }
+    
+    
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
+    }
+}; // class MoveToPoseActionServer
+
+}// namespace rbs_skill_actions
+
+int main(int argc, char ** argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions node_options;
+    node_options.automatically_declare_parameters_from_overrides(true);
+    auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
+
+    rbs_skill_actions::MoveToPoseActionServer server(node);
+    std::thread run_server([&server]() {
+        rclcpp::sleep_for(std::chrono::seconds(3));
+        server.init();
+    });
+
+    rclcpp::spin(node);
+    run_server.join();
+
+    return 0;
+}