案例描述:
启动两个turtlesim节点,一个控制其运动,另一个做镜像运动
需求:订阅乌龟1的位姿信息,解析出线速度和角速度,生成并发布控制乌龟2运动的速度指令。
1.创建工作空间
cd ws01_plumbing
cd src
ros2 pkg create cpp07_exercise --build-type ament_cmake --dependencies rclcpp turtlesim
base_interfaces_demo geometry_msgs rclcpp_action
2.新建launch文件
在src/cpp07_exercise下新建lanunch文件,文件名为launch
在src/cpp07_exercise/launch下新建文件exer01_pub_sub_launch.py
在src/cpp07_exercise/CMakeLists.txt中加入第三行
install(TARGETS exer01_pub_sub
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
在exer01_pub_sub_launch.py进行编写:
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessExit
def generate_launch_description():
# 1. 启动两个turtlesim_node,其中一个要设置命名空间
t1 = Node(package="turtlesim", executable="turtlesim_node")
t2 = Node(package="turtlesim", executable="turtlesim_node", namespace="t2")
# 2. 控制第二个乌龟掉头
rotate = ExecuteProcess(
cmd = ["ros2 action send_goal /t2/turtle1/rotate_absolute turtlesim/action/RotateAbsolute \"{'theta':3.14}\""],
output = "both",
shell = True
)
# 3. 调用自定义的节点,并且该节点调用顺序有要求(要在掉头完毕才能执行)
exer01 = Node(package="cpp07_exercise", executable="exer01_pub_sub")
# 怎么控制节点的执行顺序?通过注册事件完成
# 创建事件注册对象,在对象中声明针对哪个目标节点,在哪个时间触发时,执行哪种操作?
register_rotate_exit_event = RegisterEventHandler(
# 创建一个新对象
event_handler = OnProcessExit( # 触发动作
target_action=rotate, # 目标节点
on_exit=exer01 # 触发事件
)
)
return LaunchDescription([t1, t2, rotate, register_rotate_exit_event])
3.编写cpp文件 exer01_pub_sub_cpp
在传统的ros node文件中,有下面的流程:
流程:
1.包含头文件
2.初始化ROS2客户端
3.自定义节点类:
3-1.创建消息发布方;
3-2.创建定时器
3-3.组织并发布消息
4.调用spin函数,传入自定义类对象指针;
5.释放资源
我们聚焦在 自定义节点类 中
3.自定义节点类:
3-1 创建发布方,向乌龟2发送速度指令
3-2 创建订阅方,订阅乌龟1的位姿,解析速度
3-3 订阅方的回调函数,处理速度,生成并发布控制乌龟2的速度指令
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
// 自定义节点类
class Exer01PubSub:public rclcpp::Node{
public:
Exer01PubSub():Node("exer01_pub_sub_node_cpp"){
RCLCPP_INFO(this->get_logger(),"案例1对象创建!");
# 创建发布方,向乌龟2发布速度指令
pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/t2/turtle1/cmd_vel", 10);
# 创建订阅方,订阅乌龟1的位姿,解析速度
sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, std::bind(&Exer01PubSub::pose_cb, this, std::placeholders::_1));
}
private:
# 订阅方的回调函数,处理速度,生成并发布控制乌龟2的速度指令
void pose_cb(const turtlesim::msg::Pose & pose){
geometry_msgs::msg::Twist twist;
twist.linear.x = pose.linear_velocity;
twist.angular.z = -pose.angular_velocity;
pub_->publish(twist);
}
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_;
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Exer01PubSub>());
rclcpp::shutdown();
return 0;
}