ROS2参数与动作

1.参数

  （1）定义：参数是节点的一个配置值，你可以认为参数是一个节点的设置

  参数就相当于是一个变量，可以更改值的大小从而达到不一样的目的，用来实现动态修改和管理

  （2）组成成分：

        ROS2支持的参数类型：

• bool 和bool[]，布尔类型用来表示开关，比如我们可以控制雷达控制节点，开始扫描和停止扫描。
• int64 和int64[]，整形表示一个数字，含义可以自己来定义，这里我们可以用来表示李四节点写小说的周期值
• float64 和float64[]，浮点型，可以表示小数类型的参数值
• string 和string[]，字符串，可以用来表示雷达控制节点中真实雷达的ip地址
• byte[]，字节数组，这个可以用来表示图片，点云数据等信息

  （3）体验参数

        运行小乌龟模拟器和小乌龟控制节点

       打开终端

ros2 run turtlesim turtlesim_node

        再打开终端

ros2 run turtlesim turtle_teleop_key

 看见小乌龟出现后，再打开一个终端

ros2 param list

这个指令是查看所有节点的参数列表

查看一个详细的节点信息

ros2 param describe <node_name> <param_name>

 比如

ros2 param describe /turtlesim background_b

 

查看参数值

ros2 param get /turtlesim background_b

我们可以手动设置参数来改变小乌龟的背景颜色

ros2 param set /turtlesim background_r 44
ros2 param set /turtlesim background_g 156
ros2 param set /turtlesim background_b 10

 

之后就会改成绿色了

值得注意的是这里的颜色更改只是临时的，当你关掉重新运行的时候它的背景颜色就会变回原来的颜色

这就需要我们把参数给保存起来

ros2 param dump <node_name>

这样就会保存成yaml格式的文件了，之后再调用文件

ros2 param load  /turtlesim ./turtlesim.yaml

但是这里我不知道是什么原因，没有显示有yaml的文件保存下来，直接就显示了文件的内容所以我保存不了这个背景颜色，有没有大佬知道是什么原因。

（4）RCLCPP实现参数

这里我改了一下上一章里写的程序，计算两个数的和，我改成了参数，虽然没有什么意义，但可以体现一下写法

1.创建变量，一般都是在私有权限下创建

2.声明参数，在构造函数中进行

this->declare_parameter<std::int64_t>("名称",名称);

3.调用参数

this->get_parameter("名称",名称);

改完这些参数就能够使用了，然后运行节点，之后可以通过使用rqt改变数值。

2.动作

（1）组成成分：

• 目标：即Action客户端告诉服务端要做什么，服务端针对该目标要有响应。解决了不能确认服务端接收并处理目标问题
• 反馈：即Action服务端告诉客户端此时做的进度如何（类似与工作汇报）。解决执行过程中没有反馈问题
• 结果：即Action服务端最终告诉客户端其执行结果，结果最后返回，用于表示任务最终执行情况。

（2)CLI工具

一、action list

获取目前系统中的action列表

ros2 action list

打开小乌龟工具后将会看到

/turtle1/rotate_absolute

如果在list后加入-t参数，即可看到action的类型

/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]

知道了接口类型之后，可以使用接口相关CLI指令获取接口的信息

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

二、action info

查看action信息，在终端中输入下面的指令。

ros2 action info /turtle1/rotate_absolute 

你将看到,action客户端和服务段的数量以及名字。

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

三、actin send_goal

该指令用于发送actin请求到服务端，我们可以模拟下，让小乌龟转到我们定义的角度。

我们只需要把goal发给服务端即可，根据goal的定义，我们可以看到goal是由一个浮点类型的theta组成的，我们把theta发给服务端。

发送弧度制1.6给小乌龟

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

结果

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

Goal accepted with ID: b215ad060899444793229171e76481c7

Result:
    delta: -1.5840003490447998

Goal finished with status: SUCCEEDED

 这里我们可以加上一个feedback参数进行反馈

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

结果

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

Feedback:
    remaining: -0.0840003490447998

Goal accepted with ID: b368de0ed1a54e00890f1b078f4671c8

Feedback:
    remaining: -0.06800031661987305

Feedback:
    remaining: -0.05200028419494629

Feedback:
    remaining: -0.03600025177001953

Feedback:
    remaining: -0.020000219345092773

Feedback:
    remaining: -0.004000186920166016

Result:
    delta: 0.08000016212463379

Goal finished with status: SUCCEEDED

（3)自定义通信接口

创建接口功能包

cd chapt2_ws/
ros2 pkg create robot_control_interfaces --build-type ament_cmake --destination-directory src --maintainer-name "fishros" --maintainer-email "fishros@foxmail.com"

创建接口文件

mkdir -p src/robot_control_interfaces/action
touch src/robot_control_interfaces/action/MoveRobot.action

创建完成后进入VS code ,修改文件夹程序

packages.xml

  <depend>rosidl_default_generators</depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

 注意不是放在最后面，而是要包在里面

CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)


rosidl_generate_interfaces(${PROJECT_NAME}
  "action/MoveRobot.action"
)

 这个可以直接在最后面编写

接下来通过一个项目来体现出动作的应用

例子：控制机器人移动到一个点的位置

利用上面的接口我们继续编写

创建功能包和节点

创建example_action_rclcpp功能包，添加robot_control_interfaces、rclcpp_action、rclcpp三个依赖，自动创建action_robot_01节点，并手动创建action_control_01.cpp节点

cd chapt2_ws/
ros2 pkg create example_action_rclcpp --build-type ament_cmake --dependencies rclcpp rclcpp_action robot_control_interfaces --destination-directory src --node-name action_robot_01 --maintainer-name "fishros" --maintainer-email "fishros@foxmail.com"
touch src/example_action_rclcpp/src/action_control_01.cpp

接着我们创建Robot类的头文件和CPP文件。

touch src/example_action_rclcpp/include/example_action_rclcpp/robot.h
touch src/example_action_rclcpp/src/robot.cpp

首先先修改CMakeList.txt

        在这里要加入一些节点命令和一些依赖

完整的代码

cmake_minimum_required(VERSION 3.8)
project(example_action_rclcpp)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(robot_control_interfaces REQUIRED)
find_package(example_interfaces REQUIRED)

# action_robot节点

add_executable(action_robot_01 
src/robot.cpp
src/action_robot_01.cpp
)
target_include_directories(action_robot_01 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(action_robot_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
action_robot_01
"rclcpp"
"rclcpp_action"
"robot_control_interfaces"
"example_interfaces"
)

install(TARGETS action_robot_01
DESTINATION lib/${PROJECT_NAME})

# action_control节点

add_executable(action_control_01 
src/action_control_01.cpp
)
target_include_directories(action_control_01 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(action_control_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
action_control_01
"rclcpp"
"rclcpp_action"
"robot_control_interfaces"
"example_interfaces"
)

install(TARGETS action_control_01
DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

至于为什么要先修改这个文件是因为在防止在编写程序过程中他会一直报错。

然后编写robot.h

        这个文件是定义各种变量函数的文件，统一在一起来定义，之后再分开编写。

#ifndef EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#define EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

class Robot {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  Robot() = default;
  ~Robot() = default;
  float move_step();/*移动一小步，间隔500ms*/
  bool set_goal(float distance);/*移动一段距离*/
  float get_current_pose();
  int get_status();
  bool close_goal();/*是否接近目标*/
  void stop_move();/*停止移动*/
 private:
   float current__pose_=0.0; /*声明当前位置*/
   float target_pose_=0.0; /*目标距离*/
   float move_distance_=0.0;/*目标距离*/
   std::atomic<bool>cancel_flag_{false}; /*取消标志*/
   int status_=MoveRobot::Feedback::STATUS_STOP;


};

#endif  // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_

然后到写robot.cpp

#include "example_action_rclcpp/robot.h"
/*移动一小步，间隔500ms*/
float Robot::move_step(){
    int direct=move_distance_/fabs(move_distance_);
    float step=direct *fabs(target_pose_-current__pose_)*0.1;/*每一步移动当前目标距离的1/10*/
    current__pose_+=step;
    std::cout<<"移动了："<<step<<"当前位置；"<<current__pose_<<std::endl;
    return current__pose_;
}

/*移动一段距离*/
bool Robot::set_goal(float distance)
{
    move_distance_=distance;
    target_pose_+=move_distance_;

    /*当目标距离和当前距离大于0.01同意向目标移动*/
    if(close_goal())
    {
        status_=MoveRobot::Feedback::STATUS_STOP;
        return false;
    }
    status_=MoveRobot::Feedback::STATUS_MOVEING;
    return true;
}

float Robot::get_current_pose() {return current__pose_;}
int Robot::get_status() {return status_;}
/*是否接近目标*/
bool Robot::close_goal() {return fabs(target_pose_-current__pose_)<0.01;}
void Robot::stop_move(){
    status_=MoveRobot::Feedback::STATUS_STOP;
}/*停止移动*/

编写机器人节点action_robot_01.cpp

#include "example_action_rclcpp/robot.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

// 创建一个ActionServer类
class ActionRobot01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;

  explicit ActionRobot01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    using namespace std::placeholders;//NOLINT

    this->action_server_ = rclcpp_action::create_server<MoveRobot>(this,"move_robot",
      std::bind(&ActionRobot01::handle_goal,this,_1,_2),
      std::bind(&ActionRobot01::handle_cancel,this,_1),
      std::bind(&ActionRobot01::handle_accepted,this,_1));
  }


private:
  Robot robot;
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_ ;

  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID& uuid,
    std::shared_ptr<const MoveRobot::Goal>goal){
    RCLCPP_INFO(this->get_logger(),"Received goal request with distance %f",goal->distance);
    (void)uuid;
    if(fabs(goal->distance>100)) {
      RCLCPP_WARN(this->get_logger(),"目标距离太远了，本机器人表示拒绝！");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),"目标距离%f我可以走到,本机器人接受，准备出发！",goal->distance);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

  rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    robot.stop_move(); /*认可取消执行，让机器人停下来*/
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    const auto goal = goal_handle->get_goal();
    RCLCPP_INFO(this->get_logger(), "开始执行移动 %f 。。。", goal->distance);

    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate rate = rclcpp::Rate(2);
    robot.set_goal(goal->distance);
    while (rclcpp::ok() && !robot.close_goal()) {
      robot.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot.get_current_pose();
      feedback->status = robot.get_status();
      goal_handle->publish_feedback(feedback);
      /*检测任务是否被取消*/
      if (goal_handle->is_canceling()) {
        result->pose = robot.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/
      rate.sleep();
    }

    result->pose = robot.get_current_pose();
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
  }

  void handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    using std::placeholders::_1;
    std::thread{std::bind(&ActionRobot01::execute_move, this, _1), goal_handle}
        .detach();
  }
};

int main(int argc, char** argv) 
{
  rclcpp::init(argc, argv);
  auto action_server = std::make_shared<ActionRobot01>("action_robot_01");
  rclcpp::spin(action_server);
  rclcpp::shutdown();
  return 0;
}

 代码有点长，这里运用了三个回调函数，分别用于处理目标，受到停止以及确认接受执行

• handle_goal，收到目标，反馈是否可以执行该目标，可以则返回ACCEPT_AND_EXECUTE,不可以则返回REJECT
• handle_cancel，收到取消运行请求，可以则返回ACCEPT，不可以返回REJECT。
• handle_accepted，处理接受请求，当handle_goal中对移动请求ACCEPT后则进入到这里进行执行，这里我们是单独开了个线程进行执行execute_move函数，目的是避免阻塞主线程。

执行函数execute_move，调用机器人，进行一步步的移动，这里我们采用了while循环的形式，不断调用机器人移动并获取机器人的位置，通过feedback进行反馈。同时检测任务是否被取消，如顺利执行完成则反馈最终结果。

action_control_01.cpp

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

class ActionControl01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;

  explicit ActionControl01(
      std::string name,
      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions())
      : Node(name, node_options) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    this->client_ptr_ =
        rclcpp_action::create_client<MoveRobot>(this, "move_robot");

    this->timer_ =
        this->create_wall_timer(std::chrono::milliseconds(500),
                                std::bind(&ActionControl01::send_goal, this));
  }

  void send_goal() {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(),
                   "Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = MoveRobot::Goal();
    goal_msg.distance = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options =
        rclcpp_action::Client<MoveRobot>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&ActionControl01::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&ActionControl01::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&ActionControl01::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

 private:
  rclcpp_action::Client<MoveRobot>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(),
                  "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
      GoalHandleMoveRobot::SharedPtr,
      const std::shared_ptr<const MoveRobot::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback current pose:%f", feedback->pose);
  }

  void result_callback(const GoalHandleMoveRobot::WrappedResult& result) {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->pose);
    // rclcpp::shutdown();
  }
};  // class ActionControl01


int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<ActionControl01>("action_robot_cpp");
  rclcpp::spin(action_client);
  rclcpp::shutdown();
  return 0;
}

这里主要是处理收到不同信号的反馈以及控制机器人能否进行移动，同样也有三个回调函数

• goal_response_callback，目标的响应回调函数。
• feedback_callback，执行过程中进度反馈接收回调函数。
• result_callback，最终结果接收的回调函数。

这里利用了定时器完成了定时请求的功能，请求一次后就立马使用timer_->cancel();取消掉了这个定时器，如此就实现了节点启动后定时发一次请求的功能

编写完成之后就可以运行结果了

新建一个终端，运行机器人节点

cd chapt2_ws/
colcon build --packages-up-to example_action_rclcpp
source install/setup.bash 
ros2 run example_action_rclcpp  action_robot_01

拆分一个终端，运行控制节点

source install/setup.bash 
ros2 run example_action_rclcpp action_control_01

 

 

到这里这个项目就已经完成了，当然对于新手的我来说这个不是我自己单独能够编写出来了，我是看着教程一步一步跟着做到，教程中之给出了代码和部分解释，并没有告诉我们要从哪一步开始去编写，这些都是通过自己去摸索出来的，我认为我自己编写的方法是有利于我自己以后去编写程序的。

        在这个过程中出错是必不可少的，但往往也是最烦人的，在看着都感觉眼花缭乱的代码中找出错误，需要细心，特别是那种编写是不会报错，但一编译就会出错那种，对于小白的我来说出错的原因自己都不知道有什么，就只能对着代码一个一个去看有没有重复或者打错的地方。

        随着代码敲的次数变多，我也熟悉了部分代码，并且理解了其中的含义，我觉得对于刚学习的我来说就是应该要多敲代码才能熟悉，我认为这个项目是一个很好的锻炼例子，我可以重复地去敲它，现在一开始我需要看着代码去敲，我认为我要一直练习直到可以不用看代码，知道什么意思就能够写出程序的程度。