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>

int main(int argc, char** argv) {
ros::init(argc, argv, "moveit_demo");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

//定义需要使用的move group
moveit::planning_interface::MoveGroupInterface move_group("manipulator");

//设置机械臂终端的目标位置
geometry_msgs::Pose target_pose;
target_pose.orientation.w = 1.0;
target_pose.position.x = 0.5;
target_pose.position.y = 0.5;
target_pose.position.z = 0.5;
move_group.setPoseTarget(target_pose);

//规划并执行运动
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
if(success)
move_group.move();

//获取机械臂的当前位姿
geometry_msgs::PoseStamped current_pose = move_group.getCurrentPose();
ROS_INFO("Current pose:\n x:%f\n y:%f\n z:%f", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);

ros::shutdown();
return 0;
}