天天看點
傳回

代碼實作ROSbag錄制資料和回放資料(C++)          代碼實作ROSbag錄制資料和回放資料(C++)

          代碼實作ROSbag錄制資料和回放資料(C++)

1.代碼實作ROSbag錄制資料和回放資料(C++)

錄制資料： 

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>

int main(int argc, char** argv)
{
  // 初始化ROS節點
  ros::init(argc, argv, "record_bag_node");
  ros::NodeHandle nh;

  // 建立ROSbag對象，打開bag檔案
  rosbag::Bag bag;
  bag.open("example.bag", rosbag::bagmode::Write);

  // 建立一個String消息
  std_msgs::String msg;
  msg.data = "Hello ROSbag!";

  // 将消息寫入bag檔案
  bag.write("my_topic", ros::Time::now(), msg);

  // 關閉bag檔案
  bag.close();

  return 0;
}

在上面的代碼中，我們首先初始化了ROS節點，然後建立了一個ROSbag對象，打開了一個名為"example.bag"的bag檔案，接着建立了一個名為"my_topic"的ROS話題，并将一個String類型的消息寫入了bag檔案中。最後關閉了bag檔案。

回放資料： 

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>

int main(int argc, char** argv)
{
  // 初始化ROS節點
  ros::init(argc, argv, "play_bag_node");
  ros::NodeHandle nh;

  // 建立ROSbag對象，打開bag檔案
  rosbag::Bag bag;
  bag.open("example.bag", rosbag::bagmode::Read);

  // 擷取名為"my_topic"的話題的疊代器
  rosbag::View view(bag, rosbag::TopicQuery("my_topic"));

  // 循環周遊話題中的所有消息
  for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it)
  {
    // 轉換消息類型
    std_msgs::String::ConstPtr msg = it->instantiate<std_msgs::String>();

    if (msg != NULL)
    {
      // 列印消息内容
      ROS_INFO("Message: %s", msg->data.c_str());
    }
  }

  // 關閉bag檔案
  bag.close();

  return 0;
}

在上面的代碼中，我們首先初始化了ROS節點，然後建立了一個ROSbag對象，打開了一個名為"example.bag"的bag檔案，接着擷取了名為"my_topic"的話題的疊代器，并循環周遊話題中的所有消息。在每個消息中，我們将其轉換為String類型，并列印其内容。最後關閉了bag檔案。

請注意，這隻是一個示例，你需要根據你自己的需求修改ROS話題和消息類型。

2.代碼實作ROSbag錄制多個話題的資料和回放資料(C++)

錄制資料：

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv)
{
  // 初始化ROS節點
  ros::init(argc, argv, "record_bag_node");
  ros::NodeHandle nh;

  // 建立ROSbag對象，打開bag檔案
  rosbag::Bag bag;
  bag.open("example.bag", rosbag::bagmode::Write);

  // 建立一個String消息
  std_msgs::String str_msg;
  str_msg.data = "Hello ROSbag!";

  // 建立一個Image消息
  sensor_msgs::Image img_msg;
  img_msg.header.stamp = ros::Time::now();
  img_msg.header.frame_id = "camera";
  img_msg.width = 640;
  img_msg.height = 480;
  img_msg.encoding = "rgb8";
  img_msg.step = img_msg.width * 3;
  img_msg.data.resize(img_msg.step * img_msg.height);

  // 建立一個LaserScan消息
  sensor_msgs::LaserScan scan_msg;
  scan_msg.header.stamp = ros::Time::now();
  scan_msg.header.frame_id = "laser";
  scan_msg.angle_min = -M_PI/2;
  scan_msg.angle_max = M_PI/2;
  scan_msg.angle_increment = M_PI/180;
  scan_msg.time_increment = 0.001;
  scan_msg.scan_time = 0.1;
  scan_msg.range_min = 0.1;
  scan_msg.range_max = 10.0;
  scan_msg.ranges.resize(180);
  scan_msg.intensities.resize(180);

  // 将消息寫入bag檔案
  bag.write("my_string_topic", ros::Time::now(), str_msg);
  bag.write("my_image_topic", ros::Time::now(), img_msg);
  bag.write("my_laser_topic", ros::Time::now(), scan_msg);

  // 關閉bag檔案
  bag.close();

  return 0;
}

在上面的代碼中，我們建立了三個不同的消息類型（String、Image和LaserScan），并将它們分别寫入了三個不同的ROS話題中。最終，将所有話題的消息儲存到一個名為"example.bag"的ROSbag檔案中。

回放資料：

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv)
{
  // 初始化ROS節點
  ros::init(argc, argv, "play_bag_node");
  ros::NodeHandle nh;

  // 建立ROSbag對象，打開bag檔案
  rosbag::Bag bag;
  bag.open("example.bag", rosbag::bagmode::Read);

  // 擷取三個話題的疊代器
  rosbag::View view(bag, rosbag::TopicQuery("my_string_topic") && rosbag::TopicQuery("my_image_topic") && rosbag::TopicQuery("my_laser_topic"));

  // 循環周遊話題中的所有消息
  for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it
) {
// 從bag檔案中讀取消息
const rosbag::MessageInstance& msg = *it;

// 根據消息類型進行強制轉換
if (msg.isType<std_msgs::String>()) {
  std_msgs::String::ConstPtr str_msg = msg.instantiate<std_msgs::String>();
  ROS_INFO_STREAM("String message: " << str_msg->data);
}
else if (msg.isType<sensor_msgs::Image>()) {
  sensor_msgs::Image::ConstPtr img_msg = msg.instantiate<sensor_msgs::Image>();
  ROS_INFO_STREAM("Image message: " << img_msg->header.stamp);
  // 在此處添加處理Image消息的代碼
}
else if (msg.isType<sensor_msgs::LaserScan>()) {
  sensor_msgs::LaserScan::ConstPtr scan_msg = msg.instantiate<sensor_msgs::LaserScan>();
  ROS_INFO_STREAM("LaserScan message: " << scan_msg->header.stamp);
  // 在此處添加處理LaserScan消息的代碼
}

}

// 關閉bag檔案
bag.close();

return 0;
}

在上面的代碼中，我們首先建立了一個ROSbag對象，并打開了名為"example.bag"的ROSbag檔案。然後，我們通過rosbag::TopicQuery函數擷取了三個話題的疊代器，并循環周遊了所有話題的消息。在循環中，我們使用rosbag::MessageInstance對象來讀取每條消息，并根據消息類型進行強制轉換，以便能夠對每個消息進行處理。最後，我們關閉了ROSbag檔案。

請注意，以上代碼中僅僅輸出了每條消息的時間戳和消息内容，您可以根據需要修改代碼來處理具體的消息内容。同時，如果需要處理更多的ROS話題，隻需在代碼中添加相應的話題即可。

3.代碼實作ROSbag錄制多個話題的資料和回放資料用c++封裝成函數 

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/String.h>

// 定義一個函數，用于錄制多個話題的資料到ROSbag檔案
// 參數：
// - topics: 一個包含多個話題名的vector
// - bag_filename: ROSbag檔案名
// - duration: 錄制資料的時間長度（機關：秒），如果duration<=0，則持續錄制直到手動停止
void recordRosbag(const std::vector<std::string>& topics, const std::string& bag_filename, double duration = 0) {
  // 建立ROS節點
  ros::NodeHandle nh;

  // 建立ROSbag對象
  rosbag::Bag bag;
  bag.open(bag_filename, rosbag::bagmode::Write);

  // 建立ROS話題訂閱器
  std::vector<ros::Subscriber> subs;
  for (const auto& topic : topics) {
    if (topic == "/image") {
      subs.push_back(nh.subscribe<sensor_msgs::Image>(topic, 1, [&](const sensor_msgs::Image::ConstPtr& msg) {
        bag.write(topic, ros::Time::now(), *msg);
      }));
    }
    else if (topic == "/scan") {
      subs.push_back(nh.subscribe<sensor_msgs::LaserScan>(topic, 1, [&](const sensor_msgs::LaserScan::ConstPtr& msg) {
        bag.write(topic, ros::Time::now(), *msg);
      }));
    }
    else if (topic == "/string") {
      subs.push_back(nh.subscribe<std_msgs::String>(topic, 1, [&](const std_msgs::String::ConstPtr& msg) {
        bag.write(topic, ros::Time::now(), *msg);
      }));
    }
    else {
      ROS_WARN_STREAM("Unknown topic: " << topic);
    }
  }

  // 持續錄制資料
  if (duration <= 0) {
    ROS_INFO_STREAM("Start recording indefinitely...");
    ros::spin();
  }
  // 持續錄制一段時間後停止
  else {
    ROS_INFO_STREAM("Start recording for " << duration << " seconds...");
    ros::Time start_time = ros::Time::now();
    while ((ros::Time::now() - start_time).toSec() < duration) {
      ros::spinOnce();
    }
  }

  // 停止訂閱器并關閉ROSbag檔案
  for (auto& sub : subs) {
    sub.shutdown();
  }
  bag.close();

  ROS_INFO_STREAM("Recording finished!");
}

定義一個函數，用于回放多個話題的資料

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/ros.h>
#include <string>
#include <vector>
// 定義一個函數，用于回放多個話題的資料
// 參數：
// - bag_filename: ROSbag檔案名
// - topics: 一個包含多個話題名的vector，如果為空，則回放所有話題的資料
void playbackRosbag(const std::string& bag_filename, const std::vector<std::string>& topics = {}) {
  // 建立ROS節點
 // ros::NodeHandle nh("~");
    ros::NodeHandle nh;//sukai

  // 建立ROS話題釋出器
  std::vector<ros::Publisher> pubs;
  for (const auto& topic : topics.empty() ? bag.getTopics() : topics) {
    if (topic == "/image") {
      pubs.push_back(nh.advertise<sensor_msgs::Image>(topic, 1));
    }
    else if (topic == "/scan") {
      pubs.push_back(nh.advertise<sensor_msgs::LaserScan>(topic, 1));
    }
    else if (topic == "/string") {
      pubs.push_back(nh.advertise<std_msgs::String>(topic, 1));
    }
    else {
      ROS_WARN_STREAM("Unknown topic: " << topic);
    }
  }

  // 回放資料
  // rosbag::Bag bag(bag_filename, rosbag::bagmode::Read);
  rosbag::Bag bag;//sukai
  bag.open(bag_filename, rosbag::bagmode::Read); //sukai
  rosbag::View view(bag, rosbag::TopicQuery(topics.empty() ? bag.getTopics() : topics));
  for (const auto& msg : view) {
    ros::Time timestamp = msg.getTime();
    std::string topic = msg.getTopic();
    if (topic == "/image") {
      sensor_msgs::Image::ConstPtr image = msg.instantiate<sensor_msgs::Image>();
      if (image != nullptr) {
        pubs[0].publish(image);
      }
    }
    else if (topic == "/scan") {
      sensor_msgs::LaserScan::ConstPtr scan = msg.instantiate<sensor_msgs::LaserScan>();
      if (scan != nullptr) {
        pubs[1].publish(scan);
      }
    }
    else if (topic == "/string") {
      std_msgs::String::ConstPtr str = msg.instantiate<std_msgs::String>();
      if (str != nullptr) {
        pubs[2].publish(str);
      }
    }
    else {
      ROS_WARN_STREAM("Unknown topic: " << topic);
    }
  }

  // 關閉ROSbag檔案
  bag.close();

  ROS_INFO_STREAM("Playback finished!");
}

 示例使用

// 示例使用
int main(int argc, char** argv) {
ros::init(argc, argv, "rosbag_recorder_player");

// 錄制多個話題的資料到ROSbag檔案
std::vectorstd::string record_topics = {"/image", "/scan", "/string"};
std::string bag_filename = "/tmp/test.bag";
double duration = 10; // 持續10秒錄制
recordRosbag(record_topics, bag_filename, duration);

// 回放ROSbag檔案中的所有話題資料
playbackRosbag(bag_filename);

// 回放ROSbag檔案中的指定話題資料
std::vectorstd::string playback_topics = {"/scan", "/string"};
playbackRosbag(bag_filename, playback_topics);

return 0;
}

參考：

ros代碼中擷取ros節點名稱，rostopic名稱_再遇當年的部落格-CSDN部落格_ros檢視節點名 自動擷取topic名稱與對應的消息類型

其中：void getTopics() 擷取所有topic名稱

  1. cout << "C++列印出目前運作的所有topic name: " <<topicInfo.name << endl;
  2. cout << "C++列印出目前運作的所有topic datatype: " <<topicInfo.datatype << endl;

例子：控制台列印的資料

    C++列印出目前運作的所有topic name: /map

    C++列印出目前運作的所有topic datatype: nav_msgs/OccupancyGrid

datatype: nav_msgs/OccupancyGrid這個資料需要把/替換成::後填入代碼中

c++ ros rosbag
上一篇: Windows下的QT系統開發環境搭建
下一篇: 從一個word檔案中讀取所有的表格和标題（2）

繼續閱讀