【Nodelet】系列（二）——Nodelet版相机驱动

目录

一、nodelet编写流程

编写nodelet节点需要继承nodelet类，并按照ROS的要求进行配置，具体步骤如下：

  1. 去掉main函数；
  2. .h中声明子类并继承nodelet类，.cpp中定义子类；
  3. cpp中将类构造函数代码移动到onInit()中，用于初始化；
  4. cpp中添加PLUGINLIB_EXPORT_CLASS宏，将子类声明为插件类，并编译为动态库；
  5. CMakeLists添加nodelet依赖，生成库文件；
  6. package中添加nodelet依赖，并添加export导出插件；
  7. 编写plugin.xml，使库文件成为nodelet的一个插件。

二、camera_nodelet组织结构

该功能包主要功能是通过opencv读取USB相机画面，并通过nodelet将图像数据发布出去。

功能包结构树如下：

~/nodelet_ws/src/camera_nodelet/.
├── include/
│   ├── camera_nodelet/
│      └── my_ros_node.hpp
├── launch/
│   └── camera_nodelet.launch
├── plugins/
│   └── nodelet_plugins.xml
└── src/
    └── my_ros_node.cpp
├── package.xml
├── CMakeLists.txt

三、my_ros_node.hpp

该头文件也包含了若干功能的头文件，如下：

  • nodelet/nodelet.h，继承nodelet类
  • sensor_msgs/Image.h，以便转换为ROS图像消息发布出去
  • cv_bridge/cv_bridge.h，实现opencv数据类型向ROS数据类型转换
  • opencv2/opencv.hpp，实现读取摄像头数据功能
  • pluginlib/class_list_macros.h，实现PLUGINLIB_EXPORT_CLASS宏的使用

my_ros_node.hpp源码如下：

#ifndef MY_ROS_NODE_HPP
#define MY_ROS_NODE_HPP

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/Image.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>

namespace my_ros_node{

class MyRosNode : public nodelet::Nodelet{
public:
    MyRosNode();
    void start(const ros::TimerEvent&);
   // void start();

private:
    void onInit();
    cv::VideoCapture cap;
    ros::Timer timer;
    ros::Publisher pub;
};

} // namespace my_ros_node

#endif

使用nodelet需要继承nodelet类，因此需要创建自定义的类，同时为避免其它文件中存在相同的类名，导致冲突，一般需要添加namespace。

其中onInit()函数在Nodelet加载时被调用，通常用于节点的初始化操作，如创建发布者、订阅者、服务等。这与在普通ROS节点中在main函数中进行的初始化操作类似。

由于需要读取相机画面，并将图像发布出去，可以通过定时器定时读取，同时也可控制图像发布的频率。

最后声明了类的成员变量。

四、my_ros_node.cpp

实现文件中的主要工作如下：

  • 1.通过PLUGINLIB_EXPORT_CLASS声明该文件中的my_ros_node::MyRosNode类是nodelet的插件类型。用法：PLUGINLIB_EXPORT_CLASS(namespace::class_name, nodelet::Nodelet)

  • 2.MyRosNode类的默认构造函数中不需要填写任何代码，所有初始化代码在onInit()中实现。

  • 3.nodelet中通过一个nodehandle管理所有的子类，子类需要通过getPrivateNodeHandle()函数获取nodelet的nodehandle。

my_ros_node.cpp源码如下

#include "my_ros_node.hpp"

PLUGINLIB_EXPORT_CLASS(my_ros_node::MyRosNode,nodelet::Nodelet)

namespace my_ros_node{

MyRosNode::MyRosNode(){}


void MyRosNode::onInit(){
    ros::NodeHandle& nh_ = getPrivateNodeHandle();
    pub = nh_.advertise<sensor_msgs::Image>("image_raw", 1);

    cap = cv::VideoCapture(0);

    if (!cap.isOpened()) {
        std::cerr << "Failed to open USB camera." << std::endl;
    }

    // 设置相机参数
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 1920);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
    cap.set(cv::CAP_PROP_FPS, 30);

    // 图像句柄
    timer = nh_.createTimer(ros::Duration(0.01), &MyRosNode::start, this);
}

void MyRosNode::start(const ros::TimerEvent&)
{
    while (ros::ok()) {
        // 读取图像
        cv::Mat frame;
        if (!cap.read(frame)) {
            std::cerr << "Failed to read USB camera." << std::endl;
        }

        // 将frame图像利用image_transport传输出去
        cv_bridge::CvImage cv_image;
        cv_image.header.stamp = ros::Time::now();
        cv_image.header.frame_id = "camera";
        cv_image.encoding = sensor_msgs::image_encodings::BGR8;
        cv_image.image = frame;

        // 将frame图像转换为sensor_msgs::ImagePtr类型
        sensor_msgs::ImagePtr msg = cv_image.toImageMsg();

        pub.publish(msg);
    }
}

} // namespace my_ros_node

注意，onInit()函数主要完成节点初始化相关工作，如果在onInit()函数内添加代码运行逻辑阻塞在此处，如while循环，会导致子类无法完成初始化，也就无法正常运行nodelet！Nodelet框架期望onInit()方法在执行完所有初始化操作后返回，这样它可以继续进行其他Nodelet插件的初始化和回调处理。如果在onInit()方法无法完成，Nodelet框架将无法继续执行其他操作，这会导致问题。

正确的做法应是在onInit()函数内创建子线程或定时函数，让子线程循环执行代码。

对于读取相机画面而言，不能在onInit()函数中直接读图像画面，否则程序会卡死在这里，onInit()函数不执行完毕，nodelet节点无法正常运行，因此需要用定时器读图像画面。

此处通过设置定时器，用opencv读取相机画面，并通过cv_bridge将其转换为ROS的图像数据类型指针发布出去。

五、CMakeLists.txt

CMakeLists.txt中添加ros相关包与OpenCV的依赖。

cmake_minimum_required(VERSION 3.0.2)
project(camera_nodelet)

add_compile_options(-std=c++11)

find_package(OpenCV REQUIRED)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  nodelet
  cv_bridge
  std_msgs
  sensor_msgs
)

catkin_package(
  # INCLUDE_DIRS include
  # LIBRARIES camera_nodelet
  # CATKIN_DEPENDS roscpp nodelet cv_bridge image_transport std_msgs sensor_msgs usb_cam
  # DEPENDS system_lib
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(${PROJECT_NAME}
  src/my_ros_node.cpp
)

target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  ${OpenCV_LIBS}
)

注意：add_library生成的库名称是${PROJECT_NAME}，即camera_nodelet，对应nodelet_plugins.xml中的library标签库名称<library path="lib/libcamera_nodelet" >

若库名称是${PROJECT_NAME}_nodelet，则nodelet_plugins.xml中的library中的名称是
<library path="lib/libcamera_nodelet_nodelet" >

六、package.xml

package.xml文件中最重要的是需要导出插件配置文件，用于将自定义的这个Nodelet插件与ROS插件系统关联。它告诉插件系统在哪里找到插件的描述文件，以便在运行时动态加载插件。

<?xml version="1.0"?>
<package format="2">
  <name>camera_nodelet</name>
  <version>0.0.0</version>
  <description>The camera_nodelet package</description>

  <maintainer email="wanghuohuo@todo.todo">wanghuohuo</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <exec_depend>roscpp</exec_depend>

  <depend> nodelet </depend>
  <depend> cv_bridge </depend>
  <depend> camera_info_manager </depend>
  <depend> std_msgs </depend>
  <depend> sensor_msgs </depend>

  <export>
    <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />
  </export>
  
</package>

通过export生成一个nodelet的plugin，由标签名称nodelet可知，该插件属于nodelet域内的插件

 <export>
    <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />
  </export>

其中plugin的值为插件描述文件的路径，${prefix}的值为package.xml文件所在的路径。

编译成功后，可以通过下述命令查看该插件是否成功生成。
source devel/setup.bash
rospack plugins --attrib=plugin nodelet

七、nodelet_plugins.xml

nodelet_plugins.xml是一个描述文件，列出了自定义的Nodelet插件中包含的所有类。它包含每个类的名称、类型以及与之关联的共享库。在运行时，ROS插件系统会读取这个文件，以便知道如何加载和实例化类。

<library path="lib/libcamera_nodelet" >
 
  <class name="my_ros_node/MyRosNode" type="my_ros_node::MyRosNode" base_class_type="nodelet::Nodelet">
  <description>
  This is my nodelet.
  </description>
  </class>
</library>

以下是文件内容的详细解释：

<library path="lib/libcamera_nodelet">

这一行定义了包含Nodelet类实现的共享库的路径。path属性指定了共享库的相对路径。在这个例子中，共享库名为libcamera_nodelet，位于ROS工作空间/devel/lib目录下。

<class name="my_ros_node/MyRosNode" type="my_ros_node::MyRosNode" base_class_type="nodelet::Nodelet">

这一行定义了一个Nodelet子类：

  • name属性提供了类的标识符，通常是namespace/ClassName的形式，以便根据该标识符知道对应的命名空间与类名，以/分隔。
  • type属性指定了类的完全限定名，包括其命名空间，即代码中的namespace::类名，以::分隔。
  • base_class_type属性定义了基类类型，通常为nodelet::Nodelet。

此文件的结构允许在同一个库中定义多个Nodelet子类。只需为每个类添加一个<class>标签，并按照上述方式设置属性（name标识符须不同）。

注意：

  • launch文件通过load指令加载自定义的nodelet子类，依赖于nodelet_plugins.xml文件中<class>标签的name属性，而不是type属性。

若nodelet_plugins.xml中class标签内的name属性的值为"aaa/nodeletclass1"，则launch文件对应启动如下：

<node pkg="nodelet" type="nodelet" name="nodeletclass1" args="load aaa/nodeletclass1 manager" output="screen">

八、camera_nodelet.launch

camera_nodelet.launch包含了两个<node>标签，用于启动Nodelet管理器和自定义的Nodelet子类。

<launch>
  <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen"/>
  
  <node pkg="nodelet" type="nodelet" name="my_ros_node" args="load my_ros_node/MyRosNode manager" />
</launch>

启动Nodelet时，首先启动Nodelet管理器。Nodelet管理器是一个特殊的节点，通过属性args="manager"进行设置，负责在同一进程中管理多个Nodelet插件；其它的Nodelet节点通过load命令加载，同时必须指定Nodelet管理器的name，通过属性args="load my_ros_node/MyRosNode manager"设置，以便绑定到该管理器上，从而实现统一管理！

以下是对该launch文件内容的详细解释：

  <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen"/>

这一行用于启动Nodelet管理器，将其命名为"manager"，有时它也会被命名为"standalone_nodelet"，args属性设置了命令行参数manager，从而表明该节点是Nodelet管理器。

  <node pkg="nodelet" type="nodelet" name="my_ros_node" args="load my_ros_node/MyRosNode manager" />

这一行用于加载并启动自定义的Nodelet子类。pkg和type属性与Nodelet管理器相同，args属性设置了命令行参数，其中load指示加载一个Nodelet插件，my_ros_node/MyRosNode对应于nodelet_plugins.xml文件中class标签的name属性值，用于定位Nodelet子类，而"manager"表示将Nodelet子类加载到名为"manager"的Nodelet管理器中。

参考：
1.官方Wiki，http://wiki.ros.org/nodelet
2.ROS nodelet 介绍，https://mazhengg.github.io/2018/04/09/ROS-nodelet-%E4%BB%8B%E7%BB%8D/
3.Nodelet(无处不在的小土)，https://gaoyichao.com/Xiaotu/?book=ros&title=nodelet
4.ROS中的 nodelet，https://zhuanlan.zhihu.com/p/37537823
5.ROS中nodelet的使用，https://blog.csdn.net/zyh821351004/article/details/52143309
6.ROS中nodelet的使用，https://immortalqx.github.io/2020/09/04/ros-notes-2/
7.ROS中nodelet的使用，https://www.cnblogs.com/21207-ihome/p/8213411.html
8.nodelet中plus的源码，https://github.com/ros/nodelet_core/blob/noetic-devel/test_nodelet/src/plus.cpp
9.nodelet中plus的源码，https://github.com/ros/common_tutorials/tree/noetic-devel/nodelet_tutorial_math
10.禾赛雷达的nodelet启动文件，https://github.com/HesaiTechnology/HesaiLidar_Swift_ROS/blob/master/pandar_pointcloud/launch/PandarSwift_points.launch

最后编辑于 ：
©著作权归作者所有,转载或内容合作请联系作者
【社区内容提示】社区部分内容疑似由AI辅助生成，浏览时请结合常识与多方信息审慎甄别。
平台声明：文章内容（如有图片或视频亦包括在内）由作者上传并发布，文章内容仅代表作者本人观点，简书系信息发布平台，仅提供信息存储服务。

相关阅读更多精彩内容

友情链接更多精彩内容