ROS2学习记录

雨幕丶

已于 2024-02-28 20:15:32 修改

阅读量628

点赞数 8

分类专栏： ROS 文章标签： c++ linux

于 2023-12-27 19:07:32 首次发布

本文链接：https://blog.csdn.net/qq_41451702/article/details/135252964

版权

ROS 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

七、MultiThreadedExecutor 调用callback

一、创建工作空间

1.创建功能包

cd src
ros2 pkg create 功能包 --build-type ament_cmake --dependencies rclcpp --node-name 可执行程序

2.写源文件（cpp文件）

3.编辑配置文件

主要是CMakeLists.txt和package.xml

cmake_minimum_required(VERSION 3.8)
project(lidar_pkg)

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

# 1.添加ROS 2依赖项
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

find_package(PCL REQUIRED)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

# 2.添加可执行节点
add_executable(lidar_handler src/lidar_handler.cpp )

target_include_directories(lidar_handler PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

target_compile_features(lidar_handler PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

# 3.添加目标依赖项
ament_target_dependencies(
  lidar_handler
  "rclcpp"
  "std_msgs"
  "pcl_conversions"
)
# 4.链接PCL库
target_link_libraries(lidar_handler ${PCL_LIBRARIES})

# 5.安装
install(TARGETS lidar_handler
  DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY
  #config
  #launch
  DESTINATION share/${PROJECT_NAME}/
)

# 安装lib
#install(DIRECTORY lib/
#  DESTINATION lib/
#)

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()

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>lidar_pkg</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>pcl_conversions</depend>
  <depend>pcl_ros</depend>

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

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

4.编译

colcon build     //编译工作空间下的所有功能包

colcon build --packages-select 功能包名    //编译指定功能包

5.执行

source install/setup.bash
ros2 run 功能包 可执行程序

spin()函数会持续调用节点的回调函数，这包括订阅主题的回调、定时器的回调、服务的回调等。

rclcpp::ok() 用于检查节点是否仍然处于运行状态。只有当 rclcpp::ok() 返回 true 时，节点的主要逻辑才会执行。如果 rclcpp::ok() 返回 false，表示节点已经收到终止信号或其他关闭请求，循环将终止，节点将执行清理工作并退出。

二、通信模型

话题通信：是一种单向通信模型，在通信双方中，发布方发布数据，订阅方订阅数据，数据流单向的由发布方传输到订阅方。

//用定时器 发布消息
using namespace std::chrono_literals;

rclcpp::TimerBase::SharedPtr timer_ = 
    this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));

//发布消息对象
rclcpp::Publisher<数据类型>::SharedPtr publisher_;

void timer_callback(){
  publisher_->publish(stu);
}


//也可以用线程发布消息

void receiveThread()
{
    while(rclcpp::ok())
    {
       //具体处理
    }

}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Ars548ProcessNode>();
    std::thread receive_thread(receiveThread);
    rclcpp::spin(node);
    receive_thread.join();
    rclcpp::shutdown();

    return 0;
}

//也可以在订阅话题的回调函数中发布消息

//订阅消息

rclcpp::Subscription<数据类型>::SharedPtr subscription_ = 
    this->create_subscription<数据类型>("topic_stu", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));

void topic_callback(const Xxx& msg) const{
     //输出msg的数据
}

服务通信：是一种基于请求响应的通信模型，在通信双方中，客户端发送请求数据到服务端，服务端响应结果给客户端。

动作通信：是一种带有连续反馈的通信模型，在通信双方中，客户端发送请求数据到服务端，服务端响应结果给客户端，但是在服务端接收到请求到产生最终响应的过程中，会发送中间连续的反馈（进度）信息到客户端。

参数服务：是一种基于共享的通信模型，在通信双方中，服务端可以设置数据，而客户端可以连接服务端并操作服务端数据。

三、自定义消息

1.创建编辑.msg文件

功能包下新建msg文件夹，文件夹中创建.msg文件

2.编辑配置文件

package.xml中添加依赖包

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>base_interfaces_demo</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="yhw@todo.todo">yhw</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

   <!--编译依赖-->
   <build_depend>rosidl_default_generators</build_depend>
 
   <!--执行依赖-->
   <exec_depend>rosidl_default_runtime</exec_depend>
  
   <!--声明当前包所属的功能包组-->
   <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

cmakelists.txt添加

find_package(rosidl_default_generators REQUIRED)
 
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/xxx.msg"
)

编译

colcon build --packages-select 功能包

3.测试

编译完成，在工作空间下的install目录下将生成xxx.msg文件对应的C++和Python文件

source install/setup.bash
ros2 interface show 功能包/msg/定义的消息

四、自定义消息示例

说明：工作空间下有两个功能包，自定义消息功能包和订阅或发布消息处理功能包。

自定义消息功能包

注意：自定义消息文件名大写，定义的变量名需要小写。

CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)


rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Xxxx.msg"
 ）

Package.xml

 <buildtool_depend>ament_cmake</buildtool_depend>

 <build_depend>rosidl_default_generators</build_depend>
 <exec_depend>rosidl_default_runtime</exec_depend>
 <member_of_group>rosidl_interface_packages</member_of_group>

编译

先指定编译自定义消息功能包，编译后在install/自定义消息功能包名/include中看到生成的头文件

具体消息处理功能包

1.添加自定义消息功能包头文件

2.使用自定义消息类型当发布话题数据的类型

rclcpp::Publisher<ars548_msg::msg::ObjectList>::SharedPtr objects_pub;  
rclcpp::Publisher<ars548_msg::msg::DetectionList>::SharedPtr detections_pub;
rclcpp::Publisher<ars548_msg::msg::RadarBasicStatus>::SharedPtr status_pub;

objects_pub = this->create_publisher<ars548_msg::msg::ObjectList>("/ars548_process/object_list", 10);
detections_pub = this->create_publisher<ars548_msg::msg::DetectionList>("/ars548_process/detection_list", 10);
status_pub = this->create_publisher<ars548_msg::msg::RadarBasicStatus>("/ars548_process/radar_status", 10);

3.CMakeLists.txt

#找自定义消息功能包
find_package(ars548_msg REQUIRED)

add_executable(ars548_process_node src/ars548_process_node.cpp)

ament_target_dependencies(
  ars548_process_node
  "rclcpp"
  "std_msgs"
  "sensor_msgs"
  "geometry_msgs"
  "ars548_msg"
)

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

4.Package.xml

 <depend>ars548_msg</depend>

5.运行

使用ros2 launch运行多个节点，需要在cmakelists配置

#在ament_package()前面添加
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}/
)

五、ros代码调试方法

1.使用gdb调试

cmakelists添加

add_compile_options(-g)

节点node

ros2 run --prefix 'gdb -ex run --args' <pkg> <node>

在gdb中使用backtrace查看错误提示

(gdb) backtrace
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff79cc859 in __GI_abort () at abort.c:79
#2  0x00007ffff7c52951 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff7c5e47c in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff7c5e4e7 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff7c5e799 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff7c553eb in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x000055555555936c in std::vector<int, std::allocator<int> >::_M_range_check (
    this=0x5555555cfdb0, __n=100) at /usr/include/c++/9/bits/stl_vector.h:1070
#8  0x0000555555558e1d in std::vector<int, std::allocator<int> >::at (this=0x5555555cfdb0,
    __n=100) at /usr/include/c++/9/bits/stl_vector.h:1091
#9  0x000055555555828b in GDBTester::VectorCrash (this=0x5555555cfb40)
    at /home/steve/Documents/nav2_ws/src/gdb_test_pkg/src/gdb_test_node.cpp:44
#10 0x0000555555559cfc in main (argc=1, argv=0x7fffffffc108)
    at /home/steve/Documents/nav2_ws/src/gdb_test_pkg/src/main.cpp:25

1.main主函数25行调用了VectorCrash函数

2.在 VectorCrash 的第 44 行，在 Vector 的 at() 方法中输入100 导致崩溃

3.在STL第1091行的at()函数中因范围检查失败而抛出异常，然后崩溃了。。

launch文件

start_sync_slam_toolbox_node = Node(
    parameters=[
      get_package_share_directory("slam_toolbox") + '/config/mapper_params_online_sync.yaml',
      {'use_sim_time': use_sim_time}
    ],
    package='pkg',
    executable='node',
    name='node_name',
    prefix=['gdb -ex run --args'],      #和官方提示的有区别，不需要xterm
    output='screen')

参考官方文档

2.backward_ros

sudo apt install ros-humble-backward-ros

这个功能包快速实现用GDB调试ROS2程序，使用注意三个步骤：

①package.xml添加

<depend>backward_ros</depend>

②CMakelists.txt

find_package(backward_ros REQUIRED)

③使用Debug 或者 RelWithDebInfo选项编译程序

colcon build --cmake-args '-DCMAKE_BUILD_TYPE=RelWithDebInfo'

鱼香社区有大佬提供了运行示例，

git clone https://github.com/shoufei403/gdb_test.git

colcon build --cmake-args '-DCMAKE_BUILD_TYPE=RelWithDebInfo'

source install/setup.bash

ros2 run gdb_test gdb_test

疑问：实际运行提示如下，并没有定位到代码出问题的位置？根据提示只能发现构造函数出问题，有知道的大佬可以在评论区解答一下，非常感谢！！！

六、launch文件写法

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            namespace='turtlesim1',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='turtlesim',
            namespace='turtlesim2',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='turtlesim',
            executable='mimic',
            name='mimic',
            remappings=[
                ('/input/pose', '/turtlesim1/turtle1/pose'),     #订阅/input/pose话题实际是/turtlesim1/turtle1/pose话题的消息
                ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'),
            ]
        )
    ])

namespace:启动两个turtlesim节点时，它们之间的唯一区别是它们的命名空间值。唯一的命名空间允许系统启动两个节点，而不会出现节点名称或主题名称冲突。该系统中的两只乌龟都接收同一主题的命令并发布它们在同一主题上的pose。通过独特的命名空间，可以区分针对不同海龟的消息。

package.xml添加依赖：

<exec_depend>ros2launch</exec_depend>

更详细的launch写法参考官方文档

2.使用参数服务器从配置文件yaml读取参数值

launch文件需要添加yaml文件路径

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():

    config = os.path.join(
        get_package_share_directory('sd_lidar_pkg'),
        'config',
        'parameter.yaml'
    )


    return LaunchDescription([
        Node(
            package='sd_lidar_pkg',
            executable='lidar_handler',
            name='lidar_handler_node',
            parameters=[config]
        )
    ])

yaml文件，关于命名空间的具体写法参考文档

# /**:
/lidar_handler_node:
  ros__parameters:
    value: 58

cpp文件读取参数的值：

// 读取参数值
declare_parameter<int>("value", 0);
int myParameterValue;
get_parameter("value", myParameterValue);
// 打印参数值
RCLCPP_INFO(this->get_logger(), "My Parameter Value: %d", myParameterValue);

代码框架：

功能包/
├── config
│   └── params.yaml
├── launch
│   └── test_params.launch.py
├── package.xml
├── src

参考文档

七、MultiThreadedExecutor 调用callback

ros2的回调函数默认是串行执行的，比如下面代码订阅了两个消息，第一个消息的回调是个死循环，会一直执行，导致第二个消息的回调阻塞。可以通过多线程MultiThreadedExecutor调用回调组callback groups解决这个问题。

sub_1 = create_subscription<std_msgs::msg::Int32>("topic1", 10, std::bind(&DecisionHandlerNode::topic_callback1, this,std::placeholders::_1));
sub_2 = create_subscription<std_msgs::msg::Int32>("topic2", 10, std::bind(&DecisionHandlerNode::topic_callback2, this,std::placeholders::_1));

void topic_callback1(std_msgs::msg::Int32 aaa)
{
    RCLCPP_INFO(this->get_logger(),"回调函数1");

      while(1){
       
      }
}

void topic_callback2(std_msgs::msg::Int32 bbb){
    RCLCPP_INFO(this->get_logger(),"回调函数2");
}

ROS 2 提供了两种不同类型的回调组（callback groups）来控制回调函数的执行：

互斥回调组（Mutually Exclusive Callback Group）：阻止其回调并行执行 - 实际上使得回调组中的回调像由单线程执行器执行一样。（创建订阅的时候没有指定任何回调组，默认使用互斥回调组。）
可重入回调组（Reentrant Callback Group）：允许执行器按照任意方式调度和执行组中的回调，没有任何限制。这意味着，除了不同的回调之间可以并行运行外，同一个回调的不同实例也可以同时执行。(只要来一个消息就调用一个回调函数，同一回调的不同实例可以并发执行，比如一个订阅者前一个消息的回调函数还没处理完，后一个消息也来了，那么它会不等前一个回调函数执行完就并发执行该回调函数）。

1.使用MultiThreadedExecutor 调用 可重入回调组Reentrant Callback Group

class DecisionHandlerNode : public rclcpp::Node
{
public:

    DecisionHandlerNode();

private:

    rclcpp::CallbackGroup::SharedPtr my_callback_group_1;
    rclcpp::CallbackGroup::SharedPtr my_callback_group_2;

    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_1;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_2;

    void topic_callback1(std_msgs::msg::Int32 aaa)
    {
        RCLCPP_INFO(this->get_logger(),"回调函数1");
      while(1){
       
      }
    }

    void topic_callback2(std_msgs::msg::Int32 bbb){
        RCLCPP_INFO(this->get_logger(),"回调函数2");
    }
};


DecisionHandlerNode::DecisionHandlerNode() : Node("decision_handler")
{

    my_callback_group_1 = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    // my_callback_group_2 = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    auto sub1_opt = rclcpp::SubscriptionOptions();
    sub1_opt.callback_group = my_callback_group_1;
    // auto sub2_opt = rclcpp::SubscriptionOptions();
    // sub2_opt.callback_group = my_callback_group_2;

    sub_1 = create_subscription<std_msgs::msg::Int32>("topic1", 10, std::bind(&DecisionHandlerNode::topic_callback1, this,std::placeholders::_1),sub1_opt);
    sub_2 = create_subscription<std_msgs::msg::Int32>("topic2", 10, std::bind(&DecisionHandlerNode::topic_callback2, this,std::placeholders::_1),sub1_opt);

}

int main(int argc, char **argv)
{
    
    rclcpp::init(argc, argv);
    rclcpp::executors::MultiThreadedExecutor executor;
    auto node = std::make_shared<DecisionHandlerNode>();
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;   
}

参考文档

ros2提供了multithreaded_executor的demo，参考文章，讲的很详细.

雨幕丶

关注

8
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
ROS2学习记录

1.创建编辑.msg文件功能包下新建msg文件夹，文件夹中创建.msg文件2.编辑配置文件package.xml中添加依赖包--编译依赖-->--执行依赖-->--声明当前包所属的功能包组--><export></export></package>cmakelists.txt添加编译colcon build --packages-select 功能包3.测试编译完成，在工作空间下的install目录下将生成xxx.msg文件对应的C++和Python文件。
复制链接

扫一扫