0
点赞
收藏
分享

微信扫一扫

ROS2与C++入门教程-话题组件演示


系列文章目录

  • ​​ROS2与C++入门教程-目录​​
  • ​​ROS2与C++入门教程-新建ros2工作空间​​
  • ​​ROS2与C++入门教程-新建ros2包​​
  • ​​ROS2与C++入门教程-编写订阅和发布​​
  • ​​ROS2与C++入门教程-编写服务端和客户端​​
  • ​​ROS2与C++入门教程-创建消息(msg)文件​​
  • ​​ROS2与C++入门教程-创建服务(srv)文件​​
  • ​​ROS2与C++入门教程-创建ros2接口​​
  • ​​ROS2与C++入门教程-使用参数​​
  • ​​ROS2与C++入门教程-创建和使用插件​​
  • ​​ROS2与C++入门教程-编写动作服务器​​
  • ​​ROS2与C++入门教程-编写动作客户端​​
  • ​​ROS2与C++入门教程-Topic Statistics查看话题​​
  • ​​ROS2与C++入门教程-增加头文件​​
  • ​​ROS2与C++入门教程-在C++包里增加python支持​​
  • ​​ROS2与C++入门教程-增加命名空间​​
  • ​​ROS2与C++入门教程-多线程演示​​
  • ​​ROS2与C++入门教程-单线程演示​​
  • ​​ROS2与C++入门教程-话题组件演示​​
  • ​​ROS2与C++入门教程-话题组件增加launch启动演示​​
  • ​​ROS2与C++入门教程-服务组件演示​​
  • ​​ROS2与C++入门教程-服务组件增加launch启动演示​​
  • ​​ROS2与C++入门教程-进程内(intra_process)话题发布和订阅演示​​
  • ​​ROS2与C++入门教程-进程内(intra_process)话题发布和订阅演示2​​
  • ​​ROS2与C++入门教程-进程内(intra_process)图像处理演示​​
  • ​​ROS2与C++入门教程-生命周期节点演示​​

说明:

  • 介绍如何实现话题组件,利用组件发布话题和订阅话题

步骤:

  • 新建一个包cpp_component_topic

cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake cpp_component_topic

  • 进入include目录,新建文件talker_component.hpp

cd ~/dev_ws/src/cpp_component_topic/include/cpp_component_topic
touch talker_component.hpp

  • 内容如下:

#ifndef COMPOSITION__TALKER_COMPONENT_HPP_
#define COMPOSITION__TALKER_COMPONENT_HPP_

#include "cpp_component_topic/visibility_control.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

namespace composition
{

class Talker : public rclcpp::Node
{
public:
COMPOSITION_PUBLIC
explicit Talker(const rclcpp::NodeOptions & options);

protected:
void on_timer();

private:
size_t count_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
};

} // namespace composition

#endif // COMPOSITION__TALKER_COMPONENT_HPP_

  • 进入include目录,新建文件listener_component.hpp

cd ~/dev_ws/src/cpp_component_topic/include/cpp_component_topic
touch listener_component.hpp

  • 内容如下:

#ifndef COMPOSITION__LISTENER_COMPONENT_HPP_
#define COMPOSITION__LISTENER_COMPONENT_HPP_

#include "cpp_component_topic/visibility_control.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

namespace composition
{

class Listener : public rclcpp::Node
{
public:
COMPOSITION_PUBLIC
explicit Listener(const rclcpp::NodeOptions & options);

private:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

} // namespace composition

#endif // COMPOSITION__LISTENER_COMPONENT_HPP_

  • 进入include目录,新建文件visibility_control.h

cd ~/dev_ws/src/cpp_component_topic/include/cpp_component_topic
touch visibility_control.h

  • 内容如下:

#ifndef COMPOSITION__VISIBILITY_CONTROL_H_
#define COMPOSITION__VISIBILITY_CONTROL_H_

#ifdef __cplusplus
extern "C"
{
#endif

// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define COMPOSITION_EXPORT __attribute__ ((dllexport))
#define COMPOSITION_IMPORT __attribute__ ((dllimport))
#else
#define COMPOSITION_EXPORT __declspec(dllexport)
#define COMPOSITION_IMPORT __declspec(dllimport)
#endif
#ifdef COMPOSITION_BUILDING_DLL
#define COMPOSITION_PUBLIC COMPOSITION_EXPORT
#else
#define COMPOSITION_PUBLIC COMPOSITION_IMPORT
#endif
#define COMPOSITION_PUBLIC_TYPE COMPOSITION_PUBLIC
#define COMPOSITION_LOCAL
#else
#define COMPOSITION_EXPORT __attribute__ ((visibility("default")))
#define COMPOSITION_IMPORT
#if __GNUC__ >= 4
#define COMPOSITION_PUBLIC __attribute__ ((visibility("default")))
#define COMPOSITION_LOCAL __attribute__ ((visibility("hidden")))
#else
#define COMPOSITION_PUBLIC
#define COMPOSITION_LOCAL
#endif
#define COMPOSITION_PUBLIC_TYPE
#endif

#ifdef __cplusplus
}
#endif

#endif // COMPOSITION__VISIBILITY_CONTROL_H_

  • 进入src目录,新建文件talker_component.cpp

cd ~/dev_ws/src/cpp_component_topic/src
touch talker_component.cpp

  • 内容如下:

#include "cpp_component_topic/talker_component.hpp"

#include <chrono>
#include <iostream>
#include <memory>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

namespace composition
{

// Create a Talker "component" that subclasses the generic rclcpp::Node base class.
// Components get built into shared libraries and as such do not write their own main functions.
// The process using the component's shared library will instantiate the class as a ROS node.
Talker::Talker(const rclcpp::NodeOptions & options)
: Node("talker", options), count_(0)
{
// Create a publisher of "std_mgs/String" messages on the "chatter" topic.
pub_ = create_publisher<std_msgs::msg::String>("chatter", 10);

// Use a timer to schedule periodic message publishing.
timer_ = create_wall_timer(1s, std::bind(&Talker::on_timer, this));
}

void Talker::on_timer()
{
auto msg = std::make_unique<std_msgs::msg::String>();
msg->data = "Hello World: " + std::to_string(++count_);
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str());
std::flush(std::cout);

// Put the message into a queue to be processed by the middleware.
// This call is non-blocking.
pub_->publish(std::move(msg));
}

} // namespace composition

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(composition::Talker)

  • 进入src目录,新建文件listener_component.cpp

cd ~/dev_ws/src/cpp_component_topic/src
touch listener_component.cpp

  • 内容如下:

#include "cpp_component_topic/listener_component.hpp"

#include <iostream>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

namespace composition
{

// Create a Listener "component" that subclasses the generic rclcpp::Node base class.
// Components get built into shared libraries and as such do not write their own main functions.
// The process using the component's shared library will instantiate the class as a ROS node.
Listener::Listener(const rclcpp::NodeOptions & options)
: Node("listener", options)
{
// Create a callback function for when messages are received.
// Variations of this function also exist using, for example, UniquePtr for zero-copy transport.
auto callback =
[this](std_msgs::msg::String::ConstSharedPtr msg) -> void
{
RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str());
std::flush(std::cout);
};

// Create a subscription to the "chatter" topic which can be matched with one or more
// compatible ROS publishers.
// Note that not all publishers on the same topic with the same type will be compatible:
// they must have compatible Quality of Service policies.
sub_ = create_subscription<std_msgs::msg::String>("chatter", 10, callback);
}

} // namespace composition

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(composition::Listener)

  • 编译package.xml
  • 在<buildtool_depend>ament_cmake</buildtool_depend>后增加

<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>std_msgs</depend>

  • 编译CMakelist.txt
  • 在find_package(ament_cmake REQUIRED)后增加

find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)

  • 编译为共享库并注册talker_component和listener_component 组件

include_directories(include)
# create ament index resource which references the libraries in the binary dir
set(node_plugins "")

add_library(talker_component SHARED
src/talker_component.cpp)
target_compile_definitions(talker_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(talker_component
"rclcpp"
"rclcpp_components"
"std_msgs")
rclcpp_components_register_nodes(talker_component "composition::Talker")
set(node_plugins "${node_plugins}composition::Talker;$<TARGET_FILE:talker_component>\n")

add_library(listener_component SHARED
src/listener_component.cpp)
target_compile_definitions(listener_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(listener_component
"rclcpp"
"rclcpp_components"
"std_msgs")
rclcpp_components_register_nodes(listener_component "composition::Listener")
set(node_plugins "${node_plugins}composition::Listener;$<TARGET_FILE:listener_component>\n")

  • 生成和安装相关库文件和执行文件

install(TARGETS
talker_component
listener_component
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

  • 安装相关依赖

cd ~/dev_ws/
rosdep install -i --from-path src --rosdistro galactic -y

  • 编译包

colcon build --symlink-install --packages-select cpp_component_topic

  • 加载工作空间

. install/setup.bash

  • 查看工作区中已注册和可用的组件,
  • 执行以下命令:

ros2 component types

  • 效果如下:

$ ros2 component types
cpp_component_topic
composition::Talker
composition::Listener

  • 启动组件容器

ros2 run rclcpp_components component_container

  • 查看组件容器名称

$ ros2 component list
/ComponentManager

  • 加载话题发布组件到容器/ComponentManager

ros2 component load /ComponentManager cpp_component_topic composition::Talker

  • 加载话题订阅组件到容器/ComponentManager

ros2 component load /ComponentManager cpp_component_topic composition::Listener

  • 在运行启动容器组件的终端下显示
  • 效果如下:

$ ros2 run rclcpp_components component_container
[INFO] [1651828582.770254908] [ComponentManager]: Load Library: /home/ubuntu/dev_ws/install/cpp_component_topic/lib/libtalker_component.so
[INFO] [1651828582.771588494] [ComponentManager]: Found class: rclcpp_components::NodeFactoryTemplate<composition::Talker>
[INFO] [1651828582.771636618] [ComponentManager]: Instantiate class: rclcpp_components::NodeFactoryTemplate<composition::Talker>
[INFO] [1651828583.778235165] [talker]: Publishing: 'Hello World: 1'
[INFO] [1651828584.778106888] [talker]: Publishing: 'Hello World: 2'
[INFO] [1651828585.778120521] [talker]: Publishing: 'Hello World: 3'
[INFO] [1651828586.778131945] [talker]: Publishing: 'Hello World: 4'
[INFO] [1651828587.778147302] [talker]: Publishing: 'Hello World: 5'
[INFO] [1651828588.778142863] [talker]: Publishing: 'Hello World: 6'
[INFO] [1651828589.778147971] [talker]: Publishing: 'Hello World: 7'
[INFO] [1651828590.778161248] [talker]: Publishing: 'Hello World: 8'
[INFO] [1651828591.778150505] [talker]: Publishing: 'Hello World: 9'
[INFO] [1651828592.778146954] [talker]: Publishing: 'Hello World: 10'
[INFO] [1651828593.778187735] [talker]: Publishing: 'Hello World: 11'
[INFO] [1651828594.778167919] [talker]: Publishing: 'Hello World: 12'
[INFO] [1651828595.778172696] [talker]: Publishing: 'Hello World: 13'
[INFO] [1651828596.778191892] [talker]: Publishing: 'Hello World: 14'
[INFO] [1651828597.778179234] [talker]: Publishing: 'Hello World: 15'
[INFO] [1651828598.778193295] [talker]: Publishing: 'Hello World: 16'
[INFO] [1651828599.778488985] [talker]: Publishing: 'Hello World: 17'
[INFO] [1651828600.778116399] [talker]: Publishing: 'Hello World: 18'
[INFO] [1651828601.778118538] [talker]: Publishing: 'Hello World: 19'
[INFO] [1651828602.778213187] [talker]: Publishing: 'Hello World: 20'
[INFO] [1651828602.917031454] [ComponentManager]: Load Library: /home/ubuntu/dev_ws/install/cpp_component_topic/lib/liblistener_component.so
[INFO] [1651828602.920950689] [ComponentManager]: Found class: rclcpp_components::NodeFactoryTemplate<composition::Listener>
[INFO] [1651828602.921096170] [ComponentManager]: Instantiate class: rclcpp_components::NodeFactoryTemplate<composition::Listener>
[INFO] [1651828603.778212564] [talker]: Publishing: 'Hello World: 21'
[INFO] [1651828603.778760322] [listener]: I heard: [Hello World: 21]
[INFO] [1651828604.778251609] [talker]: Publishing: 'Hello World: 22'
[INFO] [1651828604.778627732] [listener]: I heard: [Hello World: 22]
[INFO] [1651828605.778229471] [talker]: Publishing: 'Hello World: 23'
[INFO] [1651828605.778546161] [listener]: I heard: [Hello World: 23]
[INFO] [1651828606.778232862] [talker]: Publishing: 'Hello World: 24'
[INFO] [1651828606.778629619] [listener]: I heard: [Hello World: 24]
[INFO] [1651828607.778242430] [talker]: Publishing: 'Hello World: 25'
[INFO] [1651828607.778584759] [listener]: I heard: [Hello World: 25]
[INFO] [1651828608.778185347] [talker]: Publishing: 'Hello World: 26'
[INFO] [1651828608.778519307] [listener]: I heard: [Hello World: 26]
[INFO] [1651828609.778234386] [talker]: Publishing: 'Hello World: 27'
[INFO] [1651828609.778680409] [listener]: I heard: [Hello World: 27]
[INFO] [1651828610.778255996] [talker]: Publishing: 'Hello World: 28'
[INFO] [1651828610.778559937] [listener]: I heard: [Hello World: 28]

举报

相关推荐

0 条评论