#include "mtc_tutorial/mtc_glass_bottle.hpp"
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_glass_right"); // 获取节点基础接口的实现
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode_Right::getNodeBaseInterface()
{return node_->get_node_base_interface();
}// 构造函数的实现，创建节点对象
MTCTaskNode_Right::MTCTaskNode_Right(const rclcpp::NodeOptions& options): node_{ std::make_shared<rclcpp::Node>("mtc_right_node", options) }  // 初始化节点名称为 "mtc_node"
{
}// 设置规划场景的实现
void MTCTaskNode_Right::setupPlanningScene()
{const double table_height = 1.02;std::string frame_id = "world";geometry_msgs::msg::PoseStamped bottlePose;bottlePose.header.frame_id = frame_id;bottlePose.pose.position.x = 0.3;bottlePose.pose.position.y = -0.5;bottlePose.pose.position.z = table_height;bottlePose.pose.orientation.w = 1.0;geometry_msgs::msg::PoseStamped glassPose;glassPose.header.frame_id = frame_id;glassPose.pose.position.x = -0.6;glassPose.pose.position.y = -0.5;glassPose.pose.position.z = table_height;glassPose.pose.orientation.w = 1.0;// center of table surface geometry_msgs::msg::PoseStamped tabletopPose;tabletopPose.header.frame_id = frame_id;tabletopPose.pose.position.x = 0;tabletopPose.pose.position.y = -0.25;tabletopPose.pose.position.z = table_height;tabletopPose.pose.orientation.w = 1.0;// 创建规划场景接口并将碰撞对象添加到场景中mtc_pour::cleanup();moveit::planning_interface::PlanningSceneInterface psi;std::vector<moveit_msgs::msg::CollisionObject> objs;mtc_pour::setupTable(objs, tabletopPose);mtc_pour::setupObjects(objs, bottlePose, glassPose,"package://mtc_pour/meshes/small_bottle.stl");psi.applyCollisionObjects(objs);
}// 执行任务的实现
void MTCTaskNode_Right::doTask()
{task_ = createTask();  // 创建任务// 尝试初始化任务try{task_.init();  // 初始化任务}catch (mtc::InitStageException& e)  // 捕获初始化失败的异常{RCLCPP_ERROR_STREAM(LOGGER, e);  // 打印错误信息return;}// 规划任务路径，最大规划尝试次数为 5if (!task_.plan(10)){RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");  // 打印任务规划失败的日志return;}// 发布任务解决方案以供检查task_.introspection().publishSolution(*task_.solutions().front());// 执行规划的解决方案auto result = task_.execute(*task_.solutions().front());if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS){RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");  // 执行失败的错误日志return;}return;
}// 创建任务的实现
mtc::Task MTCTaskNode_Right::createTask()
{mtc::Task task;  // 创建一个 MTC 任务对象task.stages()->setName("grabbing the glass on the right");  // 为任务命名task.loadRobotModel(node_);  // 加载机器人模型// 定义机器人操作相关的参数// const auto& left_arm_group_name = "left_ur_manipulator";  // 机械臂的操作组名称// const auto& left_hand_group_name = "left_robotiq_2f_85_gripper";  // 手部操作组名称// const auto& left_hand_frame = "left_robotiq_85_base_link";  // 机械手的坐标系const auto& right_arm_group_name = "right_ur_manipulator";  // 机械臂的操作组名称const auto& right_hand_group_name = "right_robotiq_2f_85_gripper";  // 手部操作组名称const auto& right_hand_frame = "right_grasp_point";  // 机械手的坐标系// 设置任务的属性task.setProperty("group", right_arm_group_name);task.setProperty("eef", "right_robotiq_2f_85_gripper_ee");task.setProperty("ik_frame", right_hand_frame);// 忽略未使用变量的编译器警告
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"mtc::Stage* current_state_ptr = nullptr;  // 将current_stage传递给grasp pose generator
#pragma GCC diagnostic pop// 创建并添加获取当前状态的阶段auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("right-current");current_state_ptr = stage_state_current.get();task.add(std::move(stage_state_current));  // 添加当前状态阶段到任务中// 创建不同的规划器auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);  // 创建采样规划器auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();  // 创建插值规划器// 创建笛卡尔路径规划器，并设置最大速度和加速度auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();cartesian_planner->setMaxVelocityScalingFactor(1.0);  // 设置最大速度缩放因子为 1.0cartesian_planner->setMaxAccelerationScalingFactor(1.0);  // 设置最大加速度缩放因子为 1.0cartesian_planner->setStepSize(.01);  // 设置步长为 0.01// 创建并添加手部动作的阶段（打开手）auto stage_open_right_hand =std::make_unique<mtc::stages::MoveTo>("open right_hand", interpolation_planner);  // 创建移动到目标状态的阶段stage_open_right_hand->setGroup(right_hand_group_name);  // 设置操作组为手部stage_open_right_hand->setGoal("right_open");  // 设置手部打开的目标状态task.add(std::move(stage_open_right_hand));  // 将阶段添加到任务中// Add the next lines of codes to define more stages hereauto stage_move_to_pick_right = std::make_unique<mtc::stages::Connect>("move to pick-right",mtc::stages::Connect::GroupPlannerVector{{right_arm_group_name,sampling_planner}});stage_move_to_pick_right->setTimeout(5.0);stage_move_to_pick_right->properties().configureInitFrom(mtc::Stage::PARENT);task.add(std::move(stage_move_to_pick_right));mtc::Stage* attach_object_stage_right = nullptr;// Forward attach_object_stage  to place pose generator{auto grasp_right = std::make_unique<mtc::SerialContainer>("pick glass-right");task.properties().exposeTo(grasp_right->properties(),{"eef","group","ik_frame"});grasp_right->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group","ik_frame"});{auto stage = std::make_unique<mtc::stages::MoveRelative>("approach glass-right",cartesian_planner);stage->properties().set("marker_ns","approach_glass-right");stage->properties().set("link",right_hand_frame);// TODO:猜测这是与物体发生接触的linkstage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});stage->setMinMaxDistance(0.05,0.15);// Set hand forward directiongeometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = right_hand_frame;vec.vector.z =1.0;stage->setDirection(vec);grasp_right->insert(std::move(stage));}{//Sample grasp poseauto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose -right");stage->properties().configureInitFrom(mtc::Stage::PARENT);stage->properties().set("marker_ns","grasp_pose-right");stage->setPreGraspPose("right_open");stage->setObject("glass");stage->setAngleDelta(M_PI / 12);stage->setMonitoredStage(current_state_ptr);//Hook into current stateEigen::Isometry3d grasp_frame_transform_right;//末端执行器坐标系的Z轴朝向杯子的X轴，末端执行器坐标系的X轴朝向杯子的Y轴，末端执行器坐标系的Y轴朝向杯子的Z轴Eigen::Quaterniond q = Eigen:: AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitX());// //末端执行器坐标系的Y轴朝向杯子的X轴，末端执行器坐标系的X轴朝向杯子的Y轴,末端执行器坐标系的Z轴朝向杯子的-Z轴// Eigen::Quaterniond q = Eigen:: AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ())*//                       Eigen::AngleAxisd(-M_PI,Eigen::Vector3d::UnitX());grasp_frame_transform_right.linear() = q.matrix();grasp_frame_transform_right.translation().z()=0;// Compute IK// 将stage对象的所有权转移给wrapper_right// wrapper 通常指的是一层封装，用于将某个功能或一组功能封装起来，auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK - right",std::move(stage));wrapper->setMaxIKSolutions(8);wrapper->setMinSolutionDistance(1.0);wrapper->setIKFrame(grasp_frame_transform_right,right_hand_frame);wrapper->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group"});wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE,{"target_pose"});grasp_right->insert(std::move(wrapper));}{auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object) - right");stage->allowCollisions("glass",task.getRobotModel()->getJointModelGroup(right_hand_group_name)->getLinkModelNamesWithCollisionGeometry(),true);grasp_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::MoveTo>("close hand-right",interpolation_planner);stage->setGroup(right_hand_group_name);stage->setGoal("right_close");grasp_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attcah object - right");stage->attachObject("glass",right_hand_frame);attach_object_stage_right = stage.get();grasp_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::MoveRelative>("lift object - right",cartesian_planner);stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});stage->setMinMaxDistance(0.1,0.3);stage->setIKFrame(right_hand_frame);stage->properties().set("marker_ns","lift_object_right");// Set upward directiongeometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = "dual_base";vec.vector.y = 1.0;stage->setDirection(vec);grasp_right->insert(std::move(stage));}task.add(std::move(grasp_right));  }{auto stage_move_to_place_right = std::make_unique<mtc::stages::Connect>("move to place -right",mtc::stages::Connect::GroupPlannerVector{{right_arm_group_name,sampling_planner},{right_hand_group_name,sampling_planner} });stage_move_to_place_right->setTimeout(5.0);stage_move_to_place_right->properties().configureInitFrom(mtc::Stage::PARENT);task.add(std::move(stage_move_to_place_right));}{auto place_right = std::make_unique<mtc::SerialContainer>("place object -right");task.properties().exposeTo(place_right->properties(),{"eef","group","ik_frame"});place_right->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group","ik_frame"});// {//   //Sample place pose//   auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose");//   stage->properties().configureInitFrom(mtc::Stage::PARENT);//   stage->properties().set("marker_ns","place_pose_right");//   stage->setObject("glass");//   geometry_msgs::msg::PoseStamped target_pose_msg_right;//   target_pose_msg_right.header.frame_id = "dual_base";//   target_pose_msg_right.pose.position.x = -0.5;//   target_pose_msg_right.pose.position.y = -0.5;//   target_pose_msg_right.pose.position.z = 0.55;//   tf2::Quaternion qtn;//   qtn.setRPY(0.0,0.0,-1.57);//   target_pose_msg_right.pose.orientation.x = qtn.x();//   target_pose_msg_right.pose.orientation.y = qtn.y();//   target_pose_msg_right.pose.orientation.z = qtn.z();//   target_pose_msg_right.pose.orientation.w = qtn.w();//   stage->setPose(target_pose_msg_right);//   stage->setMonitoredStage(attach_object_stage_right); //Hook into attach_object_stage//   // Compute IK//   auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK",std::move(stage));//   wrapper->setMaxIKSolutions(2);//   wrapper->setMinSolutionDistance(1.0);//   wrapper->setIKFrame("glass");//   wrapper->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group"});//   wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE,{"target_pose"});//   place_right->insert(std::move(wrapper));//  }//  {//     //Sample place pose//     auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose -right");//     stage->properties().configureInitFrom(mtc::Stage::PARENT);//     stage->properties().set("marker_ns","place_pose_right");//     stage->setObject("glass");//     // 定义放置位姿//     geometry_msgs::msg::PoseStamped target_pose_msg_right;//     target_pose_msg_right.header.frame_id = "dual_base";//     target_pose_msg_right.pose.position.x = -0.5;//     target_pose_msg_right.pose.position.y = 0.5;//     target_pose_msg_right.pose.position.z = 0.2;//     tf2::Quaternion qtn;//     qtn.setRPY(0.0, 0.0, -M_PI );  // 将RPY角度转换成四元数//     target_pose_msg_right.pose.orientation.x = qtn.x();//     target_pose_msg_right.pose.orientation.y = qtn.y();//     target_pose_msg_right.pose.orientation.z = qtn.z();//     target_pose_msg_right.pose.orientation.w = qtn.w();//     stage->setPose(target_pose_msg_right);//     stage->setMonitoredStage(attach_object_stage_right); //Hook into attach_object_stage//     // 定义放置时的末端执行器坐标系变换//     Eigen::Isometry3d place_frame_transform_right;//     Eigen::Quaterniond q = Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY()) *//                           Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitX());//     place_frame_transform_right.linear() = q.matrix();//     place_frame_transform_right.translation().z() = 0;//     // 计算IK//     auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK - right", std::move(stage));//     wrapper->setMaxIKSolutions(8); // 设置最大解的数量//     wrapper->setMinSolutionDistance(1.0);//     wrapper->setIKFrame(place_frame_transform_right, right_hand_frame);//     wrapper->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group"});//     wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, {"target_pose"});//     place_right->insert(std::move(wrapper));// }{// Sample place poseauto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose -right");stage->properties().configureInitFrom(mtc::Stage::PARENT);stage->properties().set("marker_ns", "place_pose_right");stage->setObject("glass");// 定义放置位姿geometry_msgs::msg::PoseStamped target_pose_msg_right;target_pose_msg_right.header.frame_id = "dual_base";target_pose_msg_right.pose.position.x = -0.3;target_pose_msg_right.pose.position.y = -0.5;target_pose_msg_right.pose.position.z = 0.3;// 计算放置位姿的正确姿态Eigen::Quaterniond q_place = Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());Eigen::Quaterniond q_object = q_place.inverse();tf2::Quaternion qtn(q_object.x(), q_object.y(), q_object.z(), q_object.w());target_pose_msg_right.pose.orientation = tf2::toMsg(qtn);stage->setPose(target_pose_msg_right);stage->setMonitoredStage(attach_object_stage_right); // Hook into attach_object_stage// 设置 IK 框架，与抓取阶段一致Eigen::Isometry3d place_frame_transform_right;place_frame_transform_right.linear() = q_place.toRotationMatrix();place_frame_transform_right.translation().z() = 0;// 计算 IKauto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK - right", std::move(stage));wrapper->setMaxIKSolutions(8); // 设置最大解的数量wrapper->setMinSolutionDistance(1.0);wrapper->setIKFrame(place_frame_transform_right, right_hand_frame);wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });place_right->insert(std::move(wrapper));}{auto stage = std::make_unique<mtc::stages::MoveTo>("open hand-right",interpolation_planner);stage->setGroup(right_hand_group_name);stage->setGoal("right_open");place_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (hand,object)-right");stage->allowCollisions("glass",task.getRobotModel()->getJointModelGroup(right_hand_group_name)->getLinkModelNamesWithCollisionGeometry(),false);place_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object-right");stage->detachObject("glass",right_hand_frame);place_right->insert(std::move(stage));}{auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat -right",cartesian_planner);stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});stage->setMinMaxDistance(0.1,0.3);stage->setIKFrame(right_hand_frame);stage->properties().set("marker_ns","retreat-right");// Set retreat directiongeometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = "world";vec.vector.z=0.5;stage->setDirection(vec);place_right->insert(std::move(stage));}task.add(std::move(place_right));}{auto stage = std::make_unique<mtc::stages::MoveTo>("return home -right",interpolation_planner);stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});stage->setGoal("right_home");task.add(std::move(stage));}// Stages all added to the task above this linereturn task;  // 返回创建的任务
}