base for grasp_interface.cpp is finished

now defining the task

CMakeLists.txt

@@ -28,6 +28,7 @@ find_package(moveit_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(franka_cps_msgs REQUIRED)
+find_package(franka_msgs REQUIRED)
 
 find_package(tf2)
 find_package(tf2_ros)
@@ -57,6 +58,7 @@ ament_target_dependencies(
   moveit_ros_planning_interface
   moveit_task_constructor_core
   moveit_msgs
+  franka_msgs
   franka_cps_msgs
   std_msgs
   geometry_msgs

package.xml

@@ -20,6 +20,7 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>franka_cps_msgs</depend>
+  <depend>franka_msgs</depend>
 
   <depend>eigen</depend>
 

src/grasp_interface.cpp

@@ -6,13 +6,20 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
+
 #include <franka_cps_msgs/action/grasp_sequence.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <franka_msgs/action/move.hpp>
+#include <franka_msgs/action/grasp.hpp>
 
 #include <moveit/planning_scene/planning_scene.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/task_constructor/task.h>
-#include <moveit/task_constructor/solvers.h>
 #include <moveit/task_constructor/stages.h>
+
+#include <moveit/task_constructor/solvers.h>
+
+#include "tf2_ros/transform_listener.h"
+
 #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #else
@@ -27,34 +34,194 @@
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_grasp_interface");
 namespace mtc = moveit::task_constructor;
 
-class GraspInterfaceNode
+class GraspInterfaceNode : public rclcpp::Node
 {
     public:
-        GraspInterfaceNode(const rclcpp::NodeOptions& options);
+        using GraspSequence = franka_cps_msgs::action::GraspSequence;
-    
+        using GoalHandleGraspSequence = rclcpp_action::ServerGoalHandle<GraspSequence>;
-        rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
+        using FrankaMove = franka_msgs::action::Move;
-    
+        using FrankaGrasp = franka_msgs::action::Grasp;
-        void doTask();
+
-    
+        GraspInterfaceNode()
-        void setupPlanningScene();
+        : Node("grasp_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_)
+        {
+            // Action server for getting new Grasp Sequences
+            action_server_ = rclcpp_action::create_server<GraspSequence>(
+                this,
+                "franka_grasp_moveit/grasp",
+                std::bind(&GraspInterfaceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+                std::bind(&GraspInterfaceNode::handle_cancel, this, std::placeholders::_1),
+                std::bind(&GraspInterfaceNode::handle_accepted, this, std::placeholders::_1));
+
+            // Publisher for pose
+            pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("franka_grasp_moveit/current_pose", 10);
+
+            // Service for IK
+            
+
+            // TF2
+            tf2_ros::Buffer tf_buffer_(this->get_clock());
+            tf2_ros::TransformListener tf_listener_(tf_buffer_);
+            tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&GraspInterfaceNode::compute_endeffector_pose, this));
+
+            // Setup for move groups
+            static const std::string arm_group_name = "panda_arm";
+            static const std::string  hand_group_name = "hand";
+            static const std::string  hand_frame = "panda_hand";            
+        }
 
-        rclcpp_action::create_client<franka_cps_msgs::action::GraspSequence>(this, "grasp_interface/grasp");
     private:
-        mtc::Task createTask();
+        rclcpp_action::Server<GraspSequence>::SharedPtr action_server_;
-        mtc::Task task_;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
-        rclcpp::Node::SharedPtr node_;
+        tf2_ros::Buffer tf_buffer_;
-}
+        tf2_ros::TransformListener tf_listener_;
+        rclcpp::TimerBase::SharedPtr tf2_timer_;
 
+        void compute_endeffector_pose()
+        {
+            try
+            {
+                geometry_msgs::msg::TransformStamped transform_stamped;
+                transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
+                
+                geometry_msgs::msg::PoseStamped current_pose;
+                current_pose.header.frame_id = "panda_link8";
+                current_pose.header.stamp = transform_stamped.header.stamp;
+                //Position
+                current_pose.pose.position.x = transform_stamped.transform.translation.x;
+                current_pose.pose.position.y = transform_stamped.transform.translation.y;
+                current_pose.pose.position.z = transform_stamped.transform.translation.z;
+                // Orientation
+                current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
+                current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
+                current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
+                current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
+                pose_publisher_->publish(current_pose);
+            }
+            catch (tf2::TransformException &ex)
+            {
+                RCLCPP_WARN(this->get_logger(), "%s", ex.what());
+            }
+        }        
 
-GrapsInterfaceNode::GrapsInterfaceNode(const rclccp::NodeOptions& options)
+        rclcpp_action::GoalResponse handle_goal(
-    : node_{ std::make_shared<rclcpp::Node>("grasp_interface_node", options)}
+            const rclcpp_action::GoalUUID & uuid,
-{
+            std::shared_ptr<const GraspSequence::Goal> goal)
-}
+        {
+            RCLCPP_INFO(this->get_logger(), "Received goal request!");
+            (void)uuid;
+            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+        }
+
+        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleGraspSequence> goal_handle)
+        {
+            RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+            (void)goal_handle;
+            return rclcpp_action::CancelResponse::ACCEPT;
+        }   
+        
+        void handle_accepted(const std::shared_ptr<GoalHandleGraspSequence> goal_handle) 
+        {
+        // This needs to run in a separate thread to avoid blocking the executor
+        std::thread([this, goal_handle]() {
+            const auto goal = goal_handle->get_goal();
+            auto result = std::make_shared<GraspSequence::Result>();
+
+            // Generate and execute the task
+            try {
+                auto task = createTask(*goal);
+                auto success = executeTask(task);
+                
+                if (success) {
+                    result->result_msg = "Execution successful";
+                    result->error_code = 0; // Success code
+                    goal_handle->succeed(result);
+                } else {
+                    result->result_msg = "Execution failed";
+                    result->error_code = 1; // Generic failure code
+                    goal_handle->abort(result);
+                }
+            } catch (const std::exception& e) {
+                RCLCPP_ERROR(this->get_logger(), "Execution exception: %s", e.what());
+                result->result_msg = std::string("Execution exception: ") + e.what();
+                result->error_code = 2; // Exception code
+                goal_handle->abort(result);
+            }
+        }).detach();
+        }
+        
+        // Generating a task
+        auto createTask(const GraspSequence::Goal& goal) -> moveit::task_constructor::Task {
+            moveit::task_constructor::Task task;
+            task.stages()->setName("Grasp Task");
+            task.loadRobotModel(this->shared_from_this());
+
+            // set Planners
+            auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
+
+            std::vector<double> joint_positions;
+            /*
+            // Compute IK for each pose and create corresponding stages
+            if (computeIK(goal->hover, joint_positions)) {
+                // Create and add MoveTo stage using computed joint positions
+                auto move_to_hover = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
+                move_to_hover->setGroup(arm_group_name); // set group 
+                move_to_hover->setGoal(joint_positions); // set goal
+                task.add(std::move(move_to_hover));      // add Task
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
+            }
+
+            if (computeIK(goal->grasp, joint_positions)) {
+                auto move_to_hover = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
+                move_to_hover->setGroup(arm_group_name); // set group 
+                move_to_hover->setGoal(joint_positions); // set goal
+                task.add(std::move(move_to_hover));      // add Task
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
+            }
+
+            if (computeIK(goal->hover, joint_positions)) {
+                // Create and add MoveTo stage using computed joint positions
+                auto move_to_hover2 = std::make_unique<mtc::stages::MoveTo>("Move to Hover", interpolation_planner);
+                move_to_hover2->setGroup(arm_group_name); // set group 
+                move_to_hover2->setGoal(joint_positions); // set goal
+                task.add(std::move(move_to_hover2));      // add Task
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
+            }
+
+            if (computeIK(goal->drop, joint_positions)) {
+                // Create and add MoveTo stage using computed joint positions
+                auto move_to_drop = std::make_unique<mtc::stages::MoveTo>("Move to drop", interpolation_planner);
+                move_to_drop->setGroup(arm_group_name); // set group 
+                move_to_drop->setGoal(joint_positions); // set goal
+                task.add(std::move(move_to_drop));      // add Task
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for drop pose");
+            }
+
+            if (computeIK(goal->idle, joint_positions)) {
+                // Create and add MoveTo stage using computed joint positions
+                auto move_to_idle = std::make_unique<mtc::stages::MoveTo>("Move to idle", interpolation_planner);
+                move_to_idle->setGroup(arm_group_name); // set group 
+                move_to_idle->setGoal(joint_positions); // set goal
+                task.add(std::move(move_to_idle));      // add Task
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for idle pose");
+            }
+            */
+
+            return task;
+        }
+
+        bool executeTask(moveit::task_constructor::Task& task) {
+            // Logic to execute the given task
+            // Return true if successful, false otherwise
+            return 1;
+        }
+};
 
-rclcpp::node_interfaces::NodeBaseInterface::SharedPtr GraspInterfaceNode::getNodeBaseInterface()
-{
-    return node_->get_node_base_interface();
-}
 
 int main(int argc, char** argv)
 {