runtime/robossembler_servers/src/move_cartesian_path_action_server.cpp

#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 "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
#include "robossembler_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 robossembler_actions
{

class MoveCartesianActionServer : public rclcpp::Node
{
public:
    using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;

    //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) 
    explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_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(&MoveCartesianActionServer::goal_callback, this, _1, _2),
        //     std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
        //     std::bind(&MoveCartesianActionServer::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_cartesian",
            std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1),
            std::bind(&MoveCartesianActionServer::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(&MoveCartesianActionServer::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);


        moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
        auto start_pose = move_group_interface.getCurrentPose();

        std::vector<geometry_msgs::msg::Pose> waypoints;
        RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]", 
        start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z,
            start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z, 
            start_pose.pose.orientation.w);
        //waypoints.push_back(start_pose.pose);
        geometry_msgs::msg::Pose target_pose = start_pose.pose;
        //goal->target_pose
        target_pose.position.z = target_pose.position.z + goal->target_pose.position.z;
        target_pose.position.y = target_pose.position.y + goal->target_pose.position.y;
        target_pose.position.x = target_pose.position.x + goal->target_pose.position.x;
        waypoints.push_back(target_pose);

        RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", 
        target_pose.position.x, target_pose.position.y, target_pose.position.z);
        //waypoints.push_back(start_pose.pose);
        moveit_msgs::msg::RobotTrajectory trajectory;
        const double jump_threshold = 0.0;
        const double eef_step = 0.01;
        double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        waypoints.clear();
        if(fraction>0)
        {
            RCLCPP_INFO(this->get_logger(), "Planning success");
            move_group_interface.execute(trajectory);
            //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 MoveCartesianActionServer

}// namespace robossembler_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_cartesian", "", node_options);

    robossembler_actions::MoveCartesianActionServer server(node);
    std::thread run_server([&server]() {
        rclcpp::sleep_for(std::chrono::seconds(3));
        server.init();
    });

    rclcpp::spin(node);
    run_server.join();

    return 0;
}