封装move_group节点为一个ros service，以实现move_group的开闭c++

好的，将move_group节点封装为一个ROS Service的步骤如下：

1. 首先，在ROS工作空间中创建一个新的ROS包，例如"move_group_service"。

2. 在该ROS包下创建一个名为"src"的文件夹，并在该文件夹下创建一个名为"move_group_service.cpp"的源文件。

3. 在move_group_service.cpp文件中，包含以下头文件：

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/MoveGroupAction.h>
#include <actionlib/client/simple_action_client.h>
#include <moveit_msgs/ExecuteTrajectoryAction.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit_msgs/RobotState.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/kinematic_constraints/utils.h>

4. 创建一个ROS Service，它将接收一个请求，其中包含要执行的运动规划的目标姿态和机器人的名称等信息。在move_group_service.cpp文件中，定义如下的ROS Service：

bool move_group_callback(moveit_msgs::MoveGroup::Request& req, moveit_msgs::MoveGroup::Response& res)
{
    static ros::NodeHandle nh;
    static const std::string PLANNING_GROUP = "manipulator";
    static moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    static moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    move_group.setPlannerId("RRTConnectkConfigDefault");

    move_group.setPoseReferenceFrame("base_link");

    move_group.setStartStateToCurrentState();

    geometry_msgs::Pose target_pose;
    target_pose.orientation.w = req.target_pose.orientation.w;
    target_pose.orientation.x = req.target_pose.orientation.x;
    target_pose.orientation.y = req.target_pose.orientation.y;
    target_pose.orientation.z = req.target_pose.orientation.z;

    target_pose.position.x = req.target_pose.position.x;
    target_pose.position.y = req.target_pose.position.y;
    target_pose.position.z = req.target_pose.position.z;

    move_group.setPoseTarget(target_pose);

    bool success = move_group.plan(my_plan);

    if (success)
    {
        res.success = true;
        move_group.move();
    }
    else
    {
        ROS_ERROR("Failed to plan");
        res.success = false;
    }

    return true;
}

5. 在main函数中，创建ROS节点和move_group服务，并将ROS Service注册到ROS Master上：

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

    ros::ServiceServer service = nh.advertiseService("/move_group_service", move_group_callback);

    ROS_INFO("Ready to receive move_group requests");

    ros::spin();
}

6. 编译ROS包并运行ROS节点：

$ catkin_make
$ roscore
$ rosrun move_group_service move_group_service

7. 使用ROS Service测试move_group_service节点：

$ rosservice call /move_group_service "target_pose:
position:
x: 0.5
y: 0.0
z: 0.5
orientation:
w: 1.0
x: 0.0
y: 0.0
z: 0.0"

这个ROS Service将使用move_group节点规划机器人的运动轨迹，使机器人移动到目标姿态。