机器人编程趣味实践10-做个任务（行动）

Posted 2021-06-17 zhangrelay

这是ROS2中比服务更为复杂的基础模块。

白话一下：一个行动，可以时刻汇报进展，无法完成时可以变更或者取消，完成后会告知

• Mission completed!

具体应用案例，如导航行为树

这个后续细说，现在先从简单的入手吧。

---

>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

---

本节目标通过基础案例掌握ROS2内部行动的基本使用。

预备知识

行动是 ROS 2 中的又一种通信类型，用于长时间运行的任务，由三部分组成：目标、反馈和结果。行动建立在主题和服务上。 它们的功能类似于服务，但可以取消，还提供稳定的反馈，而不同于返回单一响应的服务。行动同样使用客户端-服务器模型，类似于发布者-订阅者模型（在主题教程中进行了描述）。 “行动客户端”节点向“动作服务器”节点发送目标，该节点确认目标并返回反馈过程流和结果。

功能包要点

1. ros2基础包
2. turtlesim包
3. source

实践

1 配置

运行如下两个节点 /turtlesim 和 /teleop_turtle ：

1. ros2 run turtlesim turtlesim_node
2. ros2 run turtlesim turtle_teleop_key

2 基础使用

可以看到右侧启动/teleop_turtle节点后，终端显示如下：

使用方向键移动机器人.
使用 G|B|V|C|D|E|R|T 键使机器人转相应角度. 'F' 键取消旋转.

让我们专注于第二行，它对应于一个行动。 （第一条指令对应于“cmd_vel”主题，之前在主题教程中讨论过。）请注意，字母键 G|B|V|C|D|E|R|T 在美国 QWERTY 键盘上的 F 键周围形成一个“框”。 F 周围的每个键的位置对应于turtlesim 中的方向。 例如，E 会将海龟的方向旋转到左上角。

注意 /turtlesim 节点运行的终端。 每次按下其中一个键时，都会向作为 /turtlesim 节点一部分的动作服务器发送一个目标。 目标是旋转乌龟以使其朝向特定方向。 乌龟完成旋转后，应显示一条中继目标结果的消息：

[INFO] [1622462396.650765434] [turtlesim]: 开启二维机器人仿真节点 /turtlesim
[INFO] [1622462396.735890281] [turtlesim]: Spawning turtle [turtle1] at x=[9.855556], y=[7.388889], theta=[0.000000]
[INFO] [1622462694.049672436] [turtlesim]: Rotation goal completed successfully

如上对应：

      if (fabs(normalizeAngle(static_cast<float>(orient_) - theta)) < 0.02)
      {
        RCLCPP_INFO(nh_->get_logger(), "Rotation goal completed successfully");
        rotate_absolute_goal_handle_->succeed(rotate_absolute_result_);
        rotate_absolute_goal_handle_ = nullptr;
        lin_vel_x_ = 0.0;
        lin_vel_y_ = 0.0;
        ang_vel_ = 0.0;
      }

F 键将在执行过程中取消目标。

尝试按 B 键，然后在海龟完成旋转之前按 F 键。 在运行 /turtlesim 节点的终端中，将看到以下消息：

[INFO] [1622462850.641914700] [turtlesim]: Rotation goal canceled

如上对应：

    if (rotate_absolute_goal_handle_->is_canceling())
    {
      RCLCPP_INFO(nh_->get_logger(), "Rotation goal canceled");
      rotate_absolute_goal_handle_->canceled(rotate_absolute_result_);
      rotate_absolute_goal_handle_ = nullptr;
      lin_vel_x_ = 0.0;
      lin_vel_y_ = 0.0;
      ang_vel_ = 0.0;
    }

客户端（在Teleop中的输入）不仅可以停止目标，而且服务器端（/turtlesim节点）也可以停止目标。 当服务器端选择停止处理一个目标时，称为“中止”该目标。 尝试按 D 键，然后在旋转完成之前按 G 键。 在运行 /turtlesim 节点的终端中，将看到以下消息：

[WARN] [1622462989.905265647] [turtlesim]: Rotation goal received before a previous goal finished. Aborting previous goal

如上对应：

void Turtle::rotateAbsoluteAcceptCallback(const std::shared_ptr<RotateAbsoluteGoalHandle> goal_handle)
{
  // Abort any existing goal
  if (rotate_absolute_goal_handle_)
  {
    RCLCPP_WARN(nh_->get_logger(), "Rotation goal received before a previous goal finished. Aborting previous goal");
    rotate_absolute_goal_handle_->abort(rotate_absolute_result_);
  }
  rotate_absolute_goal_handle_ = goal_handle;
  rotate_absolute_feedback_.reset(new turtlesim::action::RotateAbsolute::Feedback);
  rotate_absolute_result_.reset(new turtlesim::action::RotateAbsolute::Result);
  rotate_absolute_start_orient_ = orient_;
}

该行动服务器选择中止第一个目标，因为有了新目标。 可以选择其他目标，例如拒绝新目标或在第一个目标完成后执行第二个目标。 不要假设每个行动服务器在获得新目标时都会选择中止当前目标。

3 节点信息

/turtlesim

•   Action Servers:
      /turtle1/rotate_absolute: turtlesim/action/RotateAbsolute

zhangrelay@ros2fun:~/RobSoft/turtlesim$ ros2 node info /turtlesim
/turtlesim
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /turtle1/cmd_vel: geometry_msgs/msg/Twist
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /turtle1/color_sensor: turtlesim/msg/Color
    /turtle1/pose: turtlesim/msg/Pose
  Service Servers:
    /clear: std_srvs/srv/Empty
    /kill: turtlesim/srv/Kill
    /reset: std_srvs/srv/Empty
    /spawn: turtlesim/srv/Spawn
    /turtle1/set_pen: turtlesim/srv/SetPen
    /turtle1/teleport_absolute: turtlesim/srv/TeleportAbsolute
    /turtle1/teleport_relative: turtlesim/srv/TeleportRelative
    /turtlesim/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /turtlesim/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /turtlesim/get_parameters: rcl_interfaces/srv/GetParameters
    /turtlesim/list_parameters: rcl_interfaces/srv/ListParameters
    /turtlesim/set_parameters: rcl_interfaces/srv/SetParameters
    /turtlesim/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:
    /turtle1/rotate_absolute: turtlesim/action/RotateAbsolute
  Action Clients:

/teleop_turtle

•   Action Clients:
      /turtle1/rotate_absolute: turtlesim/action/RotateAbsolute

zhangrelay@ros2fun:~/RobSoft/turtlesim$ ros2 node info /teleop_turtle 
/teleop_turtle
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /turtle1/cmd_vel: geometry_msgs/msg/Twist
  Service Servers:
    /teleop_turtle/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /teleop_turtle/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /teleop_turtle/get_parameters: rcl_interfaces/srv/GetParameters
    /teleop_turtle/list_parameters: rcl_interfaces/srv/ListParameters
    /teleop_turtle/set_parameters: rcl_interfaces/srv/SetParameters
    /teleop_turtle/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:
    /turtle1/rotate_absolute: turtlesim/action/RotateAbsolute

4 行动列表

• ros2 action list

显示如下：

/turtle1/rotate_absolute

• ros2 action list -t

显示如下：

/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]

5 行动信息

• ros2 action info /turtle1/rotate_absolute

显示结果如下：

Action: /turtle1/rotate_absolute
Action clients: 1
    /teleop_turtle
Action servers: 1
    /turtlesim

6 行动接口消息

• ros2 interface show turtlesim/action/RotateAbsolute

显示如下：

# The desired heading in radians
float32 theta
---
# The angular displacement in radians to the starting position
float32 delta
---
# The remaining rotation in radians
float32 remaining

7 发送目标

通用格式：

ros2 action send_goal <action_name> <action_type> <values>

具体应用：

ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.57}"

显示结果如下：

Waiting for an action server to become available...
Sending goal:
     theta: 1.57

Goal accepted with ID: 9e134c1c6d3d41bbac1aa20038487d07

Result:
    delta: 1.5520031452178955

Goal finished with status: SUCCEEDED

注意，误差大约0.02。可以修改机器人PID转向提高精度留作思考题。

当然控制周期也可以好好研究一下哦^_^

如果需要看过程：

• ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 0.0}" --feedback

Waiting for an action server to become available...
Sending goal:
     theta: 0.0

Feedback:
    remaining: -0.10079997777938843

Goal accepted with ID: e06e9a8bc25345e18ace2a0fdaa55b80

Feedback:
    remaining: -0.09919998049736023

....

Feedback:
    remaining: -0.0016000281320884824

Result:
    delta: 0.09919995069503784

Goal finished with status: SUCCEEDED

总结

行动类似服务，允许执行长时间运行的任务、提供定时反馈并且可以取消。

机器人系统可能会使用行动进行导航。 动作目标可以告诉机器人行进到一个位置。 机器人导航到该位置时，可以沿途发送更新（即反馈），一旦到达目的地，它就会发送最终结果消息。

turtlesim 有一个行动服务器，行动客户端可以将目标发送到旋转机器人。 在本教程中，学习行动 /turtle1/rotate_absolute，更好了解什么是行动以及它们如何工作。