Ros2_学习整理_8_Rosbag2（赵虚左老师）

4.Rosbag2

用于录制和回放话题的工具集
实施步骤：
1、序列化；
2、反序列化；
3、编译执行。
C++实现

1.序列化：

/* 
 需求：录制 turtle_teleop_key 节点发布的速度指令。
 步骤：
   1.包含头文件；
   2.初始化 ROS 客户端；
   3.定义节点类；
     3-1.创建写出对象指针；
     3-2.设置写出的目标文件；
     3-3.写出消息。
   4.调用 spin 函数，并传入对象指针；
   5.释放资源。
*/
#include "rclcpp/rclcpp.hpp"
#include "rosbag2_cpp/writer.hpp"
#include "geometry_msg/msg/twist.hpp"
using std::placeholder _1;
class SimpleBagRecorder :public rclcpp::Node{
	public:
		SimpleBagRecorder():Node("simple_bag_recorder_node"){
		writer_ = this->make_unique<rosbag2_cpp::writer>();	
		writer_->open("my_bag");
		sub_ = create_subscription<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10,std::bind(&SimpleBagRecorder::topic_callback,this,_1))
}

	private:
		void topic_callbak(std::shared_ptr<rclcpp::SerializedMessage> msg) const
		{
			rclcpp::Time time_stamp = this->now();
  			 // 3-3.写出消息。
  			writer_->write(msg, "/turtle1/cmd_vel", "geometry_msgs/msg/Twist", time_stamp);
		}
		std::unique_ptr<rosbag2_cpp::Writer> writer_;
		rclcpp::subscription<geometry_msgs::msg::Twist>::SharedPtr sub_;
}


int main(int argc,char argv[])
{
	rclcpp::init(argc,argv);
	node = std::make_shared<SimpleBagRecorder>();
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}

2.反序列化

>/* 
 需求：读取 bag 文件数据。
 步骤：
   1.包含头文件；
   2.初始化 ROS 客户端；
   3.定义节点类；
     3-1.创建读取对象指针；
     3-2.设置读取的目标文件；
     3-3.读消息；
     3-4.关闭文件。
   4.调用 spin 函数，并传入对象指针；
   5.释放资源。

*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rosbag2_cpp/reader.hpp"
#include "geometry_msgs/msg/twist.hpp"
// 3.定义节点类；
class SimpleBagPlayer: public rclcpp::Node {
public:
   SimpleBagPlayer():Node("simple_bag_player"){
       // 3-1.创建读取对象指针；
       reader_ = std::make_unique<rosbag2_cpp::Reader>();
       // 3-2.设置读取的目标文件；
       reader_->open("my_bag");
       // 3-3.读消息；
       while (reader_->has_next())
       {
           geometry_msgs::msg::Twist twist = reader_->read_next<geometry_msgs::msg::Twist>();
           RCLCPP_INFO(this->get_logger(),"%.2f ---- %.2f",twist.linear.x, twist.angular.z);
       }


       // 3-4.关闭文件。
       reader_->close();
   }
private:
   std::unique_ptr<rosbag2_cpp::Reader> reader_;

};

int main(int argc, char const *argv[])
{
   // 2.初始化 ROS 客户端；
   rclcpp::init(argc,argv);
   // 4.调用 spin 函数，并传入对象指针；
   rclcpp::spin(std::make_shared<SimpleBagPlayer>());
   // 5.释放资源。
   rclcpp::shutdown();
   return 0;
}

Python实现

1.序列化

"""  
 需求：录制 turtle_teleop_key 节点发布的速度指令。
 步骤：
   1.导包；
   2.初始化 ROS 客户端；
   3.定义节点类；
     3-1.创建写出对象；
     3-2.设置写出的目标文件、话题等参数；
     3-3.写出消息。
   4.调用 spin 函数，并传入对象；
   5.释放资源。

"""
# 1.导包；
import rclpy
from rclpy.node import Node
from rclpy.serialization import serialize_message
from geometry_msgs.msg import Twist
import rosbag2_py
# 3.定义节点类；
class SimpleBagRecorder(Node):
   def __init__(self):
       super().__init__('simple_bag_recorder_py')
       # 3-1.创建写出对象；
       self.writer = rosbag2_py.SequentialWriter()
       # 3-2.设置写出的目标文件、话题等参数；
       storage_options = rosbag2_py._storage.StorageOptions(
           uri='my_bag_py',
          storage_id='sqlite3')
       converter_options = rosbag2_py._storage.ConverterOptions('', '')
       self.writer.open(storage_options, converter_options)

       topic_info = rosbag2_py._storage.TopicMetadata(
           name='/turtle1/cmd_vel',
           type='geometry_msgs/msg/Twist',
           serialization_format='cdr')
       self.writer.create_topic(topic_info)

       self.subscription = self.create_subscription(
           Twist,
           '/turtle1/cmd_vel',
           self.topic_callback,
           10)
       self.subscription

   def topic_callback(self, msg):
       # 3-3.写出消息。
       self.writer.write(
           '/turtle1/cmd_vel',
           serialize_message(msg),
           self.get_clock().now().nanoseconds)


def main(args=None):
   # 2.初始化 ROS 客户端；
   rclpy.init(args=args)
   # 4.调用 spin 函数，并传入对象；
   sbr = SimpleBagRecorder()
   rclpy.spin(sbr)
   # 5.释放资源。
   rclpy.shutdown()


if __name__ == '__main__':
   main()

2.反序列化

"""  
   需求：读取 bag 文件数据。
   步骤：
       1.导包；
       2.初始化 ROS 客户端；
       3.定义节点类；
           3-1.创建读取对象；
           3-2.设置读取的目标文件、话题等参数；
           3-3.读消息；
           3-4.关闭文件。
       4.调用 spin 函数，并传入对象；
       5.释放资源。

"""
# 1.导包；
import rclpy
from rclpy.node import Node
import rosbag2_py
from rclpy.logging import get_logger
# 3.定义节点类；
class SimpleBagPlayer(Node):
   def __init__(self):
       super().__init__('simple_bag_player_py')
       # 3-1.创建读取对象；
       self.reader = rosbag2_py.SequentialReader()
       # 3-2.设置读取的目标文件、话题等参数；
       storage_options = rosbag2_py._storage.StorageOptions(
               uri="my_bag_py",
               storage_id='sqlite3')
       converter_options = rosbag2_py._storage.ConverterOptions('', '')
       self.reader.open(storage_options,converter_options)

 def read(self):    
       # 3-3.读消息；
       while self.reader.has_next():
           msg = self.reader.read_next()
           get_logger("rclpy").info("topic = %s, time = %d, value=%s" % (msg[0], msg[2], msg[1]))

def main(args=None):
   # 2.初始化 ROS 客户端；
   rclpy.init(args=args)

   # 4.调用 spin 函数，并传入对象；
   reader = SimpleBagPlayer()
   reader.read()
   rclpy.spin(reader)
   # 5.释放资源。
   rclpy.shutdown()


if __name__ == '__main__':
   main()