一、新建工作空间
//设置ROS2的环境变量。
source /opt/ros/humble/setup.bash //找不到包时可以通过此命令找到
//创建新的工作空间
mkdir -p ~/my_work_place/src
cd ~/my_work_place/src
//放置样例代码
git clone https://github.com/ros/ros_tutorials.git -b humble-devel
//查看包结构
~/my_work_place/src$ ls ros_tutorials/
roscpp_tutorials rospy_tutorials ros_tutorials turtlesim
//解决依赖问题
rosdep install -i --from-path src --rosdistro humble -y
//在工作空间的根目录my_work_place下编译代码
colcon build
*编译工具安装：sudo apt install python3-colcon-common-extensions
* colcon build后边还可以跟一些常用的参数：
--packages-up-to ：编译指定的功能包，而不是整个工作空间；
--symlink-install ：节省每次重建python脚本的时间；
--event-handlers console_direct+ ：在终端中显示编译过程中的详细日志；
//重新打开一个终端，然后来设置my_work_place工作空间的环境变量
cd ~/my_work_place
. install/local_setup.sh
* install里有两个很类似的文件：local_setup.sh和setup.sh；
* 前者仅会设置当前工作空间中功能包的相关环境变量，后者还会设置该工作空间下其他底层工作空间的环境变量。
或者用（. install/setup.bash）
二、新建ros2包
cd ~/my_work_place/src
//新建名为my_package的包
ros2 pkg create --build-type ament_cmake --node-name my_node my_package
*--node-name为可选参数；
//回工作空间目录编译
cd ..
colcon build
//单独编译一个包：colcon build --package-select my_package
//刷新环境变量
. install/setup.bash
//运行功能包
ros2 run my_package my_node
三、订阅和发布
1、发布
//新建包cpp_pubsub
cd ~/my_workplace/src
ros2 pkg create --build-type ament_cmake cpp_pubsub
//创建cpp文件
cd cpp_pubsub/src
gedit publisher_member_function.cpp
//publisher_member_function的C++代码
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
private:
void timer_callback()
{
auto message = std_msgs::msg::String();
message.data = "Hello, world! " + std::to_string(count_++);
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
publisher_->publish(message);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
//编写package.xml，添加依赖
<depend>rclcpp</depend>
<depend>std_msgs</depend>
//编写CMakelist.txt，添加依赖
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
install(TARGETS
talker
DESTINATION lib/${PROJECT_NAME})
//安装相关依赖
cd ~/my_work_place/
rosdep install -i --from-path src --rosdistro humble -y
//编译
colcon build --symlink-install --packages-select cpp_pubsub
//刷新环境变量
. install/setup.bash
//运行发布程序
ros2 run cpp_pubsub talker
2、订阅
cd ~/my_work_place/src/cpp_pubsub/src
gedit subscriber_member_function.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using std::placeholders::_1;
class MinimalSubscriber : public rclcpp::Node
{
public:
MinimalSubscriber()
: Node("minimal_subscriber")
{
subscription_ = this->create_subscription<std_msgs::msg::String>(
"topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
}
private:
void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
{
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
}
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalSubscriber>());
rclcpp::shutdown();
return 0;
}
cd ~/my_work_place/
//安装相关依赖
rosdep install -i --from-path src --rosdistro humble -y
//编译包
colcon build --symlink-install --packages-select cpp_pubsub
//刷新环境变量
. install/setup.bash
//运行订阅程序
ros2 run cpp_pubsub listener
四、服务端和客户端
1、服务端
cd ~/my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_srvcli --dependencies rclcpp example_interfaces
cd cpp_srvcli/src
gedit add_two_ints_server.cpp
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <memory>
void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response)
{
response->sum = request->a + request->b;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
request->a, request->b);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");
rclcpp::spin(node);
rclcpp::shutdown();
}
//修改CMakeLists.txt，增加如下行
add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(server rclcpp example_interfaces)
install(TARGETS
server
DESTINATION lib/${PROJECT_NAME})
2、客户端
cd cpp_srvcli/src
gedit add_two_ints_client.cpp
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
if (argc != 3) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");
return 1;
}
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");
rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =
node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = atoll(argv[1]);
request->b = atoll(argv[2]);
while (!client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
}
rclcpp::shutdown();
return 0;
}
//编写CMakelist.txt文件
cmake_minimum_required(VERSION 3.5)
project(cpp_srvcli)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(example_interfaces REQUIRED)
add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(server
rclcpp example_interfaces)
add_executable(client src/add_two_ints_client.cpp)
ament_target_dependencies(client
rclcpp example_interfaces)
install(TARGETS
server
client
DESTINATION lib/${PROJECT_NAME})
ament_package()
//安装依赖包
rosdep install -i --from-path src --rosdistro<distro> -y
//编译
colcon build --package-select cpp_srvcli
//运行服务端
cd ~/my_work_place
. install/setup.bash
ros2 run cpp_srvcli server
//运行客户端
cd ~/my_work_place
. /install/setup.bash
ros2 run cpp_srvcli client 2 3
五、创建消息（msg）文件
cd ~/my_work_place
ros2 pkg create --build-type ament_cmake tutorial_interfaces
cd ~/tutorial_interfaces
mkdir msg
cd msg
gedit Num.msg
//Num.msg文件内容
int64 num //定义了一个64为的整数
//编辑CMakelist.txt，添加下面几行
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Num.msg"
)
//编辑package.xml，增加下面几行
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
//编译
cd ~/my_work_place
colcon build --symlink-install --packages-select tutorial_interfaces
. install/setup.bash
//查询消息接口信息
ros2 interface show tutorial_interfaces/msg/Num
测试，使用cpp_pubsub包的发布与订阅；
//复制publisher_member_function.cpp到publisher_member_function_num.cpp
cd ~/my_work_place/src/cpp_pubsub/src
cp publisher_member_function.cpp publisher_member_function_num.cpp
//发布改版
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp" // CHANGE
using namespace std::chrono_literals;
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<tutorial_interfaces::msg::Num>("topic", 10); // CHANGE
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
private:
void timer_callback()
{
auto message = tutorial_interfaces::msg::Num(); // CHANGE
message.num = this->count_++; // CHANGE
RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", message.num); // CHANGE
publisher_->publish(message);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<tutorial_interfaces::msg::Num>::SharedPtr publisher_; // CHANGE
size_t count_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
cp subscriber_member_function.cpp subscriber_member_function_num.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp" // CHANGE
using std::placeholders::_1;
class MinimalSubscriber : public rclcpp::Node
{
public:
MinimalSubscriber()
: Node("minimal_subscriber")
{
subscription_ = this->create_subscription<tutorial_interfaces::msg::Num>( // CHANGE
"topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
}
private:
void topic_callback(const tutorial_interfaces::msg::Num::SharedPtr msg) const // CHANGE
{
RCLCPP_INFO(this->get_logger(), "I heard: '%d'", msg->num); // CHANGE
}
rclcpp::Subscription<tutorial_interfaces::msg::Num>::SharedPtr subscription_; // CHANGE
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalSubscriber>());
rclcpp::shutdown();
return 0;
}
//修改CMakelist.txt
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tutorial_interfaces REQUIRED) # CHANGE
add_executable(talker_num src/publisher_member_function_num.cpp) # CHANGE
ament_target_dependencies(talker_num rclcpp tutorial_interfaces) # CHANGE
add_executable(listener_num src/subscriber_member_function_num.cpp) # CHANGE
ament_target_dependencies(listener_num rclcpp tutorial_interfaces) # CHANGE
install(TARGETS
talker
listener
talker_num # CHANGE
listener_num # CHANGE
DESTINATION lib/${PROJECT_NAME})
ament_package()
//修改package.xml，增加如下代码
<depend>tutorial_interfaces</depend>
//编译
cd ~/my_work_place
colcon build --package-select cpp_pubsub
//运行
ros2 run cpp_pubsub talker_num
ros2 run cpp_pubsub listener_num
六、创建服务（srv）文件
//创建一个包，这我使用之前创建的
cd ~/my_work_place/src
ros2 pkg create --build-type ament_cmake tutorial_interfaces
cd ~/tutorial_interfaces
mkdir srv
cd srv
gedit AddThreeInts.srv
//定义a b c三个输入，输出sum，都是整数
int64 a
int64 b
int64 c
---
int64 sum
//编辑CMakeLists.txt，增加如下行
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/AddThreeInts.srv"
)
//编辑package.xml，增加如下行
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
//编译
cd ~/my_work_place
colcon build --symlink-install --packages-select tutorial_interfaces
. install/srtup.bash
//查询消息接口
ros2 interfaces show tutorial_interfaces/srv/AddThreeInts
测试，使用之前的cpp_pubsub包的发布；
//修改服务端，新建文件_srv
gedit add_two_ints_server_srv.cpp
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp" // CHANGE
#include <memory>
void add(const std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Request> request, // CHANGE
std::shared_ptr<tutorial_interfaces::srv::AddThreeInts::Response> response) // CHANGE
{
response->sum = request->a + request->b + request->c; // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld" " c: %ld", // CHANGE
request->a, request->b, request->c); // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_server"); // CHANGE
rclcpp::Service<tutorial_interfaces::srv::AddThreeInts>::SharedPtr service = // CHANGE
node->create_service<tutorial_interfaces::srv::AddThreeInts>("add_three_ints", &add); // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add three ints."); // CHANGE
rclcpp::spin(node);
rclcpp::shutdown();
}
//修改客户端，新建文件_srv
gedit add_two_ints_server_srv.cpp
#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/srv/add_three_ints.hpp" // CHANGE
#include <chrono>
#include <cstdlib>
#include <memory>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
if (argc != 4) { // CHANGE
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_three_ints_client X Y Z"); // CHANGE
return 1;
}
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_three_ints_client"); // CHANGE
rclcpp::Client<tutorial_interfaces::srv::AddThreeInts>::SharedPtr client = // CHANGE
node->create_client<tutorial_interfaces::srv::AddThreeInts>("add_three_ints"); // CHANGE
auto request = std::make_shared<tutorial_interfaces::srv::AddThreeInts::Request>(); // CHANGE
request->a = atoll(argv[1]);
request->b = atoll(argv[2]);
request->c = atoll(argv[3]); // CHANGE
while (!client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_three_ints"); // CHANGE
}
rclcpp::shutdown();
return 0;
}
//修改CMakelist.txt
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tutorial_interfaces REQUIRED) # CHANGE
add_executable(server_srv src/add_two_ints_server_srv.cpp)
ament_target_dependencies(server_srv
rclcpp tutorial_interfaces) #CHANGE
add_executable(client_srv src/add_two_ints_client_srv.cpp)
ament_target_dependencies(client_srv
rclcpp tutorial_interfaces) #CHANGE
install(TARGETS
server_srv
client_srv
DESTINATION lib/${PROJECT_NAME})
ament_package()
//修改package.xml 增加依赖
<depend>tutorial_interfaces</depend>
//编译
colcon build --packages-select cpp_srvcli
//运行
ros2 run cpp_srvcli server_srv
ros2 run cpp_srvcli client_srv 2 3 4
七、创建ros2接口
//以创建一个msg类型的接口为例
cd ~/my_work_place/src
ros2 pkg create --build-type ament_cmake more_interfaces
mkdir more_interfaces/msg
gedit AddressBook.msg
bool FEMALE=true
bool MALE=false
string first_name
string last_name
bool gender
uint8 age
string address
//在package.xml中添加如下代码
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
//在CMakelist.txt添加如下代码
find_package(rosidl_default_generators REQUIRED)
set(msg_files
"msg/AddressBook.msg"
)
rosidl_generate_interfaces(${PROJECT_NAME}
${msg_files}
)
ament_export_dependencies(rosidl_default_runtime)
//在src目录下，新建publish_address_book.cpp
gedit publish_address_book.cpp
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "more_interfaces/msg/address_book.hpp"
using namespace std::chrono_literals;
class AddressBookPublisher : public rclcpp::Node
{
public:
AddressBookPublisher()
: Node("address_book_publisher")
{
address_book_publisher_ =
this->create_publisher<more_interfaces::msg::AddressBook>("address_book", 10);
auto publish_msg = [this]() -> void {
auto message = more_interfaces::msg::AddressBook();
message.first_name = "John";
message.last_name = "Doe";
message.age = 30;
message.gender = message.MALE;
message.address = "unknown";
std::cout << "Publishing Contact\nFirst:" << message.first_name <<
" Last:" << message.last_name << std::endl;
this->address_book_publisher_->publish(message);
};
timer_ = this->create_wall_timer(1s, publish_msg);
}
private:
rclcpp::Publisher<more_interfaces::msg::AddressBook>::SharedPtr address_book_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<AddressBookPublisher>());
rclcpp::shutdown();
return 0;
}
//修改CMakelist.txt 增加如下内容：
find_package(rclcpp REQUIRED)
add_executable(publish_address_book
src/publish_address_book.cpp
)
ament_target_dependencies(publish_address_book
"rclcpp"
)
install(TARGETS publish_address_book
DESTINATION lib/${PROJECT_NAME})
rosidl_target_interfaces(publish_address_book
${PROJECT_NAME} "rosidl_typesupport_cpp")
//编译
cd ~/my_work_place
colcon run build --packages-up-to more_interfaces
. install/setup.bash
ros2 run more_interfaces publish_address_book
//打开新终端，查看topic
. install/setup.bash
ros2 topic echo /address_book
八、在ros2中使用参数
cd ~/my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_parameters --dependencies rclcpp
mkdir cpp_parameters/src
gedit cpp_parameters_node.cpp
#include <rclcpp/rclcpp.hpp>
#include <chrono>
#include <string>
#include <functional>
using namespace std::chrono_literals;
class ParametersClass: public rclcpp::Node
{
public:
ParametersClass()
: Node("parameter_node")
{
this->declare_parameter<std::string>("my_parameter", "world");
timer_ = this->create_wall_timer(
1000ms, std::bind(&ParametersClass::respond, this));
}
void respond()
{
this->get_parameter("my_parameter", parameter_string_);
RCLCPP_INFO(this->get_logger(), "Hello %s", parameter_string_.c_str());
}
private:
std::string parameter_string_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ParametersClass>());
rclcpp::shutdown();
return 0;
}
//修改CMakeLists.txt
add_executable(parameter_node src/cpp_parameters_node.cpp)
ament_target_dependencies(parameter_node rclcpp)
install(TARGETS
parameter_node
DESTINATION lib/${PROJECT_NAME}
)
//安装依赖
cd ~/my_work_place
rosdep install -i --from-path src --rosdistro humble -y
//编译
colcon build --packages-select cpp_parameters
. install/setup.bash
//运行
ros2 run cpp_parameters parameter_node
//查看值,get命令
ros2 param get /parameter_node my_parameter
//在终端下更改参数值,set命令
ros2 param list
ros2 param set /parameter_node my_parameter earth
//再次查看
ros2 param get /parameter_node my_parameter
九、创建和使用插件（plugin）
因为很多时候我们需要对产品进行包装，核心的代码是只留下输入输出接口的，所以我们使用plugin来实现 .so文件 的封装以及动态调取。
实现一个插件主要需要以下几个步骤：
- 创建基类，定义统一的接口；如果是基于现有的基类实现plugin，则不需要这个步骤
- 创建plugin类，继承基类，实现统一的接口；
- 注册插件；
- 编译生成插件的动态链接库；
- 将插件加入ROS系统；
//安装pluginlib
sudo apt-get install ros-humble-pluginlib
//创建一个新包,基类
cd my_work_place/src
ros2 pkg create --build-type ament_cmake polygon_base --dependencies pluginlib --node-name area_node
//编辑文件,这里创建了个RegularPolygon抽象基类，后边的插件类就是继承该类
gedit polygon_base/include/polygon_base/regular_polygon.hpp
#ifndef POLYGON_BASE_REGULAR_POLYGON_HPP
#define POLYGON_BASE_REGULAR_POLYGON_HPP
namespace polygon_base
{
class RegularPolygon
{
public:
virtual void initialize(double side_length) = 0;
virtual double area() = 0;
virtual ~RegularPolygon(){}
protected:
RegularPolygon(){}
};
} // namespace polygon_base
#endif // POLYGON_BASE_REGULAR_POLYGON_HPP
//在my_work_place/src/polygon_base/CMakeLists.txt添加如下代码，要在ament_target_dependencies命令后添加以下行
install(
DIRECTORY include/
DESTINATION include
)
//在命令之前添加此ament_package命令
ament_export_include_directories(
include
)
//在基类的基础上创建插件包(继承)
cd my_work_place/src
ros2 pkg craete --build-type ament_cmake polygon_plugins --dependencies polygon_base pluginlib --library-name polygon_plugins
//编写c++文件
gedit my_work_place/src/polygon_plugins/src/polygon_plugins.cpp
#include <polygon_base/regular_polygon.hpp>
#include <cmath>
namespace polygon_plugins
{
class Square : public polygon_base::RegularPolygon
{
public:
void initialize(double side_length) override
{
side_length_ = side_length;
}
double area() override
{
return side_length_ * side_length_;
}
protected:
double side_length_;
};
class Triangle : public polygon_base::RegularPolygon
{
public:
void initialize(double side_length) override
{
side_length_ = side_length;
}
double area() override
{
return 0.5 * side_length_ * getHeight();
}
double getHeight()
{
return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
}
protected:
double side_length_;
};
}
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)
这里创建了两个继承自基础类RegularPolygon的插件子类Triangle和Square。
//在plugins.xml添加以下内容
<library path="polygon_plugins">
<class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
<description>This is a square plugin.</description>
</class>
<class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
<description>This is a triangle plugin.</description>
</class>
</library>
//在polygon_plugins/CMakelist.txt添加以下内容
add_library(polygon_plugins src/polygon_plugins.cpp)
target_include_directories(polygon_plugins PUBLIC
<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
<INSTALL_INTERFACE:include>)
ament_target_dependencies(
polygon_plugins
polygon_base
pluginlib
)
pluginlib_export_plugin_description_file(polygon_base plugins.xml)
install(
TARGETS polygon_plugins
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
//在ament_package命令之前添加以下内容
ament_export_libraries(
polygon_plugins
)
ament_export_targets(
export_${PROJECT_NAME}
)
//使用插件
//新建文件area_node.cpp
#include <pluginlib/class_loader.hpp>
#include <polygon_base/regular_polygon.hpp>
int main(int argc, char** argv)
{
// To avoid unused parameter warnings
(void) argc;
(void) argv;
pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("polygon_base", "polygon_base::RegularPolygon");
try
{
std::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createSharedInstance("polygon_plugins::Triangle");
triangle->initialize(10.0);
std::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createSharedInstance("polygon_plugins::Square");
square->initialize(10.0);
printf("Triangle area: %.2f\n", triangle->area());
printf("Square area: %.2f\n", square->area());
}
catch(pluginlib::PluginlibException& ex)
{
printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
}
return 0;
}
cd my_work_place
//colcon命令选中了polgon_base包和他的应用基类包polgon_plugins
colcon build --packages-select polygon_base polygon_plugins
. install/setup.bash
ros2 run polygon_base area_node
十、动作服务端&客户端（action）
1、服务端（action-server）
cd my_work_place/src
ros2 pkg create --dependences action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components --action_tutorials_cpp
//添加可见性控制，在action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h添加以下内容
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__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 ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
#define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
#else
#define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
#define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
#else
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#else
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_IMPORT
#if __GNUC__ >= 4
#define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
//编写动作服务器代码
#include <functional>
#include <memory>
#include <thread>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "action_tutorials_cpp/visibility_control.h"
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
ACTION_TUTORIALS_CPP_PUBLIC
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class FibonacciActionServer
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
//编译动作服务器，在action_tutorials_cpp/CMakeLists.txt添加以下内容
//并在find_package调用后添加以下内容
add_library(action_server SHARED
src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
cd my_work_place
. install/setup.bash
ros2 run action_tutorials_cpp fibonacci_action_server
2、客户端（action-client）
cd my_work_place/src/action_tutorials_cpp/src
gedit fibonacci_action_client.cpp
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
auto goal_handle = future.get();
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(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const GoalHandleFibonacci::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;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)
//在action_tutorials_cpp/CMakeLists.txt添加以下内容
add_library(action_client SHARED
src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
action_client
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
//编译
cd my_work_place
colcon build
//运行
ros2 run action_tutorials_cpp fibonacci_action_client
十一、Topic Statistics查看话题
//以cpp_pubsub为示例
cd my_work_place/src/cpp_pubsub/src
//下载示例
wget -O member_function_with_topic_statistics.cpp https://raw.githubusercontent.com/ros2/examples/master/rclcpp/topics/minimal_subscriber/member_function_with_topic_statistics.cpp
//编辑下载的文件
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/subscription_options.hpp"
#include "std_msgs/msg/string.hpp"
class MinimalSubscriberWithTopicStatistics : public rclcpp::Node
{
public:
MinimalSubscriberWithTopicStatistics()
: Node("minimal_subscriber_with_topic_statistics")
{
// manually enable topic statistics via options
auto options = rclcpp::SubscriptionOptions();
options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
// configure the collection window and publish period (default 1s)
options.topic_stats_options.publish_period = std::chrono::seconds(10);
// configure the topic name (default '/statistics')
// options.topic_stats_options.publish_topic = "/topic_statistics"
auto callback = [this](std_msgs::msg::String::SharedPtr msg) {
this->topic_callback(msg);
};
subscription_ = this->create_subscription<std_msgs::msg::String>(
"topic", 10, callback, options);
}
private:
void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
{
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
}
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalSubscriberWithTopicStatistics>());
rclcpp::shutdown();
return 0;
}
//编写CMakelist.txt文件，添加如下代码
add_executable(listener_with_topic_statistics member_function_with_topic_statistics.cpp)
ament_target_dependencies(listener_with_topic_statistics rclcpp std_msgs)
install(TARGETS
talker
listener
listener_with_topic_statistics
DESTINATION lib/${PROJECT_NAME})
//运行发布端
ros2 run cpp_pubsub talker
//使用统计信息的节点运行订阅端
ros2 run cpp_pubsub listener_with_topic_statistics
十二、增加头文件（.h文件）
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_header
cd cpp_header/include/cpp_header
gedit minimal_publisher.hpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher();
private:
void timer_callback();
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
cd my_work_place/src/cpp_header/src
gedit minimal_publisher.cpp
#include "cpp_header/minimal_publisher.hpp"
MinimalPublisher::MinimalPublisher ()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
void MinimalPublisher::timer_callback()
{
auto message = std_msgs::msg::String();
message.data = "Hello, world! " + std::to_string(count_++);
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
publisher_->publish(message);
}
cd my_work_place/src/cpp_header/src
gedit minimal_publisher_node.cpp
#include "cpp_header/minimal_publisher.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
//编辑package.xml,在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
//编辑 CMakelist.txt
//在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
//再增加可执行文件，ros2 run能够调用的名称
include_directories(include)
add_executable(talker src/minimal_publisher_node.cpp src/minimal_publisher.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
//增加可执行文件位置，ros2 run可以找到这个可执行文件
install(TARGETS
talker
DESTINATION lib/${PROJECT_NAME})
//安装相关依赖
cd my_work_place
rosdep install -i --from-path src --rosdistro galactic -y
//编译包
colcon build --symlink-install --packages-select cpp_header
//加载工作空间
. install/setup.bash
//执行
ros2 run cpp_header talker
十二、在C++包里增加python支持
cd my_work_place/src
ros2 pkg craete my_cpp_py_pkg --build-type ament_cmake
//增加cpp节点和头文件
cd my_cpp_py_pkg
gedit include/cpp_talker.hpp
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
gedit src/cpp_node.cpp
#include "my_cpp_py_pkg/cpp_talker.hpp"
using namespace std::chrono_literals;
/* This example creates a subclass of Node and uses std::bind() to register a
* member function as a callback from the timer. */
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
private:
void timer_callback()
{
auto message = std_msgs::msg::String();
message.data = "Hello, world! " + std::to_string(count_++);
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
publisher_->publish(message);
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
//增加python节点和模块导入
mkdir my_cpp_py_pkg
gedit my_cpp_py_pkg/__init__.py
mkdir scripts
gedit my_cpp_py_pkg/module_to_import.py
def listener_write(data) :
my_open = open("/tmp/my_write.txt", 'w')
#打开/tmp路径下的my_write.txt文件,采用写入模式
#若文件不存在,创建，若存在，清空并写入
my_open.write(data)
#在文件中写入一个字符串
my_open.write('\n')
my_open.close()
gedit scripts/py_listener.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from my_cpp_py_pkg.module_to_import import listener_write
class MinimalSubscriber(Node):
def __init__(self):
super().__init__('minimal_subscriber')
self.subscription = self.create_subscription(
String,
'topic',
self.listener_callback,
10)
self.subscription #
def listener_callback(self, msg):
self.get_logger().info('I heard: "%s"' % msg.data)
listener_write(msg.data)
def main(args=None):
rclpy.init(args=args)
minimal_subscriber = MinimalSubscriber()
rclpy.spin(minimal_subscriber)
minimal_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
//增加执行权限
chmod +x scripts/py_listener.py
//配置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>my_cpp_py_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="your@email.com">Name</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
//增加buildtool_depend::ament_cmake_python,来支持python使用,意味着我们将能够使用 cmake 设置我们的 Python 内容。
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>rclcpp</depend>
//增加rclpy，来支持python使用。
<depend>rclpy</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
//配置CMakelist.txt
cmake_minimum_required(VERSION 3.5)
project(my_cpp_py_pkg)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
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(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
# Include Cpp "include" directory
include_directories(include)
# Create Cpp executable
add_executable(cpp_talker src/cpp_talker.cpp)
ament_target_dependencies(cpp_executable rclcpp)
# Install Cpp executables
install(TARGETS
cpp_talker
DESTINATION lib/${PROJECT_NAME}
)
# Install Python modules
ament_python_install_package(${PROJECT_NAME})
# Install Python executables
install(PROGRAMS
scripts/py_listener.py
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
//编译
cd my_work_place
colcon build --symlink-install --packages-select my_cpp_py_pkg
//运行
ros2 run my_cpp_py_pkg cpp_talker
ros2 run my_cpp_py_pkg py_listener.py
重要提示：如果您希望使用 --symlink-install 选项进行编译（这样您就可以修改和重新运行脚本而无需重新编译），您必须使用 chmod +x 使您的脚本可执行，否则会出现"No executable found"错误。
十三、增加命名空间
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_namespace
cd cpp_namespace/include/cpp_namespace
gedit minimal_publisher_namespace.hpp
#ifndef PUBLISHER_NAMESPACE_H_
#define PUBLISHER_NAMESPACE_H_
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
namespace minimal
{
class MinimalPublisher : public rclcpp::Node
{
public:
MinimalPublisher();
private:
void timer_callback();
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
} //namespace minimal
#endif /* PUBLISHER_NAMESPACE_H_ */
cd my_work_space/src/cpp_namespace/src
gedit minimal_publisher_namespace.cpp
#include "cpp_namespace/minimal_publisher_namespace.hpp"
namespace minimal
{
MinimalPublisher::MinimalPublisher ()
: Node("minimal_publisher"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
timer_ = this->create_wall_timer(
500ms, std::bind(&MinimalPublisher::timer_callback, this));
}
void MinimalPublisher::timer_callback()
{
auto message = std_msgs::msg::String();
message.data = "Hello, world! NameSpace" + std::to_string(count_++);
RCLCPP_INFO(this->get_logger(), "NameSpace Publishing : '%s'", message.data.c_str());
publisher_->publish(message);
}
} //namespace minimal
cd my_work_space/src/cpp_namespace/src
gedit minimal_publisher_namespace_node.cpp
#include "cpp_namespace/minimal_publisher_namespace.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<minimal::MinimalPublisher>());
rclcpp::shutdown();
return 0;
}
//编辑package.xml
//在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
//编辑 CMakelist.txt
//在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
//再增加可执行文件，ros2 run能够调用的名称
include_directories(include)
add_executable(talker_namespace src/minimal_publisher_namespace_node.cpp src/minimal_publisher_namespace.cpp)
ament_target_dependencies(talker_namespace rclcpp std_msgs)
//增加可执行文件位置，ros2 run可以找到这个可执行文件
install(TARGETS
talker_namespace
DESTINATION lib/${PROJECT_NAME})
//安装相关依赖
cd my_work_place
rosdep install -i --from-path src --rosdistro galactic -y
//编译包
colcon build --symlink-install --packages-select cpp_namespace
//加载工作空间
. install/setup.bash
//执行
ros2 run cpp_namespace talker_namespace
十四、多线程
cd my_word_place/src
ros2 pkg create --build-type ament_cmake cpp_threads
cd cpp_threads/src
gedit multithreaded_executor.cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
/**
* A small convenience function for converting a thread ID to a string
**/
std::string string_thread_id()
{
auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());
return std::to_string(hashed);
}
/* For this example, we will be creating a publishing node like the one in minimal_publisher.
* We will have a single subscriber node running 2 threads. Each thread loops at different speeds, and
* just repeats what it sees from the publisher to the screen.
*/
class PublisherNode : public rclcpp::Node
{
public:
PublisherNode()
: Node("PublisherNode"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
auto timer_callback =
[this]() -> void {
auto message = std_msgs::msg::String();
message.data = "Hello World! " + std::to_string(this->count_++);
// Extract current thread
auto curr_thread = string_thread_id();
// Prep display message
RCLCPP_INFO(
this->get_logger(), "\n<<THREAD %s>> Publishing '%s'",
curr_thread.c_str(), message.data.c_str());
this->publisher_->publish(message);
};
timer_ = this->create_wall_timer(500ms, timer_callback);
}
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
class DualThreadedNode : public rclcpp::Node
{
public:
DualThreadedNode()
: Node("DualThreadedNode")
{
/* These define the callback groups
* They don't really do much on their own, but they have to exist in order to
* assign callbacks to them. They're also what the executor looks for when trying to run multiple threads
*/
callback_group_subscriber1_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_subscriber2_ = this->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive);
// Each of these callback groups is basically a thread
// Everything assigned to one of them gets bundled into the same thread
auto sub1_opt = rclcpp::SubscriptionOptions();
sub1_opt.callback_group = callback_group_subscriber1_;
auto sub2_opt = rclcpp::SubscriptionOptions();
sub2_opt.callback_group = callback_group_subscriber2_;
subscription1_ = this->create_subscription<std_msgs::msg::String>(
"topic",
rclcpp::QoS(10),
// std::bind is sort of C++'s way of passing a function
// If you're used to function-passing, skip these comments
std::bind(
&DualThreadedNode::subscriber1_cb, // First parameter is a reference to the function
this, // What the function should be bound to
std::placeholders::_1), // At this point we're not positive of all the
// parameters being passed
// So we just put a generic placeholder
// into the binder
// (since we know we need ONE parameter)
sub1_opt); // This is where we set the callback group.
// This subscription will run with callback group subscriber1
subscription2_ = this->create_subscription<std_msgs::msg::String>(
"topic",
rclcpp::QoS(10),
std::bind(
&DualThreadedNode::subscriber2_cb,
this,
std::placeholders::_1),
sub2_opt);
}
private:
/**
* Simple function for generating a timestamp
* Used for somewhat ineffectually demonstrating that the multithreading doesn't cripple performace
*/
std::string timing_string()
{
rclcpp::Time time = this->now();
return std::to_string(time.nanoseconds());
}
/**
* Every time the Publisher publishes something, all subscribers to the topic get poked
* This function gets called when Subscriber1 is poked (due to the std::bind we used when defining it)
*/
void subscriber1_cb(const std_msgs::msg::String::ConstSharedPtr msg)
{
auto message_received_at = timing_string();
// Extract current thread
RCLCPP_INFO(
this->get_logger(), "THREAD %s => Heard '%s' at %s",
string_thread_id().c_str(), msg->data.c_str(), message_received_at.c_str());
}
/**
* This function gets called when Subscriber2 is poked
* Since it's running on a separate thread than Subscriber 1, it will run at (more-or-less) the same time!
*/
void subscriber2_cb(const std_msgs::msg::String::ConstSharedPtr msg)
{
auto message_received_at = timing_string();
// Prep display message
RCLCPP_INFO(
this->get_logger(), "THREAD %s => Heard '%s' at %s",
string_thread_id().c_str(), msg->data.c_str(), message_received_at.c_str());
}
rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_;
rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription1_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription2_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
// You MUST use the MultiThreadedExecutor to use, well, multiple threads
rclcpp::executors::MultiThreadedExecutor executor;
auto pubnode = std::make_shared<PublisherNode>();
auto subnode = std::make_shared<DualThreadedNode>(); // This contains BOTH subscriber callbacks.
// They will still run on different threads
// One Node. Two callbacks. Two Threads
executor.add_node(pubnode);
executor.add_node(subnode);
executor.spin();
rclcpp::shutdown();
return 0;
}
//编译package.xml
//在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
//编译CMakelist.txt
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(multithreaded_executor src/multithreaded_executor.cpp)
ament_target_dependencies(multithreaded_executor rclcpp std_msgs)
install(TARGETS
multithreaded_executor
DESTINATION lib/${PROJECT_NAME}
)
//安装相关依赖
cd my_work_place
rosdep install -i --from-path src --rosdistro galactic -y
//编译包
colcon build --symlink-install --packages-select cpp_threads
//加载工作空间
. install/setup.bash
//执行
ros2 run cpp_threads multithreaded_executor
十四、单线程
cd my_work_place/src
ros2 pkg craete --build-type ament_cmake cpp_threads
cd cpp_threads/src
gedit singlenthreaded_executor.cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
/**
* A small convenience function for converting a thread ID to a string
**/
std::string string_thread_id()
{
auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());
return std::to_string(hashed);
}
/* For this example, we will be creating a publishing node like the one in minimal_publisher.
* We will have a single subscriber node running 2 threads. Each thread loops at different speeds, and
* just repeats what it sees from the publisher to the screen.
*/
class PublisherNode : public rclcpp::Node
{
public:
PublisherNode()
: Node("PublisherNode"), count_(0)
{
publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
auto timer_callback =
[this]() -> void {
auto message = std_msgs::msg::String();
message.data = "Hello World! " + std::to_string(this->count_++);
// Extract current thread
auto curr_thread = string_thread_id();
// Prep display message
RCLCPP_INFO(
this->get_logger(), "\n<<THREAD %s>> Publishing '%s'",
curr_thread.c_str(), message.data.c_str());
this->publisher_->publish(message);
};
timer_ = this->create_wall_timer(500ms, timer_callback);
}
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
};
class SingleThreadedNode : public rclcpp::Node
{
public:
SingleThreadedNode()
: Node("SingleThreadedNode")
{
// Select MutuallyExclusive or Reentrant Callback Group Type
my_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
// my_callback_group = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
// Run timer at 10fps
timer_ = this->create_wall_timer(
100ms, std::bind(&SingleThreadedNode::timer_callback, this), my_callback_group);
// Run subscription with depth 10
subscription_ = this->create_subscription<std_msgs::msg::String>(
"topic", 10, std::bind(&SingleThreadedNode::topic_callback, this, _1));
}
private:
void timer_callback()
{
std::cout << "timer callback" << std::endl;
// Sleep here, to produce a long callback
std::this_thread::sleep_for(5ms);
}
void topic_callback(const std_msgs::msg::String &) const
{
std::cout << "subscription callback" << std::endl;
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::CallbackGroup::SharedPtr my_callback_group;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node1 = std::make_shared<PublisherNode>();
auto node2 = std::make_shared<SingleThreadedNode>();
// Select SingleThreadedExecutor or MultiThreadedExecutor
rclcpp::executors::SingleThreadedExecutor executor;
// rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node1);
executor.add_node(node2);
executor.spin();
rclcpp::shutdown();
return 0;
}
//编译package.xml
//在<buildtool_depend>ament_cmake</buildtool_depend>后增加
<depend>rclcpp</depend>
<depend>std_msgs</depend>
//编译CMakelist.txt
//在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(singlethreaded_executor src/singlethreaded_executor.cpp)
ament_target_dependencies(singlethreaded_executor rclcpp std_msgs)
install(TARGETS
singlethreaded_executor
DESTINATION lib/${PROJECT_NAME}
)
//安装相关依赖
cd my_work_place
rosdep install -i --from-path src --rosdistro galactic -y
//编译包
colcon build --symlink-install --packages-select cpp_threads
//加载工作空间
. install/setup.bash
//执行
ros2 run cpp_threads singlethreaded_executor
十五、话题组件
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_component_topic
cd cpp_component_topic/include/cpp_component_topic
gedit 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_
gedit 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_
gedit 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_
cd my_work_place/src/cpp_component_topic/src
gedit 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)
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)
//安装相关依赖
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 run rclcpp_components component_container
//查看组件容器名称
ros2 component list
//加载话题发布组件到容器/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
话题组件增加launch启动
cd my_work_place/src/cpp_component_topic
mkdir launch
touch launch/composition_demo.launch.py
"""Launch a talker and a listener in a component container."""
import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
def generate_launch_description():
"""Generate launch description with multiple components."""
container = ComposableNodeContainer(
name='my_container',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='cpp_component_topic',
plugin='composition::Talker',
name='talker'),
ComposableNode(
package='cpp_component_topic',
plugin='composition::Listener',
name='listener')
],
output='screen',
)
return launch.LaunchDescription([container])
//编辑CMakeLists.txt
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
//编译
colcon build --symlink-install --packages-select cpp_component_topic
//加载工作空间
. install/setup.bash
//启动launch
ros2 launch cpp_component_topic composition_demo.launch.py
十六、服务组件
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_component_service
cd cpp_component_service/include/cpp_component_service
gedit server_component.hpp
#ifndef COMPOSITION__SERVER_COMPONENT_HPP_
#define COMPOSITION__SERVER_COMPONENT_HPP_
#include "cpp_component_service/visibility_control.h"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
namespace composition
{
class Server : public rclcpp::Node
{
public:
COMPOSITION_PUBLIC
explicit Server(const rclcpp::NodeOptions & options);
private:
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
};
} // namespace composition
#endif // COMPOSITION__SERVER_COMPONENT_HPP_
touch client_component.hpp
#ifndef COMPOSITION__CLIENT_COMPONENT_HPP_
#define COMPOSITION__CLIENT_COMPONENT_HPP_
#include "cpp_component_service/visibility_control.h"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
namespace composition
{
class Client : public rclcpp::Node
{
public:
COMPOSITION_PUBLIC
explicit Client(const rclcpp::NodeOptions & options);
protected:
void on_timer();
private:
rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace composition
#endif // COMPOSITION__CLIENT_COMPONENT_HPP_
gedit 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_
cd my_work_place/src/cpp_component_service/src
gedit server_component.cpp
#include "cpp_component_service/server_component.hpp"
#include <cinttypes>
#include <iostream>
#include <memory>
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
namespace composition
{
Server::Server(const rclcpp::NodeOptions & options)
: Node("Server", options)
{
auto handle_add_two_ints =
[this](
const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response
) -> void
{
RCLCPP_INFO(
this->get_logger(), "Incoming request: [a: %" PRId64 ", b: %" PRId64 "]",
request->a, request->b);
std::flush(std::cout);
response->sum = request->a + request->b;
};
srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);
}
} // 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::Server)
touch client_component.cpp
#include "cpp_component_service/client_component.hpp"
#include <cinttypes>
#include <iostream>
#include <memory>
#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;
namespace composition
{
Client::Client(const rclcpp::NodeOptions & options)
: Node("Client", options)
{
client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
// Note(dhood): The timer period must be greater than the duration of the timer callback.
// Otherwise, the timer can starve a single-threaded executor.
// See https://github.com/ros2/rclcpp/issues/392 for updates.
timer_ = create_wall_timer(2s, std::bind(&Client::on_timer, this));
}
void Client::on_timer()
{
if (!client_->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(
this->get_logger(),
"Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "Service not available after waiting");
return;
}
auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = 2;
request->b = 3;
// In order to wait for a response to arrive, we need to spin().
// However, this function is already being called from within another spin().
// Unfortunately, the current version of spin() is not recursive and so we
// cannot call spin() from within another spin().
// Therefore, we cannot wait for a response to the request we just made here
// within this callback, because it was executed by some other spin function.
// The workaround for this is to give the async_send_request() method another
// argument which is a callback that gets executed once the future is ready.
// We then return from this callback so that the existing spin() function can
// continue and our callback will get called once the response is received.
using ServiceResponseFuture =
rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
auto response_received_callback = [this](ServiceResponseFuture future) {
RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum);
};
auto future_result = client_->async_send_request(request, response_received_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::Client)
//编译package.xml
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>example_interfaces</depend>
//编译CMakelist.txt
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(example_interfaces REQUIRED)
//编译为共享库并注册server_component和client_component组件
nclude_directories(include)
# create ament index resource which references the libraries in the binary dir
set(node_plugins "")
add_library(server_component SHARED
src/server_component.cpp)
target_compile_definitions(server_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(server_component
"example_interfaces"
"rclcpp"
"rclcpp_components")
rclcpp_components_register_nodes(server_component "composition::Server")
set(node_plugins "${node_plugins}composition::Server;$<TARGET_FILE:server_component>\n")
add_library(client_component SHARED
src/client_component.cpp)
target_compile_definitions(client_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(client_component
"example_interfaces"
"rclcpp"
"rclcpp_components")
rclcpp_components_register_nodes(client_component "composition::Client")
set(node_plugins "${node_plugins}composition::Client;$<TARGET_FILE:client_component>\n")
//生成和安装相关库文件和执行文件
install(TARGETS
server_component
client_component
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
//安装相关依赖
cd my_work_place
rosdep install -i --from-path src --rosdistro galactic -y
//编译包
colcon build --symlink-install --packages-select cpp_component_service
//加载工作空间
. install/setup.bash
//查看工作区中已注册和可用的组件
ros2 component types
//启动组件容器
ros2 run rclcpp_components component_container
//查看组件容器名称
ros2 component list
//加载话题发布组件到容器/ComponentManager
ros2 component load /ComponentManager cpp_component_service composition::Server
//加载话题订阅组件到容器/ComponentManager
ros2 component load /ComponentManager cpp_component_service composition::Client
服务组件增加launch启动
cd my_work_place/src/cpp_component_service
mkdir launch
touch launch/composition_demo.launch.py
"""Launch a server and a client in a component container."""
import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
def generate_launch_description():
"""Generate launch description with multiple components."""
container = ComposableNodeContainer(
name='my_container',
namespace='',
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=[
ComposableNode(
package='cpp_component_service',
plugin='composition::Server',
name='talker'),
ComposableNode(
package='cpp_component_service',
plugin='composition::Client',
name='listener')
],
output='screen',
)
return launch.LaunchDescription([container])
//编辑CMakeLists.txt
# Install launch files.
install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}
)
//编译
colcon build --symlink-install --packages-select cpp_component_service
//加载工作空间
. install/setup.bash
//启动launch
ros2 launch cpp_component_service composition_demo.launch.py
十七、进程内(intra_process)话题发布和订阅
- 在同一进程内的不同节点,可以通过共享指针方式实现内容读取,减少消息的拷贝开销；
- 对于图像之类数据量比较大的节点间处理的效率和性能将大大提高；
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_intra_process_topic
cd cpp_intra_process_topic/src
touch two_node_pipeline.cpp
#include <chrono>
#include <cinttypes>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
using namespace std::chrono_literals;
// Node that produces messages.
struct Producer : public rclcpp::Node
{
Producer(const std::string & name, const std::string & output)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true))
{
// Create a publisher on the output topic.
pub_ = this->create_publisher<std_msgs::msg::Int32>(output, 10);
std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
// Create a timer which publishes on the output topic at ~1Hz.
auto callback = [captured_pub]() -> void {
auto pub_ptr = captured_pub.lock();
if (!pub_ptr) {
return;
}
static int32_t count = 0;
std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32());
msg->data = count++;
printf(
"Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get()));
pub_ptr->publish(std::move(msg));
};
timer_ = this->create_wall_timer(1s, callback);
}
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
};
// Node that consumes messages.
struct Consumer : public rclcpp::Node
{
Consumer(const std::string & name, const std::string & input)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true))
{
// Create a subscription on the input topic which prints on receipt of new messages.
sub_ = this->create_subscription<std_msgs::msg::Int32>(
input,
10,
[](std_msgs::msg::Int32::UniquePtr msg) {
printf(
" Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get()));
});
}
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
};
int main(int argc, char * argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
auto producer = std::make_shared<Producer>("producer", "number");
auto consumer = std::make_shared<Consumer>("consumer", "number");
executor.add_node(producer);
executor.add_node(consumer);
executor.spin();
rclcpp::shutdown();
return 0;
}
//编译package.xml
<build_depend>rclcpp</build_depend>
<build_depend>std_msgs</build_depend>
//编译CMakelist.txt
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(include)
add_executable(two_node_pipeline
src/two_node_pipeline.cpp)
target_link_libraries(two_node_pipeline
rclcpp::rclcpp
${std_msgs_TARGETS})
install(TARGETS
two_node_pipeline
DESTINATION lib/${PROJECT_NAME})
//编译包
cd my_work_place
colcon build --symlink-install --packages-select cpp_intra_process_topic
//加载工作空间
. install/setup.bash
//测试
ros2 run cpp_intra_process_topic two_node_pipeline
十七、进程内(intra_process)话题发布和订阅2
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_intra_process_topic2
cd cpp_intra_process_topic2/src
touch cylic_pipeline.cpp
#include <chrono>
#include <cinttypes>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
using namespace std::chrono_literals;
// This node receives an Int32, waits 1 second, then increments and sends it.
struct IncrementerPipe : public rclcpp::Node
{
IncrementerPipe(const std::string & name, const std::string & in, const std::string & out)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true))
{
// Create a publisher on the output topic.
pub = this->create_publisher<std_msgs::msg::Int32>(out, 10);
std::weak_ptr<std::remove_pointer<decltype(pub.get())>::type> captured_pub = pub;
// Create a subscription on the input topic.
sub = this->create_subscription<std_msgs::msg::Int32>(
in,
10,
[captured_pub](std_msgs::msg::Int32::UniquePtr msg) {
auto pub_ptr = captured_pub.lock();
if (!pub_ptr) {
return;
}
printf(
"Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get()));
printf(" sleeping for 1 second...\n");
if (!rclcpp::sleep_for(1s)) {
return; // Return if the sleep failed (e.g. on ctrl-c).
}
printf(" done.\n");
msg->data++; // Increment the message's data.
printf(
"Incrementing and sending with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get()));
pub_ptr->publish(std::move(msg)); // Send the message along to the output topic.
});
}
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub;
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub;
};
int main(int argc, char * argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
// Create a simple loop by connecting the in and out topics of two IncrementerPipe's.
// The expectation is that the address of the message being passed between them never changes.
auto pipe1 = std::make_shared<IncrementerPipe>("pipe1", "topic1", "topic2");
auto pipe2 = std::make_shared<IncrementerPipe>("pipe2", "topic2", "topic1");
rclcpp::sleep_for(1s); // Wait for subscriptions to be established to avoid race conditions.
// Publish the first message (kicking off the cycle).
std::unique_ptr<std_msgs::msg::Int32> msg(new std_msgs::msg::Int32());
msg->data = 42;
printf(
"Published first message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get()));
pipe1->pub->publish(std::move(msg));
executor.add_node(pipe1);
executor.add_node(pipe2);
executor.spin();
rclcpp::shutdown();
return 0;
}
//编译package.xml
<build_depend>rclcpp</build_depend>
<build_depend>std_msgs</build_depend>
//编译CMakelist.txt
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(include)
add_executable(cyclic_pipeline
src/cyclic_pipeline.cpp)
target_link_libraries(cyclic_pipeline
rclcpp::rclcpp
${std_msgs_TARGETS})
install(TARGETS
cyclic_pipeline
DESTINATION lib/${PROJECT_NAME})
//编译包
colcon build --symlink-install --packages-select cpp_intra_process_topic2
//加载工作空间
. install/setup.bash
//测试
ros2 run cpp_intra_process_topic2 cyclic_pipeline
十八、进程内(intra_process)图像处理
实现图像处理，启动相机，订阅图像消息并处理。
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_intra_process_image
cd cpp_intra_process_image/include/cpp_intra_process_image
touch common.hpp
#ifndef IMAGE_PIPELINE__COMMON_HPP_
#define IMAGE_PIPELINE__COMMON_HPP_
#ifdef _WIN32
#include <process.h>
#define GETPID _getpid
#else
#include <unistd.h>
#define GETPID getpid
#endif
#include <chrono>
#include <string>
#include "opencv2/opencv.hpp"
#include "builtin_interfaces/msg/time.hpp"
int
encoding2mat_type(const std::string & encoding)
{
if (encoding == "mono8") {
return CV_8UC1;
} else if (encoding == "bgr8") {
return CV_8UC3;
} else if (encoding == "mono16") {
return CV_16SC1;
} else if (encoding == "rgba8") {
return CV_8UC4;
}
throw std::runtime_error("Unsupported mat type");
}
std::string
mat_type2encoding(int mat_type)
{
switch (mat_type) {
case CV_8UC1:
return "mono8";
case CV_8UC3:
return "bgr8";
case CV_16SC1:
return "mono16";
case CV_8UC4:
return "rgba8";
default:
throw std::runtime_error("Unsupported encoding type");
}
}
void set_now(builtin_interfaces::msg::Time & time)
{
std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch();
if (now <= std::chrono::nanoseconds(0)) {
time.sec = time.nanosec = 0;
} else {
time.sec = static_cast<builtin_interfaces::msg::Time::_sec_type>(now.count() / 1000000000);
time.nanosec = now.count() % 1000000000;
}
}
void
draw_on_image(cv::Mat & image, const std::string & text, int height)
{
cv::Mat c_mat = image;
cv::putText(
c_mat,
text.c_str(),
cv::Point(10, height),
cv::FONT_HERSHEY_SIMPLEX,
0.3,
cv::Scalar(0, 255, 0));
}
#endif // IMAGE_PIPELINE__COMMON_HPP_
touch camera_node.hpp
#ifndef IMAGE_PIPELINE__CAMERA_NODE_HPP_
#define IMAGE_PIPELINE__CAMERA_NODE_HPP_
#include <chrono>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cpp_intra_process_image/common.hpp"
/// Node which captures images from a camera using OpenCV and publishes them.
/// Images are annotated with this process's id as well as the message's ptr.
class CameraNode final : public rclcpp::Node
{
public:
/// \brief Construct a new CameraNode object for capturing video
/// \param output The output topic name to use
/// \param node_name The node name to use
/// \param watermark Whether to add a watermark to the image before publishing
/// \param device Which camera device to use
/// \param width What video width to capture at
/// \param height What video height to capture at
explicit CameraNode(
const std::string & output, const std::string & node_name = "camera_node",
bool watermark = true, int device = 0, int width = 800, int height = 600)
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)),
canceled_(false), watermark_(watermark)
{
// Initialize OpenCV
cap_.open(device);
// TODO(jacobperron): Remove pre-compiler check when we drop support for Xenial
#if CV_MAJOR_VERSION < 3
cap_.set(CV_CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#else
cap_.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#endif
if (!cap_.isOpened()) {
throw std::runtime_error("Could not open video stream!");
}
// Create a publisher on the output topic.
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, rclcpp::SensorDataQoS());
// Create the camera reading loop.
thread_ = std::thread(std::bind(&CameraNode::loop, this));
}
~CameraNode()
{
// Make sure to join the thread on shutdown.
canceled_.store(true);
if (thread_.joinable()) {
thread_.join();
}
}
/// \brief Capture and publish data until the program is closed
void loop()
{
// While running...
while (rclcpp::ok() && !canceled_.load()) {
// Capture a frame from OpenCV.
cap_ >> frame_;
if (frame_.empty()) {
continue;
}
// Create a new unique_ptr to an Image message for storage.
sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
if (watermark_) {
std::stringstream ss;
// Put this process's id and the msg's pointer address on the image.
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(frame_, ss.str(), 20);
}
// Pack the OpenCV image into the ROS image.
set_now(msg->header.stamp);
msg->header.frame_id = "camera_frame";
msg->height = frame_.rows;
msg->width = frame_.cols;
msg->encoding = mat_type2encoding(frame_.type());
msg->is_bigendian = false;
msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
msg->data.assign(frame_.datastart, frame_.dataend);
pub_->publish(std::move(msg)); // Publish.
}
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
std::thread thread_;
std::atomic<bool> canceled_;
/// whether or not to add a watermark to the image showing process id and
/// pointer location
bool watermark_;
cv::VideoCapture cap_;
cv::Mat frame_;
};
#endif // IMAGE_PIPELINE__CAMERA_NODE_HPP_
touch image_view_node.hpp
#ifndef IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
#define IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
#include <sstream>
#include <string>
#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cpp_intra_process_image/common.hpp"
/// Node which receives sensor_msgs/Image messages and renders them using OpenCV.
class ImageViewNode final : public rclcpp::Node
{
public:
/// \brief Construct a new ImageViewNode for visualizing image data
/// \param input The topic name to subscribe to
/// \param node_name The node name to use
/// \param watermark Whether to add a watermark to the image before displaying
explicit ImageViewNode(
const std::string & input, const std::string & node_name = "image_view_node",
bool watermark = true)
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true))
{
// Create a subscription on the input topic.
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input,
rclcpp::SensorDataQoS(),
[node_name, watermark](sensor_msgs::msg::Image::ConstSharedPtr msg) {
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
const_cast<unsigned char *>(msg->data.data()));
if (watermark) {
// Annotate with the pid and pointer address.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(cv_mat, ss.str(), 60);
}
// Show the image.
cv::Mat c_mat = cv_mat;
cv::imshow(node_name.c_str(), c_mat);
char key = cv::waitKey(1); // Look for key presses.
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown();
}
if (key == ' ') { // If <space> then pause until another <space>.
key = '\0';
while (key != ' ') {
key = cv::waitKey(1);
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown();
}
if (!rclcpp::ok()) {
break;
}
}
}
});
}
private:
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
cv::VideoCapture cap_;
cv::Mat frame_;
};
#endif // IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
touch watermark_node.hpp
#ifndef IMAGE_PIPELINE__WATERMARK_NODE_HPP_
#define IMAGE_PIPELINE__WATERMARK_NODE_HPP_
#include <memory>
#include <sstream>
#include <string>
#include <utility>
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cpp_intra_process_image/common.hpp"
/// Node that receives an image, adds some text as a watermark, and publishes it again.
class WatermarkNode final : public rclcpp::Node
{
public:
/// \brief Construct a WatermarkNode that accepts an image, adds a watermark, and republishes it
/// \param input The name of the topic to subscribe to
/// \param output The topic to publish watermarked images to
/// \param text The text to add to the image
/// \param node_name The node name to use
explicit WatermarkNode(
const std::string & input, const std::string & output, const std::string & text,
const std::string & node_name = "watermark_node")
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true))
{
rclcpp::SensorDataQoS qos;
// Create a publisher on the input topic.
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, qos);
std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
// Create a subscription on the output topic.
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input,
qos,
[captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) {
auto pub_ptr = captured_pub.lock();
if (!pub_ptr) {
return;
}
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
// Annotate the image with the pid, pointer address, and the watermark text.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;
draw_on_image(cv_mat, ss.str(), 40);
pub_ptr->publish(std::move(msg)); // Publish it along.
});
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
cv::VideoCapture cap_;
cv::Mat frame_;
};
#endif // IMAGE_PIPELINE__WATERMARK_NODE_HPP_
cd my_work_place/src/cpp_intra_process_image/src
touch camera_node.cpp
#include <memory>
#include "cpp_intra_process_image/camera_node.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<CameraNode> camera_node = nullptr;
try {
camera_node = std::make_shared<CameraNode>("image");
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting..\n", e.what());
return 1;
}
rclcpp::spin(camera_node);
rclcpp::shutdown();
return 0;
}
touch image_view_node.cpp
#include <memory>
#include "cpp_intra_process_image/image_view_node.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");
rclcpp::spin(image_view_node);
rclcpp::shutdown();
return 0;
}
touch watermark_node.cpp
#include <memory>
#include "cpp_intra_process_image/watermark_node.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
rclcpp::spin(watermark_node);
rclcpp::shutdown();
return 0;
}
touch two_image_view.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "cpp_intra_process_image/camera_node.hpp"
#include "cpp_intra_process_image/image_view_node.hpp"
#include "cpp_intra_process_image/watermark_node.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
// Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node
// And the extra image view as a fork: \-> image_view_node2
std::shared_ptr<CameraNode> camera_node = nullptr;
try {
camera_node = std::make_shared<CameraNode>("image");
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting..\n", e.what());
return 1;
}
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");
auto image_view_node2 = std::make_shared<ImageViewNode>("watermarked_image", "image_view_node2");
executor.add_node(camera_node);
executor.add_node(watermark_node);
executor.add_node(image_view_node);
executor.add_node(image_view_node2);
executor.spin();
rclcpp::shutdown();
return 0;
}
touch all_in_one.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "cpp_intra_process_image/camera_node.hpp"
#include "cpp_intra_process_image/image_view_node.hpp"
#include "cpp_intra_process_image/watermark_node.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
// Connect the nodes as a pipeline: camera_node -> watermark_node -> image_view_node
std::shared_ptr<CameraNode> camera_node = nullptr;
try {
camera_node = std::make_shared<CameraNode>("image");
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting ..\n", e.what());
return 1;
}
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!");
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image");
executor.add_node(camera_node);
executor.add_node(watermark_node);
executor.add_node(image_view_node);
executor.spin();
rclcpp::shutdown();
return 0;
}
//编译package.xml
<depend>libopencv-dev</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
//编译CMakelist.txt
find_package(builtin_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rmw REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc videoio)
//生成执行文件
include_directories(include)
# A single program with one of each of the image pipeline demo nodes.
add_executable(all_in_one
src/all_in_one.cpp)
target_link_libraries(all_in_one
rclcpp::rclcpp
${builtin_interfaces_TARGETS}
${sensor_msgs_TARGETS}
opencv_core
opencv_highgui)
# A single program with one of each of the image pipeline demo nodes, but two image views.
add_executable(two_image_view
src/two_image_view.cpp)
target_link_libraries(two_image_view
rclcpp::rclcpp
${builtin_interfaces_TARGETS}
${sensor_msgs_TARGETS}
opencv_core
opencv_highgui)
# A stand alone node which produces images from a camera using OpenCV.
add_executable(camera_node
src/camera_node.cpp)
target_link_libraries(camera_node
rclcpp::rclcpp
${builtin_interfaces_TARGETS}
${sensor_msgs_TARGETS}
opencv_core
opencv_highgui
opencv_imgproc)
# A stand alone node which adds some text to an image using OpenCV before passing it along.
add_executable(watermark_node
src/watermark_node.cpp)
target_link_libraries(watermark_node
rclcpp::rclcpp
${builtin_interfaces_TARGETS}
${sensor_msgs_TARGETS}
opencv_core
opencv_videoio)
# A stand alone node which consumes images and displays them using OpenCV.
add_executable(image_view_node
src/image_view_node.cpp)
target_link_libraries(image_view_node
rclcpp::rclcpp
${builtin_interfaces_TARGETS}
${sensor_msgs_TARGETS}
opencv_core
opencv_highgui)
//安装相关库文件和执行文件
install(TARGETS
all_in_one
two_image_view
camera_node
watermark_node
image_view_node
DESTINATION lib/${PROJECT_NAME})
//编译包
colcon build --symlink-install --packages-select cpp_intra_process_image
//加载工作空间
. install/setup.bash
//测试1
ros2 run cpp_intra_process_image all_in_one
//测试2
ros2 run cpp_intra_process_image two_image_view
十九、生命周期节点
//安装依赖
sudo apt install ros-galactic-lifecycle-msgs
//新建一个包cpp_lifecycle
cd my_work_place/src
ros2 pkg create --build-type ament_cmake cpp_lifecycle
cd cpp_lifecycle/src
touch lifecycle_listener.cpp
#include <memory>
#include <string>
#include "lifecycle_msgs/msg/transition_event.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcutils/logging_macros.h"
#include "std_msgs/msg/string.hpp"
/// LifecycleListener class as a simple listener node
/**
* We subscribe to two topics
* - lifecycle_chatter: The data topic from the talker
* - lc_talker__transition_event: The topic publishing
* notifications about state changes of the node
* lc_talker
*/
class LifecycleListener : public rclcpp::Node
{
public:
explicit LifecycleListener(const std::string & node_name)
: Node(node_name)
{
// Data topic from the lc_talker node
sub_data_ = this->create_subscription<std_msgs::msg::String>(
"lifecycle_chatter", 10,
std::bind(&LifecycleListener::data_callback, this, std::placeholders::_1));
// Notification event topic. All state changes
// are published here as TransitionEvents with
// a start and goal state indicating the transition
sub_notification_ = this->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
"/lc_talker/transition_event",
10,
std::bind(&LifecycleListener::notification_callback, this, std::placeholders::_1));
}
void data_callback(std_msgs::msg::String::ConstSharedPtr msg)
{
RCLCPP_INFO(get_logger(), "data_callback: %s", msg->data.c_str());
}
void notification_callback(lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg)
{
RCLCPP_INFO(
get_logger(), "notify callback: Transition from state %s to %s",
msg->start_state.label.c_str(), msg->goal_state.label.c_str());
}
private:
std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>> sub_data_;
std::shared_ptr<rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>>
sub_notification_;
};
int main(int argc, char ** argv)
{
// force flush of the stdout buffer.
// this ensures a correct sync of all prints
// even when executed simultaneously within the launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
auto lc_listener = std::make_shared<LifecycleListener>("lc_listener");
rclcpp::spin(lc_listener);
rclcpp::shutdown();
return 0;
}
touch lifecycle_talker.cpp
#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include "lifecycle_msgs/msg/transition.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "rcutils/logging_macros.h"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
/// LifecycleTalker inheriting from rclcpp_lifecycle::LifecycleNode
/**
* The lifecycle talker does not like the regular "talker" node
* inherit from node, but rather from lifecyclenode. This brings
* in a set of callbacks which are getting invoked depending on
* the current state of the node.
* Every lifecycle node has a set of services attached to it
* which make it controllable from the outside and invoke state
* changes.
* Available Services as for Beta1:
* - <node_name>__get_state
* - <node_name>__change_state
* - <node_name>__get_available_states
* - <node_name>__get_available_transitions
* Additionally, a publisher for state change notifications is
* created:
* - <node_name>__transition_event
*/
class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
{
public:
/// LifecycleTalker constructor
/**
* The lifecycletalker/lifecyclenode constructor has the same
* arguments a regular node.
*/
explicit LifecycleTalker(const std::string & node_name, bool intra_process_comms = false)
: rclcpp_lifecycle::LifecycleNode(node_name,
rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms))
{}
/// Callback for walltimer in order to publish the message.
/**
* Callback for walltimer. This function gets invoked by the timer
* and executes the publishing.
* For this demo, we ask the node for its current state. If the
* lifecycle publisher is not activate, we still invoke publish, but
* the communication is blocked so that no messages is actually transferred.
*/
void
publish()
{
static size_t count = 0;
auto msg = std::make_unique<std_msgs::msg::String>();
msg->data = "Lifecycle HelloWorld #" + std::to_string(++count);
// Print the current state for demo purposes
if (!pub_->is_activated()) {
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is currently inactive. Messages are not published.");
} else {
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str());
}
// We independently from the current state call publish on the lifecycle
// publisher.
// Only if the publisher is in an active state, the message transfer is
// enabled and the message actually published.
pub_->publish(std::move(msg));
}
/// Transition callback for state configuring
/**
* on_configure callback is being called when the lifecycle node
* enters the "configuring" state.
* Depending on the return value of this function, the state machine
* either invokes a transition to the "inactive" state or stays
* in "unconfigured".
* TRANSITION_CALLBACK_SUCCESS transitions to "inactive"
* TRANSITION_CALLBACK_FAILURE transitions to "unconfigured"
* TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_configure(const rclcpp_lifecycle::State &)
{
// This callback is supposed to be used for initialization and
// configuring purposes.
// We thus initialize and configure our publishers and timers.
// The lifecycle node API does return lifecycle components such as
// lifecycle publishers. These entities obey the lifecycle and
// can comply to the current state of the node.
// As of the beta version, there is only a lifecycle publisher
// available.
pub_ = this->create_publisher<std_msgs::msg::String>("lifecycle_chatter", 10);
timer_ = this->create_wall_timer(
1s, std::bind(&LifecycleTalker::publish, this));
RCLCPP_INFO(get_logger(), "on_configure() is called.");
// We return a success and hence invoke the transition to the next
// step: "inactive".
// If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine
// would stay in the "unconfigured" state.
// In case of TRANSITION_CALLBACK_ERROR or any thrown exception within
// this callback, the state machine transitions to state "errorprocessing".
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
/// Transition callback for state activating
/**
* on_activate callback is being called when the lifecycle node
* enters the "activating" state.
* Depending on the return value of this function, the state machine
* either invokes a transition to the "active" state or stays
* in "inactive".
* TRANSITION_CALLBACK_SUCCESS transitions to "active"
* TRANSITION_CALLBACK_FAILURE transitions to "inactive"
* TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_activate(const rclcpp_lifecycle::State & state)
{
// The parent class method automatically transition on managed entities
// (currently, LifecyclePublisher).
// pub_->on_activate() could also be called manually here.
// Overriding this method is optional, a lot of times the default is enough.
LifecycleNode::on_activate(state);
pub_->on_activate();
RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called.");
// Let's sleep for 2 seconds.
// We emulate we are doing important
// work in the activating phase.
std::this_thread::sleep_for(2s);
// We return a success and hence invoke the transition to the next
// step: "active".
// If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine
// would stay in the "inactive" state.
// In case of TRANSITION_CALLBACK_ERROR or any thrown exception within
// this callback, the state machine transitions to state "errorprocessing".
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
/// Transition callback for state deactivating
/**
* on_deactivate callback is being called when the lifecycle node
* enters the "deactivating" state.
* Depending on the return value of this function, the state machine
* either invokes a transition to the "inactive" state or stays
* in "active".
* TRANSITION_CALLBACK_SUCCESS transitions to "inactive"
* TRANSITION_CALLBACK_FAILURE transitions to "active"
* TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & state)
{
// The parent class method automatically transition on managed entities
// (currently, LifecyclePublisher).
// pub_->on_deactivate() could also be called manually here.
// Overriding this method is optional, a lot of times the default is enough.
LifecycleNode::on_deactivate(state);
RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.");
// We return a success and hence invoke the transition to the next
// step: "inactive".
// If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine
// would stay in the "active" state.
// In case of TRANSITION_CALLBACK_ERROR or any thrown exception within
// this callback, the state machine transitions to state "errorprocessing".
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
/// Transition callback for state cleaningup
/**
* on_cleanup callback is being called when the lifecycle node
* enters the "cleaningup" state.
* Depending on the return value of this function, the state machine
* either invokes a transition to the "unconfigured" state or stays
* in "inactive".
* TRANSITION_CALLBACK_SUCCESS transitions to "unconfigured"
* TRANSITION_CALLBACK_FAILURE transitions to "inactive"
* TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &)
{
// In our cleanup phase, we release the shared pointers to the
// timer and publisher. These entities are no longer available
// and our node is "clean".
timer_.reset();
pub_.reset();
RCUTILS_LOG_INFO_NAMED(get_name(), "on cleanup is called.");
// We return a success and hence invoke the transition to the next
// step: "unconfigured".
// If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine
// would stay in the "inactive" state.
// In case of TRANSITION_CALLBACK_ERROR or any thrown exception within
// this callback, the state machine transitions to state "errorprocessing".
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
/// Transition callback for state shutting down
/**
* on_shutdown callback is being called when the lifecycle node
* enters the "shuttingdown" state.
* Depending on the return value of this function, the state machine
* either invokes a transition to the "finalized" state or stays
* in its current state.
* TRANSITION_CALLBACK_SUCCESS transitions to "finalized"
* TRANSITION_CALLBACK_FAILURE transitions to current state
* TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
*/
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
on_shutdown(const rclcpp_lifecycle::State & state)
{
// In our shutdown phase, we release the shared pointers to the
// timer and publisher. These entities are no longer available
// and our node is "clean".
timer_.reset();
pub_.reset();
RCUTILS_LOG_INFO_NAMED(
get_name(),
"on shutdown is called from state %s.",
state.label().c_str());
// We return a success and hence invoke the transition to the next
// step: "finalized".
// If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine
// would stay in the current state.
// In case of TRANSITION_CALLBACK_ERROR or any thrown exception within
// this callback, the state machine transitions to state "errorprocessing".
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
private:
// We hold an instance of a lifecycle publisher. This lifecycle publisher
// can be activated or deactivated regarding on which state the lifecycle node
// is in.
// By default, a lifecycle publisher is inactive by creation and has to be
// activated to publish messages into the ROS world.
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;
// We hold an instance of a timer which periodically triggers the publish function.
// As for the beta version, this is a regular timer. In a future version, a
// lifecycle timer will be created which obeys the same lifecycle management as the
// lifecycle publisher.
std::shared_ptr<rclcpp::TimerBase> timer_;
};
/**
* A lifecycle node has the same node API
* as a regular node. This means we can spawn a
* node, give it a name and add it to the executor.
*/
int main(int argc, char * argv[])
{
// force flush of the stdout buffer.
// this ensures a correct sync of all prints
// even when executed simultaneously within the launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exe;
std::shared_ptr<LifecycleTalker> lc_node =
std::make_shared<LifecycleTalker>("lc_talker");
exe.add_node(lc_node->get_node_base_interface());
exe.spin();
rclcpp::shutdown();
return 0;
}
touch lifecycle_service_client.cpp
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include "lifecycle_msgs/msg/state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcutils/logging_macros.h"
using namespace std::chrono_literals;
// which node to handle
static constexpr char const * lifecycle_node = "lc_talker";
// Every lifecycle node has various services
// attached to it. By convention, we use the format of
// <node name>/<service name>.
// In this demo, we use get_state and change_state
// and thus the two service topics are:
// lc_talker/get_state
// lc_talker/change_state
static constexpr char const * node_get_state_topic = "lc_talker/get_state";
static constexpr char const * node_change_state_topic = "lc_talker/change_state";
template<typename FutureT, typename WaitTimeT>
std::future_status
wait_for_result(
FutureT & future,
WaitTimeT time_to_wait)
{
auto end = std::chrono::steady_clock::now() + time_to_wait;
std::chrono::milliseconds wait_period(100);
std::future_status status = std::future_status::timeout;
do {
auto now = std::chrono::steady_clock::now();
auto time_left = end - now;
if (time_left <= std::chrono::seconds(0)) {break;}
status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
} while (rclcpp::ok() && status != std::future_status::ready);
return status;
}
class LifecycleServiceClient : public rclcpp::Node
{
public:
explicit LifecycleServiceClient(const std::string & node_name)
: Node(node_name)
{}
void
init()
{
// Every lifecycle node spawns automatically a couple
// of services which allow an external interaction with
// these nodes.
// The two main important ones are GetState and ChangeState.
client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(
node_get_state_topic);
client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(
node_change_state_topic);
}
/// Requests the current state of the node
/**
* In this function, we send a service request
* asking for the current state of the node
* lc_talker.
* If it does return within the given time_out,
* we return the current state of the node, if
* not, we return an unknown state.
* \param time_out Duration in seconds specifying
* how long we wait for a response before returning
* unknown state
*/
unsigned int
get_state(std::chrono::seconds time_out = 3s)
{
auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();
if (!client_get_state_->wait_for_service(time_out)) {
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_get_state_->get_service_name());
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}
// We send the service request for asking the current
// state of the lc_talker node.
auto future_result = client_get_state_->async_send_request(request);
// Let's wait until we have the answer from the node.
// If the request times out, we return an unknown state.
auto future_status = wait_for_result(future_result, time_out);
if (future_status != std::future_status::ready) {
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}
// We have an succesful answer. So let's print the current state.
if (future_result.get()) {
RCLCPP_INFO(
get_logger(), "Node %s has current state %s.",
lifecycle_node, future_result.get()->current_state.label.c_str());
return future_result.get()->current_state.id;
} else {
RCLCPP_ERROR(
get_logger(), "Failed to get current state for node %s", lifecycle_node);
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}
}
/// Invokes a transition
/**
* We send a Service request and indicate
* that we want to invoke transition with
* the id "transition".
* By default, these transitions are
* - configure
* - activate
* - cleanup
* - shutdown
* \param transition id specifying which
* transition to invoke
* \param time_out Duration in seconds specifying
* how long we wait for a response before returning
* unknown state
*/
bool
change_state(std::uint8_t transition, std::chrono::seconds time_out = 3s)
{
auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
request->transition.id = transition;
if (!client_change_state_->wait_for_service(time_out)) {
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_change_state_->get_service_name());
return false;
}
// We send the request with the transition we want to invoke.
auto future_result = client_change_state_->async_send_request(request);
// Let's wait until we have the answer from the node.
// If the request times out, we return an unknown state.
auto future_status = wait_for_result(future_result, time_out);
if (future_status != std::future_status::ready) {
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
return false;
}
// We have an answer, let's print our success.
if (future_result.get()->success) {
RCLCPP_INFO(
get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition));
return true;
} else {
RCLCPP_WARN(
get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
return false;
}
}
private:
std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;
std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_;
};
/**
* This is a little independent
* script which triggers the
* default lifecycle of a node.
* It starts with configure, activate,
* deactivate, activate, deactivate,
* cleanup and finally shutdown
*/
void
callee_script(std::shared_ptr<LifecycleServiceClient> lc_client)
{
rclcpp::WallRate time_between_state_changes(0.1); // 10s
// configure
{
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// activate
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// deactivate
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// activate it again
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// and deactivate it again
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// we cleanup
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)) {
return;
}
if (!lc_client->get_state()) {
return;
}
}
// and finally shutdown
// Note: We have to be precise here on which shutdown transition id to call
// We are currently in the unconfigured state and thus have to call
// TRANSITION_UNCONFIGURED_SHUTDOWN
{
time_between_state_changes.sleep();
if (!rclcpp::ok()) {
return;
}
if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN))
{
return;
}
if (!lc_client->get_state()) {
return;
}
}
}
void
wake_executor(std::shared_future<void> future, rclcpp::executors::SingleThreadedExecutor & exec)
{
future.wait();
// Wake the executor when the script is done
// https://github.com/ros2/rclcpp/issues/1916
exec.cancel();
}
int main(int argc, char ** argv)
{
// force flush of the stdout buffer.
// this ensures a correct sync of all prints
// even when executed simultaneously within the launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
auto lc_client = std::make_shared<LifecycleServiceClient>("lc_client");
lc_client->init();
rclcpp::executors::SingleThreadedExecutor exe;
exe.add_node(lc_client);
std::shared_future<void> script = std::async(
std::launch::async,
std::bind(callee_script, lc_client));
auto wake_exec = std::async(
std::launch::async,
std::bind(wake_executor, script, std::ref(exe)));
exe.spin_until_future_complete(script);
rclcpp::shutdown();
return 0;
}
//编译package.xml
<depend>lifecycle_msgs</depend>
<depend>rclcpp_lifecycle</depend>
<depend>std_msgs</depend>
//编译CMakelist.txt
//在project(cpp_lifecycle)后增加
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
//在find_package(ament_cmake REQUIRED)后增加
find_package(rclcpp_lifecycle REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
//生成执行文件
### demos
add_executable(lifecycle_talker
src/lifecycle_talker.cpp)
ament_target_dependencies(lifecycle_talker
"lifecycle_msgs"
"rclcpp_lifecycle"
"std_msgs"
)
add_executable(lifecycle_listener
src/lifecycle_listener.cpp)
ament_target_dependencies(lifecycle_listener
"lifecycle_msgs"
"rclcpp_lifecycle"
"std_msgs"
)
add_executable(lifecycle_service_client
src/lifecycle_service_client.cpp)
ament_target_dependencies(lifecycle_service_client
"lifecycle_msgs"
"rclcpp_lifecycle"
"std_msgs"
)
//安装相关库文件和执行文件
install(TARGETS
lifecycle_talker
lifecycle_listener
lifecycle_service_client
DESTINATION lib/${PROJECT_NAME})
//编译包
colcon build --symlink-install --packages-select cpp_lifecycle
//加载工作空间
. install/setup.bash
//启动
ros2 run cpp_lifecycle lifecycle_talker
ros2 run cpp_lifecycle lifecycle_listener
ros2 run cpp_lifecycle lifecycle_service_client
//启动了lifecycle_service_client后才开始触发输出，并在切换到activate状态之后才会发布话题信息
稳定状态:
unconfigured #未配置
inactive #未激活
active #已激活
shutdown #已关闭
中间状态:
configuring #正在配置
activating #正在激活
deactivating #正在停用
cleaningup #正在清除
shuttingdown #正在关闭
可使用的转换是：
configure #配置
activate #激活
deactivate #停用
cleanup #清除
shutdown #关闭
#设置为配置
ros2 lifecycle set /lc_talker configure
#设置为激活
ros2 lifecycle set /lc_talker activate
----------------------------OVER------------------------
3884
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
