【moveit2】实时机械臂伺服Realtime Arm Servoing

MoveIt Servo 允许您将末端执行器 (EEF)速度命令流式传输到您的操纵器并让它同时执行它们。这可以通过广泛的输入方案实现远程操作，或让其他自主软件控制机器人 - 例如在视觉伺服或闭环位置控制中。

本教程展示了如何使用 MoveIt Servo 向支持 ROS 的机器人发送实时伺服命令。伺服节点的一些不错的功能是奇点处理和碰撞检查，可防止操作员破坏机器人。

Launching a Standalone Servo Node

MoveIt Servo 提供了一个组件节点 moveit_servo::ServoServer，它允许您通过 ROS 主题发送命令。命令可以来自任何地方，例如操纵杆、键盘或其他控制器。

此演示demo是为 Xbox 1 控制器编写的，但可以通过修改 joystick_servo_example.cpp（https://github.com/ros-planning/moveit2/blob/foxy/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp） 文件轻松修改为使用与 Joy 包兼容的任何控制器.

要运行演示，请确保您的控制器已插入并且可以被 ros2 run joy joy_node 检测到。通常这会在插入控制器后自动发生。然后启动

ros2 launch moveit2_tutorials servo_teleop.launch.py

您现在应该可以使用控制器控制手臂，MoveIt Servo 会自动避免奇异点和碰撞.

---

---

Without a Controller

如果您没有控制器，您仍然可以使用键盘尝试演示。在demo仍在运行的情况下，在新终端中运行

ros2 run moveit2_tutorials servo_keyboard_input

您将能够使用键盘来伺服机器人。使用箭头键和 . 和 ; 键。将笛卡尔命令框架更改为 W 代表世界，E 代表末端执行器。使用键 1-7 发送关节慢跑命令（使用 R 反转方向）

Expected Output 

请注意，此处的控制器覆盖仅用于演示目的，实际上并未包含在内。

---

---

Using the C++ Interface

您可以通过 C++ 接口将 Servo包含在您自己的节点中，而不是将 Servo 作为自己的组件启动。在这两种情况下，向机器人发送命令非常相似，但对于 C++ 接口，需要对 Servo 进行一些设置。作为交换，您将能够通过其 C++ API 直接与 Servo 交互。

这个基本的 C++ 接口演示以预定的方式移动机器人，并且可以启动

ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py

应该会出现一个带有 Panda 手臂和碰撞对象的 Rviz 窗口。在切换到笛卡尔运动之前，手臂将关节慢跑几秒钟。当手臂接近碰撞对象时，它会减速并停止。

Expected Output

Entire Code

*********************************************************************/


/*      Title     : servo_cpp_interface_demo.cpp
 *      Project   : moveit2_tutorials
 *      Created   : 07/13/2020
 *      Author    : Adam Pettinger
 */


// ROS
#include <rclcpp/rclcpp.hpp>


// Servo
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>


using namespace std::chrono_literals;


static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit2_tutorials.servo_demo_node.cpp");


// BEGIN_TUTORIAL


// Setup
// ^^^^^
// First we declare pointers to the node and publisher that will publish commands to Servo
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
size_t count_ = 0;


// BEGIN_SUB_TUTORIAL publishCommands
// Here is the timer callback for publishing commands. The C++ interface sends commands through internal ROS topics,
// just like if Servo was launched using ServoServer.
void publishCommands()
{
  // First we will publish 100 joint jogging commands. The :code:`joint_names` field allows you to specify individual
  // joints to move, at the velocity in the cooresponding :code:`velocities` field. It is important that the message
  // contains a recent timestamp, or Servo will think the command is stale and will not move the robot.
  if (count_ < 100)
  {
    auto msg = std::make_unique<control_msgs::msg::JointJog>();
    msg->header.stamp = node_->now();
    msg->joint_names.push_back("panda_joint1");
    msg->velocities.push_back(0.3);
    joint_cmd_pub_->publish(std::move(msg));
    ++count_;
  }
    // After a while, we switch to publishing twist commands. The provided frame is the frame in which the twist is
  // defined, not the robot frame that will follow the command. Again, we need a recent timestamp in the message
  else
  {
    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = "panda_link0";
    msg->twist.linear.x = 0.3;
    msg->twist.angular.z = 0.5;
    twist_cmd_pub_->publish(std::move(msg));
  }
}
// END_SUB_TUTORIAL


// Next we will set up the node, planning_scene_monitor, and collision object
int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.use_intra_process_comms(true);
  node_ = std::make_shared<rclcpp::Node>("servo_demo_node", node_options);


  // Pause for RViz to come up. This is necessary in an integrated demo with a single launch file
  rclcpp::sleep_for(std::chrono::seconds(4));


  // Create the planning_scene_monitor. We need to pass this to Servo's constructor, and we should set it up first
  // before initializing any collision objects
  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node_, "robot_description", tf_buffer, "planning_scene_monitor");


  // Here we make sure the planning_scene_monitor is updating in real time from the joint states topic
  if (planning_scene_monitor->getPlanningScene())
  {
    planning_scene_monitor->startStateMonitor("/joint_states");
    planning_scene_monitor->setPlanningScenePublishingFrequency(25);
    planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                         "/moveit_servo/publish_planning_scene");
    planning_scene_monitor->startSceneMonitor();
  }
  else
  {
    RCLCPP_ERROR(LOGGER, "Planning scene not configured");
    return EXIT_FAILURE;
  }


  // These are the publishers that will send commands to MoveIt Servo. Two command types are supported: JointJog
  // messages which will directly jog the robot in the joint space, and TwistStamped messages which will move the
  // specified link with the commanded Cartesian velocity. In this demo, we jog the end effector link.
  joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("servo_demo_node/delta_joint_cmds", 10);
  twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("servo_demo_node/delta_twist_cmds", 10);


  // Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down
  // and stop before colliding
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = "panda_link0";
  collision_object.id = "box";


  // Make a box and put it in the way
  shape_msgs::msg::SolidPrimitive box;
  box.type = box.BOX;
  box.dimensions = { 0.1, 0.4, 0.1 };
  geometry_msgs::msg::Pose box_pose;
  box_pose.position.x = 0.6;
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.6;


  // Add the box as a collision object
  collision_object.primitives.push_back(box);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;


  // Create the message to publish the collision object
  moveit_msgs::msg::PlanningSceneWorld psw;
  psw.collision_objects.push_back(collision_object);
  moveit_msgs::msg::PlanningScene ps;
  ps.is_diff = true;
  ps.world = psw;


  // Publish the collision object to the planning scene
  auto scene_pub = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10);
  scene_pub->publish(ps);


  // Initializing Servo
  // ^^^^^^^^^^^^^^^^^^
  // Servo requires a number of parameters to dictate its behavoir. These can be read automatically by using the
  // :code:`makeServoParameters` helper function
  auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_, LOGGER);
  if (!servo_parameters)
  {
    RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
    return EXIT_FAILURE;
  }


  // Initialize the Servo C++ interface by passing a pointer to the node, the parameters, and the PSM
  auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);


  // You can start Servo directly using the C++ interface. If launched using the alternative ServoServer, a ROS service
  // is used to start Servo. Before it is started, MoveIt Servo will not accept any commands or move the robot
  servo->start();


  // Sending Commands
  // ^^^^^^^^^^^^^^^^
  // For this demo, we will use a simple ROS timer to send joint and twist commands to the robot
  rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);


  // CALL_SUB_TUTORIAL publishCommands


  // We use a multithreaded executor here because Servo has concurrent processes for moving the robot and avoiding collisions
  auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
  executor->add_node(node_);
  executor->spin();


  // END_TUTORIAL


  rclcpp::shutdown();
  return 0;
}

*********************************************************************/
//键盘控制
/*      Title     : servo_keyboard_input.cpp
 *      Project   : moveit2_tutorials
 *      Created   : 05/31/2021
 *      Author    : Adam Pettinger
 */


#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <control_msgs/msg/joint_jog.hpp>


#include <signal.h>
#include <stdio.h>
#include <termios.h>
#include <unistd.h>


// Define used keys
#define KEYCODE_RIGHT 0x43
#define KEYCODE_LEFT 0x44
#define KEYCODE_UP 0x41
#define KEYCODE_DOWN 0x42
#define KEYCODE_PERIOD 0x2E
#define KEYCODE_SEMICOLON 0x3B
#define KEYCODE_1 0x31
#define KEYCODE_2 0x32
#define KEYCODE_3 0x33
#define KEYCODE_4 0x34
#define KEYCODE_5 0x35
#define KEYCODE_6 0x36
#define KEYCODE_7 0x37
#define KEYCODE_Q 0x71
#define KEYCODE_W 0x77
#define KEYCODE_E 0x65
#define KEYCODE_R 0x72


// Some constants used in the Servo Teleop demo
const std::string TWIST_TOPIC = "/servo_server/delta_twist_cmds";
const std::string JOINT_TOPIC = "/servo_server/delta_joint_cmds";
const size_t ROS_QUEUE_SIZE = 10;
const std::string EEF_FRAME_ID = "panda_hand";
const std::string BASE_FRAME_ID = "panda_link0";


// A class for reading the key inputs from the terminal
class KeyboardReader
{
public:
  KeyboardReader() : kfd(0)
  {
    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    struct termios raw;
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &= ~(ICANON | ECHO);
    // Setting a new line, then end of file
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
  }
  void readOne(char* c)
{
    int rc = read(kfd, c, 1);
    if (rc < 0)
    {
      throw std::runtime_error("read failed");
    }
  }
  void shutdown()
{
    tcsetattr(kfd, TCSANOW, &cooked);
  }


private:
  int kfd;
  struct termios cooked;
};


// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
class KeyboardServo
{
public:
  KeyboardServo();
  int keyLoop();


private:
  void spin();


  rclcpp::Node::SharedPtr nh_;


  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
  rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;


  std::string frame_to_publish_;
  double joint_vel_cmd_;
};


KeyboardServo::KeyboardServo() : frame_to_publish_(BASE_FRAME_ID), joint_vel_cmd_(1.0)
{
  nh_ = rclcpp::Node::make_shared("servo_keyboard_input");


  twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
  joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
}


KeyboardReader input;


void quit(int sig)
{
  (void)sig;
  input.shutdown();
  rclcpp::shutdown();
  exit(0);
}


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  KeyboardServo keyboard_servo;


  signal(SIGINT, quit);


  int rc = keyboard_servo.keyLoop();
  input.shutdown();
  rclcpp::shutdown();


  return rc;
}


void KeyboardServo::spin()
{
  while (rclcpp::ok())
  {
    rclcpp::spin_some(nh_);
  }
}


int KeyboardServo::keyLoop()
{
  char c;
  bool publish_twist = false;
  bool publish_joint = false;


  std::thread{ std::bind(&KeyboardServo::spin, this) }.detach();


  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use arrow keys and the '.' and ';' keys to Cartesian jog");
  puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame");
  puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.");
  puts("'Q' to quit.");


  for (;;)
  {
    // get the next event from the keyboard
    try
    {
      input.readOne(&c);
    }
    catch (const std::runtime_error&)
    {
      perror("read():");
      return -1;
    }


    RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);


    // // Create the messages we might publish
    auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();


    // Use read key-press
    switch (c)
    {
      case KEYCODE_LEFT:
        RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
        twist_msg->twist.linear.y = -0.2;
        publish_twist = true;
        break;
      case KEYCODE_RIGHT:
        RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
        twist_msg->twist.linear.y = 0.2;
        publish_twist = true;
        break;
      case KEYCODE_UP:
        RCLCPP_DEBUG(nh_->get_logger(), "UP");
        twist_msg->twist.linear.x = 0.2;
        publish_twist = true;
        break;
      case KEYCODE_DOWN:
        RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
        twist_msg->twist.linear.x = -0.2;
        publish_twist = true;
        break;
      case KEYCODE_PERIOD:
        RCLCPP_DEBUG(nh_->get_logger(), "PERIOD");
        twist_msg->twist.linear.z = -0.2;
        publish_twist = true;
        break;
      case KEYCODE_SEMICOLON:
        RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON");
        twist_msg->twist.linear.z = 0.2;
        publish_twist = true;
        break;
      case KEYCODE_E:
        RCLCPP_DEBUG(nh_->get_logger(), "E");
        frame_to_publish_ = EEF_FRAME_ID;
        break;
      case KEYCODE_W:
        RCLCPP_DEBUG(nh_->get_logger(), "W");
        frame_to_publish_ = BASE_FRAME_ID;
        break;
      case KEYCODE_1:
        RCLCPP_DEBUG(nh_->get_logger(), "1");
        joint_msg->joint_names.push_back("panda_joint1");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_2:
        RCLCPP_DEBUG(nh_->get_logger(), "2");
        joint_msg->joint_names.push_back("panda_joint2");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_3:
        RCLCPP_DEBUG(nh_->get_logger(), "3");
        joint_msg->joint_names.push_back("panda_joint3");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_4:
        RCLCPP_DEBUG(nh_->get_logger(), "4");
        joint_msg->joint_names.push_back("panda_joint4");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_5:
        RCLCPP_DEBUG(nh_->get_logger(), "5");
        joint_msg->joint_names.push_back("panda_joint5");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_6:
        RCLCPP_DEBUG(nh_->get_logger(), "6");
        joint_msg->joint_names.push_back("panda_joint6");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_7:
        RCLCPP_DEBUG(nh_->get_logger(), "7");
        joint_msg->joint_names.push_back("panda_joint7");
        joint_msg->velocities.push_back(joint_vel_cmd_);
        publish_joint = true;
        break;
      case KEYCODE_R:
        RCLCPP_DEBUG(nh_->get_logger(), "R");
        joint_vel_cmd_ *= -1;
        break;
      case KEYCODE_Q:
        RCLCPP_DEBUG(nh_->get_logger(), "quit");
        return 0;
    }


    // If a key requiring a publish was pressed, publish the message now
    if (publish_twist)
    {
      twist_msg->header.stamp = nh_->now();
      twist_msg->header.frame_id = frame_to_publish_;
      twist_pub_->publish(std::move(twist_msg));
      publish_twist = false;
    }
    else if (publish_joint)
    {
      joint_msg->header.stamp = nh_->now();
      joint_msg->header.frame_id = BASE_FRAME_ID;
      joint_pub_->publish(std::move(joint_msg));
      publish_joint = false;
    }
  }


  return 0;
}

Setup

首先，我们声明指向将向伺服servo发布命令的节点和发布者的指针

rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
size_t count_ = 0;

接下来我们将设置节点、planning_scene_monitor 和碰撞对象

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.use_intra_process_comms(true);
  node_ = std::make_shared<rclcpp::Node>("servo_demo_node", node_options);

暂停让 RViz 出现。这在具有单个启动文件的集成演示中是必需的

rclcpp::sleep_for(std::chrono::seconds(4));

创建planning_scene_monitor。我们需要将它传递给 Servo 的构造函数，我们应该在初始化任何碰撞对象之前先设置它

auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
    node_, "robot_description", tf_buffer, "planning_scene_monitor");

在这里，我们确保planning_scene_monitor 从关节状态主题实时更新

if (planning_scene_monitor->getPlanningScene())
{
  planning_scene_monitor->startStateMonitor("/joint_states");
  planning_scene_monitor->setPlanningScenePublishingFrequency(25);
  planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                       "/moveit_servo/publish_planning_scene");
  planning_scene_monitor->startSceneMonitor();
}
else
{
  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
  return EXIT_FAILURE;
}

这些是向 MoveIt Servo 发送命令的发布者。支持两种命令类型：JointJog 消息，它将直接在关节空间中使机器人jog 慢跑，以及 TwistStamped消息，它将以命令的笛卡尔速度移动指定的连杆。在这个演示中，我们点动末端执行器连杆。

joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("servo_demo_node/delta_joint_cmds", 10);
twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("servo_demo_node/delta_twist_cmds", 10);

接下来我们将以手臂的方式创建一个碰撞对象。当手臂被伺服朝向它时，它会在碰撞前减速并停止

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0";
collision_object.id = "box";

做一个盒子，放在路上

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.6;
box_pose.position.y = 0.0;
box_pose.position.z = 0.6;

将盒子添加为碰撞对象

collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

创建消息以发布碰撞对象

moveit_msgs::msg::PlanningSceneWorld psw;
psw.collision_objects.push_back(collision_object);
moveit_msgs::msg::PlanningScene ps;
ps.is_diff = true;
ps.world = psw;

将碰撞对象发布到规划场景

auto scene_pub = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10);
scene_pub->publish(ps);

Initializing Servo

伺服需要许多参数来决定其行为。这些可以通过使用makeServoParameters 辅助函数自动读取

auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_, LOGGER);
if (!servo_parameters)
{
  RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
  return EXIT_FAILURE;
}

通过传递指向节点、参数和 PSM 的指针来初始化 Servo C++ 接口

auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);

您可以使用 C++ 接口直接启动Servo。如果使用替代的 ServoServer 启动，则使用 ROS 服务来启动 Servo。在启动之前，MoveIt Servo 不会接受任何命令或移动机器人

servo->start();

Sending Commands

在这个演示中，我们将使用一个简单的 ROS 计时器向机器人发送关节和扭转命令

rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

这是发布命令的计时器回调。C++ 接口通过内部ROS 主题发送命令，就像使用ServoServer 启动Servo 一样。

void publishCommands()
{

首先，我们将发布100 个关节慢跑jogging 命令。Joint_names 字段允许您指定单个关节以相应速度字段中的速度移动。消息中包含最近的时间戳很重要，否则伺服将认为该命令已过时并且不会移动机器人。

if (count_ < 100)
{
  auto msg = std::make_unique<control_msgs::msg::JointJog>();
  msg->header.stamp = node_->now();
  msg->joint_names.push_back("panda_joint1");
  msg->velocities.push_back(0.3);
  joint_cmd_pub_->publish(std::move(msg));
  ++count_;
}

过了一会儿，我们切换到发布扭曲命令。提供的框架是定义扭曲的框架，而不是遵循命令的机器人框架。同样，我们需要消息中的最近时间戳

else
  {
    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = "panda_link0";
    msg->twist.linear.x = 0.3;
    msg->twist.angular.z = 0.5;
    twist_cmd_pub_->publish(std::move(msg));
  }
  }

我们在这里使用多线程执行器，因为Servo 具有用于移动机器人和避免碰撞的并发进程

auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
executor->add_node(node_);
executor->spin();

Servo Overview

以下部分提供了有关 MoveIt Servo 的一些背景信息，并描述了在机器人上设置它的第一步。

伺服包括许多不错的功能：

 笛卡尔末端执行器扭曲命令

 关节命令

 碰撞检查

 奇点检查

 强制执行关节位置和速度限制

 输入是通用的 ROS 消息

Setup on a New Robot

PreliminariesÁ

使用机器人运行 MoveIt Servo 的最低要求包括：

机器人的有效 URDF 和 SRDF

可以接受来自 ROS 主题的关节位置或速度的控制器

提供快速准确的关节位置反馈的关节编码器

由于运动学由 MoveIt 的核心部分处理，因此建议您为您的机器人准备一个有效的配置包，并且可以运行其中包含的演示启动文件。

Input Devices

MoveIt Servo 的两个主要输入是笛卡尔命令和关节命令。这些分别作为 TwistStamped 和 JointJog 消息进入 Servo。命令的来源几乎可以是任何东西，包括：游戏手柄、语音命令、SpaceNav鼠标或 PID 控制器（例如用于视觉伺服）。

无论输入设备如何，对传入命令消息的要求是：

TwistStamped 和JointJog：需要在消息发布时更新header标头中的时间戳

JointJog：joint_names字段中必须具有与位移或速度字段中给出的命令相对应的有效关节名称

（可选）TwistStamped：可以在将应用twist 的标头中提供任意 frame_id。如果为空，则使用配置中的默认值

Servo Configs

演示配置文件显示了 MoveIt Servo 所需的参数，并且有据可查。

https://github.com/ros-planning/moveit2/blob/foxy/moveit_ros/moveit_servo/config/panda_simulated_config.yaml

从演示文件中的参数开始，但必须针对您的特定设置更改一些参数：

robot_link_command_frame：将其更新为机器人中的有效框架，推荐作为规划框架或 EEF 框架

command_in_type：如果您的输入来自操纵杆，则设置为“无单位”，如果输入以米/秒或弧度/秒为单位，则设置为“speed_units”

command_out_topic：将其更改为控制器的输入主题topic 

command_out_type：根据控制器需要的消息类型更改此设置

publish_joint_positions 和 publish_joint_velocities：根据控制器的需要更改它们。注意如果command_out_type == std_msgs/Float64MultiArray，其中只有一个可以是True

joint_topic：将其更改为手臂的关节状态主题，通常是/joint_states

move_group_name：将其更改为您的move group的名称，如您的 SRDF 中所定义

planning_frame：这应该是您的规划组的规划框架

参考：

https://moveit.picknik.ai/foxy/doc/realtime_servo/realtime_servo_tutorial.html 

The End