base for grasp_interface.cpp is finished
now defining the task
This commit is contained in:
parent
c976f7242a
commit
f4313063e5
@ -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
|
||||
|
||||
@ -20,6 +20,7 @@
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>franka_cps_msgs</depend>
|
||||
<depend>franka_msgs</depend>
|
||||
|
||||
<depend>eigen</depend>
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
|
||||
|
||||
void doTask();
|
||||
|
||||
void setupPlanningScene();
|
||||
using GraspSequence = franka_cps_msgs::action::GraspSequence;
|
||||
using GoalHandleGraspSequence = rclcpp_action::ServerGoalHandle<GraspSequence>;
|
||||
using FrankaMove = franka_msgs::action::Move;
|
||||
using FrankaGrasp = franka_msgs::action::Grasp;
|
||||
|
||||
GraspInterfaceNode()
|
||||
: 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();
|
||||
mtc::Task task_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
}
|
||||
rclcpp_action::Server<GraspSequence>::SharedPtr action_server_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
|
||||
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)
|
||||
: node_{ std::make_shared<rclcpp::Node>("grasp_interface_node", options)}
|
||||
{
|
||||
}
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
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)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user