cpp话题订阅与发布
发布速度控制小海龟画圆
查看小海龟节点的话题列表
在 chapter_3/topic_ws/src/ 下创建包 demo_cpp_topic
进入 demo_cpp_topic/src 创建 turtlesim_circle.cpp，其内容如下
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
using namespace std::chrono_literals; // 方便使用时间单位如1s, 100ms等
// 继承 rclcpp::Node，表示一个ROS2节点
class TurtleCircleNode : public rclcpp::Node{
private:
// 定义定时器的智能指针，用于定时触发回调函数
rclcpp::TimerBase::SharedPtr timer_;
// 定义发布者的智能指针，用于发布Twist消息
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
public:
// 构造函数，初始化节点并创建定时器和发布者
explicit TurtleCircleNode(const std::string &node_name) : Node(node_name){
// 创建发布者，负责将Twist消息发布到指定话题"/turtle1/cmd_vel"上，队列大小为10
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
// 传入间隔周期，传入回调函数(可以使用成员函数作为回调函数，也可以使用 Lambda)
// 成员函数作为回调函数需要使用bind与当前对象进行绑定
// bind(param1, param2, param3) param1:函数的位置，param2:指向当前对象的指针, param3:参数列表占位符
// 创建一个定时器，周期为1000ms（1秒），定时调用timer_callback()回调函数
// 由于 timer_callback() 是一个不带参数的成员函数，所以参数列表中只有两个参数，如果成员函数有参数，那么bind的参数列表中也要相应的添加对应参数
timer_ = this->create_wall_timer(1000ms, std::bind(&TurtleCircleNode::timer_callback, this));
};
// 这里使用成员函数来作为回调函数
// 定时器回调函数，按设定的周期触发
void timer_callback(){
// 创建Twist消息对象
auto msg = geometry_msgs::msg::Twist();
// 设置线速度：x轴方向速度为1.0，y轴方向为0.0（不移动）
msg.linear.x = 1.0;
msg.linear.y = 0.0;
// 设置角速度：z轴角速度为0.5，表示沿着Z轴旋转
msg.angular.z = 0.5;
// 发布消息，通过发布者将控制命令发布出去
publisher_->publish(msg);
};
};
int main(int argc, char** argv){
rclcpp::init(argc, argv);
auto node = std::make_shared<TurtleCircleNode>("turtle_circle");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}如果上面的回调函数使用 Lambda 表达式实现，则写成这样
2
3
4
5
6
7
8
9
10
11
1000ms,
[this]() {
// Lambda 捕获当前对象，并在 Lambda 内部调用成员函数
auto msg = geometry_msgs::msg::Twist();
msg.linear.x = 1.0;
msg.linear.y = 0.0;
msg.angular.z = 0.5;
publisher_->publish(msg);
}
);在 CMakeLists 中添加相应语句
2
3
4
5
6
7
8
add_executable(turtle_circle src/turtlesim_circle.cpp)
# 包含依赖
ament_target_dependencies(turtle_circle rclcpp geometry_msgs)
# 拷贝节点到 install/lib 下
install(TARGETS turtle_circle
DESTINATION lib/${PROJECT_NAME}
)编译并运行(先运行小海龟节点)
2
3
4
colcon build
source install/setup.bash
ros2 run demo_cpp_topic turtle_circle
运行起来之后，在新的终端可以查看相关信息
订阅pose实现闭环控制
查看当前节点的话题列表
找到小海龟位置信息的话题，/turtle1/pose，对应的消息接口为 turtlesim/msg/Pose
查看消息接口的定义
在 demo_cpp_topic/src 下创建文件 turtle_control.cpp，其内容如下
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
using namespace std::chrono_literals;
class TurtleControlNode : public rclcpp::Node{
private:
// 发布者的智能指针
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber_;
double target_x_{1.0};
double target_y_{1.0};
double k_{1.0}; // 比例系数
double max_speed_{3.0}; // 最大速度
public:
explicit TurtleControlNode(const std::string &node_name) : Node(node_name){
// 创建发布者
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", 10);
subscriber_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, std::bind(&TurtleControlNode::on_pose_received_, this, std::placeholders::_1));
};
void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose){
// 获取小海龟当前位置
double current_x = pose->x;
double current_y = pose->y;
RCLCPP_INFO(get_logger(), "当前位置 x: %f,y: %f ", current_x, current_y);
// 计算当前位置与目标位置的距离差和角度差
double distance = std::sqrt((target_x_ - current_x)*(target_x_ - current_x) + (target_y_ - current_y)*(target_y_ - current_y));
double angle = std::atan2((target_y_ - current_y), (target_x_ - current_x)) - pose->theta;
// 控制策略
auto msg = geometry_msgs::msg::Twist();
if(distance > 0.1){
if(fabs(angle) > 0.2){
msg.angular.z = fabs(angle);
}
else{
msg.linear.x = k_ * distance;
}
}
// 限制线速度最大值
if(msg.linear.x > max_speed_){
msg.linear.x = max_speed_;
}
publisher_->publish(msg);
};
};
int main(int argc, char** argv){
rclcpp::init(argc, argv);
auto node = std::make_shared<TurtleControlNode>("turtle_control");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}在 CMakeLists 文件中添加相应语句
2
3
4
5
6
7
8
add_executable(turtle_control src/turtle_control.cpp)
添加依赖
ament_target_dependencies(turtle_control rclcpp geometry_msgs turtlesim)
拷贝节点到 install/lib
install(TARGETS turtle_circle turtle_control
DESTINATION lib/${PROJECT_NAME}
)编译运行(先运行 turtle 节点)
2
3
4
colcon build
source install/setup.bash
ros2 run demo_cpp_topic turtle_control
