moveit获取末端姿态的C++程序

以下是一个使用MoveIt获取末端姿态的C程序的基本示例：

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_demo");
    ros::NodeHandle nh;

    // 创建MoveGroup接口
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");

    // 设置目标终端姿态
    geometry_msgs::PoseStamped target_pose;
    target_pose.header.frame_id = "base_link";
    target_pose.pose.position.x = 0.5;
    target_pose.pose.position.y = 0.0;
    target_pose.pose.position.z = 0.5;
    tf2::Quaternion q;
    q.setRPY(0,0,1.57); // 绕z轴旋转90度
    target_pose.pose.orientation = tf2::toMsg(q);

    // 设置终端姿态为目标姿态
    move_group.setPoseTarget(target_pose);

    // 进行规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    // 移动到目标姿态
    if(success) {
        move_group.move();
        ROS_INFO("Move to target pose successfully.");
    } else {
        ROS_ERROR("Failed to move to target pose!");
    }

    // 获取当前终端姿态
    geometry_msgs::PoseStamped current_pose = move_group.getCurrentPose();

    // 输出当前终端姿态
    ROS_INFO_STREAM("Current end effector pose:\n" << current_pose.pose);

    return 0;
}

该程序使用MoveIt来规划机械臂的运动，并获取当前机械臂的末端姿态。首先，创建一个MoveGroup接口并设置目标终端姿态。然后，使用MoveIt进行规划并执行运动。最后，获取当前终端姿态并输出。