ROS自定义接口解析

fegxg

于 2024-12-12 15:14:14 发布

阅读量1.2k

点赞数 39

文章标签： ubuntu linux c++

本文链接：https://blog.youkuaiyun.com/fegxg/article/details/144401170

版权

一、自定义接口创建

1.接口的理解

接口（Interface）是一种用于节点间通信的数据结构，是各节点之间进行交流的统一标准。如使用加速度传感器时，用其定义好的规范的数据接口，用温度传感器时，使用其定义好的规范的数据接口。

2.接口分类

ROS 2 提供了三种主要的接口类型：
消息（Message）：用于发布和订阅。文件为.msg
服务（Service）：用于客户端-服务端的请求/响应。文件为.srv
动作（Action）：用于长时间执行的任务，具有反馈和结果。文件为.action

3.数据类型

基础的数据类型为：可以加上[]成为数组

bool
byte
char
float32,float64
int8,uint8
int16,uint16
int32,uint32
int64,uint64
string

也可以在已有的接口类型上进行包含，

uint32 id
string image_name
sensor_msgs/Image  #系统自带的消息接口

通过ROS2的IDL模块，将.msg .srv .action文件变为Python和C++的头文件，有了头文件，我们就可以在程序里导入并使用这个接口模块。

4.自定义接口

机器人节点：移动指定距离，移动完成后返回当前位置，对外发布机器人的位置和状态

控制机器人节点：控制机器人移动指定距离，获取机器人的当前位置和状态，通过服务完成

1.MoveRobot.src文件

# 请求部分（客户端发送给服务端）
float32 distance

---

# 响应部分（服务端返回给客户端）
float32 pose

这里一定要用---进行分隔。

该接口主要是控制节点控制机器人移动时使用，控制节点作为客户端发送指令给服务端机器人，要求机器人移动指定的距离。

机器人节点接收到request后执行操作，并将移动完成后的当前位置返回给客户端控制节点。

2.RobotStatus.msg文件

uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 2
uint32 status
float32 pose

该文件使用基础数据类型，主要用于话题通信中，该文件就是话题topic的消息类型。

机器人节点发布机器人的实时状态和当前位置到话题topic中，控制节点订阅该topic并获取机器人的状态和位置。

3.RobotPose.msg文件

uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 2
uint32 status
geometry_msgs/Pose pose

该文件使用混合数据类型，也是用于话题通信，但它使用了标准消息类型 geometry_msgs/Pose 来提供机器人当前位置的详细信息，包括位置和方向，状态信息则是我们使用基础数据类型来表示。

该文件的使用和上一个文件一致。

4.自定义接口文件的转换

1.cmakelists.txt文件

find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
# 添加下面的内容
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/RobotPose.msg"
  "msg/RobotStatus.msg"
  "srv/MoveRobot.srv"
  DEPENDENCIES geometry_msgs
)

我们需要使用rosidl_default_generators该工具，即IDL转换器。

rosidl_default_generators 是 ROS 2 中的一个包，提供了 ROS 接口定义语言（IDL）生成工具，它负责将 .msg 和 .srv 文件转换为不同语言（如 C++、Python）可以使用的代码。

接着我们使用rosidl_generate_interfaces来告诉 CMake 要生成哪些接口。

${PROJECT_NAME}：当前的 ROS 2 包名，这里是.msg .src .msg这三个文件。

DEPENDENCIES 指定外部依赖包，这里我们的.msg文件使用混合数据类型，使用了标准消息类型 geometry_msgs/Pose，因此我们需要依赖这个包。

整体流程为：

.msg 和 .srv 文件 → rosidl_generate_interfaces → 生成代码（C++、Python） → 节点中使用

2.package.xml文件

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rosidl_default_generators</depend>
  <depend>geometry_msgs</depend>
  
  <member_of_group>rosidl_interface_packages</member_of_group> #添加这一行

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

添加这一行

<member_of_group>rosidl_interface_packages</member_of_group>

将当前包声明为 rosidl_interface_packages 组的成员，用于标记当前这个功能包主要是一个 接口定义包，通过这种方式，工具链可以针对接口包进行特殊处理，比如优化接口生成过程。

二、节点使用自定义消息接口

机器人节点example_interfaces_robot_01

控制节点example_interfaces_control_01

1.机器人节点

1.机器人类

class Robot
{
  public:
  Robot()=default;
  ~Robot()=default;
  float move_distance(float distance)
  {
    status_=example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING;
    target_pose_+=distance;
    while (fabs(target_pose_-current_pose_)>0.01)
    {
      float step=distance/fabs(distance)*fabs(target_pose_-current_pose_)*0.1;
      current_pose_+=step;
      std::cout<<"move: "<<step<<" current pose: "<<current_pose_<<std::endl;
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    status_=example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
    return current_pose_;
  }
  float get_current_pose() {return current_pose_;}
  int get_status() {return status_;}
  private:
  float current_pose_=0.0;
  float target_pose_=0.0;
  int status_=example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
};

该类主要用于建立一个机器人对象，实现机器人运动。

该类的框架为

成员变量：
    current_pose_，用于表示当前的位置
    target_pose_，用于表示目标位置
    status_，机器人当前的运动状态，初始值为STATUS_STOP静止状态
成员函数：
    move_distance，让机器人以一定步长向目标位置移动
    get_current_pose，获取机器人当前位置
    get_status，获取机器人状态

1.构造函数与析构函数

Robot()=default;
~Robot()=default;

使用了编译器提供的默认版本

2.move_distance 函数

float move_distance(float distance)
{
  status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING;
  target_pose_ += distance;

  while (fabs(target_pose_ - current_pose_) > 0.01)
  {
    float step = distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1;
    current_pose_ += step;

    std::cout << "move: " << step << " current pose: " << current_pose_ << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
  }

  status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
  return current_pose_;
}

传入的 distance，即要移动的距离。

进入函数后，首先将状态status_设置为正在移动STATUS_MOVEING，再将target_pose_加上distance。

之后进入while循环，当当前的位置current_pose_和目标位置target_pose_相差大于0.01时，进入while循环中。

计算步长step

float step = distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1;

distance/fabs(distance)用于得到步进方向是正还是负，

fabs(target_pose_ - current_pose_) * 0.1，以当前距离的 10% 作为步进量，确保每次移动逐渐接近目标

之后更新当前的位置current_pose_，并打印步长和当前位置

之后使用sleep_for(std::chrono::milliseconds(500))，添加 500 毫秒的延时，模拟移动过程中时间消耗。

跳出循环后，将机器人的状态设置为STATUS__STOP，并返回当前的位置current_pose_。

get_current_pose 函数返回当前位置current_pose_。

get_status 函数返回当前的状态status。

2.ExampleInterfacesRobot 类

ExampleInterfacesRobot 类主要有三个功能，

作为服务端，响应客户端控制节点的请求，控制机器人移动指定距离

作为发布者，使用定时器周期性发布机器人的当前位置和状态

类的结构为：

成员变量：
    move_robot_server_，服务端
    robot_status_publisher_，发布者
    timer_，定时器，用于定时进入回调函数
    robot，机器人类对象
成员函数：
    构造函数，创建服务端、发布者和定时器
    timer_callback()，发布者周期性进入该函数，发布机器人当前位置和状态
    handle_move_robot()，服务回调函数，接受到客户端请求后处理请求信息

1.构造函数

ExampleInterfacesRobot(std::string name):Node(name)
  {
    RCLCPP_INFO(this->get_logger(),"node has started: %s",name.c_str());
    move_robot_server_=this->create_service<example_ros2_interfaces::srv::MoveRobot>(
      "move_robot",std::bind(&ExampleInterfacesRobot::handle_move_robot,this,std::placeholders::_1,std::placeholders::_2)
    );
    robot_status_publisher_=this->create_publisher<example_ros2_interfaces::msg::RobotStatus>(
      "robot_status",
      10
    );
    timer_=this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&ExampleInterfacesRobot::timer_callback,this));
  }

首先创建服务

move_robot_server_ = this->create_service<example_ros2_interfaces::srv::MoveRobot>(
  "move_robot", std::bind(&ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2)
);

服务名称为"move_robot"，收到请求之后进入服务函数为handle_move_robot，用于控制机器人移动。

创建发布者

robot_status_publisher_ = this->create_publisher<example_ros2_interfaces::msg::RobotStatus>("robot_status", 10);

话题topic的名称为robot_status，队列长度为10，消息的类型是我们之前定义的example_ros2_interfaces::msg::RobotStatus。

创建定时器

timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&ExampleInterfacesRobot::timer_callback, this));

定时周期为500ms，进入回调函数timer_callback。

2.timer_callback() 回调函数

void timer_callback()
  {
    example_ros2_interfaces::msg::RobotStatus message;
    message.status=robot.get_status();
    message.pose=robot.get_current_pose();
    RCLCPP_INFO(this->get_logger(),"Publishing: %f",robot.get_current_pose());
    robot_status_publisher_->publish(message);
  };

定时器每500ms进入一次timer_callback() 回调函数

首先创建了一个 RobotStatus 消息对象，它属于 example_ros2_interfaces::msg 命名空间，该对象用于存储并传输机器人状态和位置信息。

通过 Robot 类中的 get_status() 方法获取当前的机器人状态STATUS_MOVEING或者STATUS_STOP，通过 Robot 类中的 get_current_pose() 方法获取机器人的当前位置，赋值到message的结构体中。

使用RCLCPP_INFO将当前机器人位置输出到终端。

之后使用 robot_status_publisher_ 发布填充好的 RobotStatus 消息，该消息会发布到"robot_status" 话题，控制节点订阅该话题，可以接受到机器人的状态和位置。

3.handle_move_robot()函数

void handle_move_robot(const std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Request> request,
                         std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Response> response){
    RCLCPP_INFO(this->get_logger(),"cover request moveing distance: %f, current pose: %f",request->distance,robot.get_current_pose());
    robot.move_distance(request->distance);
    response->pose=robot.get_current_pose();
  };

request接口类型是MoveRobot::Request，使用智能指针shared_ptr管理其生命周期，其包含客户端发送的移动距离 (distance) 请求。

response接口类型为MoveRobot::Response，使用指针指针shared_ptr管理其生命周期，用于向客户端返回移动后的机器人位置 (pose)。

使用 RCLCPP_INFO 输出日志信息，输出request->distance即客户端请求的移动距离robot.get_current_pose()即当前机器人的位置信息。

robot.move_distance(request->distance);

之后使用机器人类的move_distance函数移动指定距离。

response->pose = robot.get_current_pose();

并向客户端返回当前机器人的位置信息。

流程图为

[main函数]
   |
   V
[初始化ROS2节点: example_interfaces_robot_01]
   |
   V
[创建 ExampleInterfacesRobot 节点实例]
   |      ├──> 创建 ROS2 服务: move_robot
   |      ├──> 创建 发布者: robot_status
   |      ├──> 创建 定时器: 每500ms发布机器人状态
   |
   V
[事件循环: rclcpp::spin]
   |
   |  ├─> [服务请求处理: handle_move_robot]
   |  |      ├──> 读取请求距离
   |  |      ├──> 调用 robot.move_distance() 逻辑执行移动
   |  |      └──> 返回最终位置
   |
   |  ├─> [定时器触发: timer_callback]
   |  |      ├──> 读取 robot 当前位置信息
   |  |      ├──> 构建 robot_status 消息
   |  |      └──> 发布消息到 /robot_status 话题
   |
   └──> 等待事件 (请求、回调等)

[节点结束: rclcpp::shutdown]

2.控制节点

控制节点代码为

#include "rclcpp/rclcpp.hpp"
#include "example_ros2_interfaces/msg/robot_status.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
class ExampleInterfacesControl:public rclcpp::Node
{
    public:
    ExampleInterfacesControl(std::string name):Node(name)
    {
        RCLCPP_INFO(this->get_logger(),"node has started: %s",name.c_str());
        client_=this->create_client<example_ros2_interfaces::srv::MoveRobot>("move_robot");
        robot_status_subscribe_=this->create_subscription<example_ros2_interfaces::msg::RobotStatus>("robot_status",10,std::bind(&ExampleInterfacesControl::robot_status_callback_,this,std::placeholders::_1));                                                                            
    }
    void move_robot(float distance)
    {
        RCLCPP_INFO(this->get_logger(),"request robot move %f",distance);
        while (!client_->wait_for_service(std::chrono::seconds(1)))
        {
            if(!rclcpp::ok())
            {
                RCLCPP_ERROR(this->get_logger(),"wait be interupted");
                return;
            }
            RCLCPP_INFO(this->get_logger(),"wait server online");
        }
        auto request=std::make_shared<example_ros2_interfaces::srv::MoveRobot::Request>();
        request->distance=distance;
        client_->async_send_request(
            request,std::bind(&ExampleInterfacesControl::result_callback_,this,std::placeholders::_1)
        );
        
    }

    private:
    rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedPtr client_;
    rclcpp::Subscription<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_subscribe_;
    void result_callback_(rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedFuture result_future)
    {
        auto response=result_future.get();
        RCLCPP_INFO(this->get_logger(),"cover move result: %f",response->pose);
    }
    void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(),"cover status pose: %f status: %d",msg->pose,msg->status);
    }
};
int main(int argc,char ** argv)
{
    rclcpp::init(argc,argv);
    auto node =std::make_shared<ExampleInterfacesControl>("example_interfaces_control_01");
    node->move_robot(5.0);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

控制节点创建了ExampleInterfacesControl类用于控制机器人节点移动。

该类的框架为

成员变量：
    client_，服务端，用于发送控制移动的请求
    robot_status_subscribe_，订阅者，用于订阅话题获得机器人的位姿信息
成员函数：
    构造函数，创建客户端、订阅者
    move_robot，服务请求机器人移动指定的距离
    robot_status_callback_，话题回调函数，打印机器人当前状态和位置
    result_callback_，服务结果回调，获取响应中的机器人当前位置

1.构造函数

创建客户端

client_ = this->create_client<example_ros2_interfaces::srv::MoveRobot>("move_robot");

创建一个服务客户端client_，用于调用名为 move_robot 的服务，该服务是我们之前定义好的接口文件里面的MoveRobot类型。

创建订阅者

robot_status_subscribe_=this->create_subscription<example_ros2_interfaces::msg::RobotStatus>("robot_status",10,std::bind(&ExampleInterfacesControl::robot_status_callback_,this,std::placeholders::_1));

创建一个订阅者robot_status_subscribe_，用于订阅第一个参数 robot_status 的主题，接收并处理发布者发布到话题中的RobotStatus 类型消息。第二个参数表示队列长度为10，第三个参数表示回调函数为robot_status_callback_。

2.move_robot函数

void move_robot(float distance)
{
    RCLCPP_INFO(this->get_logger(),"request robot move %f",distance);
    while (!client_->wait_for_service(std::chrono::seconds(1)))
    {
        if(!rclcpp::ok())
        {
            RCLCPP_ERROR(this->get_logger(),"wait be interupted");
            return;
        }
        RCLCPP_INFO(this->get_logger(),"wait server online");
    }
    auto request=std::make_shared<example_ros2_interfaces::srv::MoveRobot::Request>();
    request->distance=distance;
    client_->async_send_request(
        request, std::bind(&ExampleInterfacesControl::result_callback_, this, std::placeholders::_1)
    );
}

接受参数为distance，进入函数后先输出distance到终端上，然后进入循环，等待服务端的上线。

std::chrono::seconds(1) 是一个时间参数，表示最多等待 1 秒，如果1秒之内上线返回true，否则返回false。

当返回false即服务器还没上线时会进入循环内部，如果rclcpp::ok() 返回 false，说明节点被中断，程序会输出错误日志并退出，否则一直打印日志等待服务端上线到终端。

当服务端上线后，构造一个对象request，类型为example_ros2_interfaces::srv::MoveRobot::Request，这是我们之前写的接口文件内的类型。

将request的distance进行赋值，通过异步发送向指定的服务端发送请求，接收到回应后进入服务回调函数result_back。

3.服务回调函数

void result_callback_(rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedFuture result_future)
{
    auto response = result_future.get();
    RCLCPP_INFO(this->get_logger(),"cover move result: %f",response->pose);
}

在异步请求完成后，result_callback_ 回调函数获取响应中的机器人当前位置，从SharedFuture 类型的对象result_future中获得response结果，并将位置信息打印。

4.话题回调函数

void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg)
{
    RCLCPP_INFO(this->get_logger(),"cover status pose: %f status: %d",msg->pose,msg->status);
}

从订阅的消息 msg 中获取机器人当前位置信息 (pose) 和状态码 (status)，msg类型为我们定义的RobotStatus类型，并将其打印到终端。

整个流程为

服务客户端：调用 move_robot 服务，控制机器人移动。
订阅者：监听 robot_status 主题，实时获取机器人状态。
回调机制：result_callback_：处理服务响应。
robot_status_callback_：处理订阅消息。
流程控制：主函数初始化节点，发起服务请求，进入消息循环 (spin)