I used rosbag2 to attempt to record sensor data from Puck to be replayed later so I could debug code without having to run the actual robot around the house. Besides the feature failure of rosbag2, especially the inability to view data one time increment at a time, and the inability to control playback, such as pausing, looping and backing up, my current experiment shows that the recording ability of rosbag2 is insufficient.
I need to do further testing, but initially it looks like a LOT of data was simply not recorded.
There is also the whole problem about how to write code that can read the bag data. I know I’m slow at times, but I found the code base to be excessively obscure. Examples are almost non existent. And the examples I found were inconsistent in how to use the API. Still, I managed to cobble up some code that was able to read the bag file and do some analysis of my various proximity sensor reports. Note, I still have not found a way to read a compressed bag file—the runtime error reports are virtually meaningless.
The Teensy monitor in the robot reads eight time-of-flight sensors and four sonar sensors and gathers all of those readings and a few more and publishes it as a single message via Ethernet. Then there is a ROS node I wrote which polls that data and converts it into a set of Range messages, one for reach reported sensor reading. When I read the bag file, I find that instead of the same count of messages for all twelve sensors, there is a big variation in how many messages were captured per sensor.
The exercise is still useful in that I did get some useful analysis regarding how well the sensors were operating. In particular, the time-of-flight sensors are housed in a case I designed and printed and, as a guess, it appears that I don’t have sufficient clearance for the laser beam and return signal. That is, for several of the sensors, if there is a reflecting surface within about two meters, a strong signal is detected and a good distance is reported. But if there isn’t a close reflecting surface, it seems that a weak signal is detected and a bogus distance is reported. When no reflecting surface is nearby, the sensors report a distance of infinity. It appears that I should replace that infinity value with a zero value, which will play nicer with rviz, which doesn’t appear to like infinity values.
Here is the output of running the code I show later below, with lots of lines elided. It begins with showing some metadata from the bag files. Then it shows the timestamp, frame_id and distance data for each database record, along with the running minimum and maximum distance detected for the sensor. At the end, I show the number of messages received for each sensor, the number of messages reporting a value of infinity for the distance, and the detected minimum and maximum distance for the whole recording. The “*” after the infinity count is just flagging that all the reported distances were infinity for that sensor.
Opened database '/home/ros/rosbag2_2021_10_04-09_04_44/rosbag2_2021_10_04-09_04_44_0.db3' for READ_ONLY.
meta name: /t265/gyro/imu_info
meta type: realsense2_camera_msgs/msg/IMUInfo
meta serialization_format: cdr
meta name: /cmd_vel
meta type: geometry_msgs/msg/Twist
meta serialization_format: cdr
meta name: /tofSensor
meta type: sensor_msgs/msg/Range
meta serialization_format: cdr
...
START READ
[0/0] 1633363456.724302976 tof_0 inf 1000000.000000, 0.000000
[1/3] 1633363456.724393408 tof_3 0.200 0.200000, 0.200000
[2/6] 1633363456.724437120 tof_6 inf 1000000.000000, 0.000000
...
[806/3] 1633363456.339151744 sonar_3 0.944 0.935000, 0.957000
END READ, found: 525
TOF message count: 525
tof_0 count: 73, inf_count: 73*, min: 1e+06, max: 0
tof_1 count: 55, inf_count: 1 , min: 0.033, max: 0.112
tof_2 count: 46, inf_count: 2 , min: 0.937, max: 0.995
tof_3 count: 48, inf_count: 30 , min: 0.026, max: 0.583
tof_4 count: 63, inf_count: 56 , min: 0.548, max: 0.999
tof_5 count: 41, inf_count: 41*, min: 1e+06, max: 0
tof_6 count: 54, inf_count: 50 , min: 0.903, max: 0.972
tof_7 count: 145, inf_count: 23 , min: 0.917, max: 0.999
SONAR message count: 282
sonar_0 count: 46, inf_count: 46*, min: 1e+06, max: 0
sonar_1 count: 37, inf_count: 0 , min: 0.731, max: 0.736
sonar_2 count: 54, inf_count: 0 , min: 0.831, max: 0.993
sonar_3 count: 145, inf_count: 0 , min: 0.935, max: 0.957
Here is the code, warts and all. You can probably remove the stuff that is specific for my robot and use it as a basis for writing your own code to read a bag file. At the top, I also include a few web references to materials that helped my create this code.
// Invoke, e.g.: clear;ros2 run bag_utils playback_proximity_data_node
// --ros-args -p bagpath:="/home/ros/rosbag2_2021_10_04-09_04_44"
// http://ftp-osl.osuosl.org/pub/ros/ros_docs_mirror/en/foxy/Tutorials/Ros2bag/Recording-A-Bag-From-Your-Own-Node.html
// https://github.com/Kyungpyo-Kim/ROS2BagFileParsing/blob/master/dev_ws/src/dev_cpp_pkg/src/dev_cpp_node.cpp
// https://answers.ros.org/question/366301/rosbag2-sequential-reading-linker-error/
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/converter_interfaces/serialization_format_converter.hpp>
#include <rosbag2_cpp/readers/sequential_reader.hpp>
#include <rosbag2_cpp/typesupport_helpers.hpp>
using rosbag2_cpp::converter_interfaces::SerializationFormatConverter;
using std::placeholders::_1;
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
#include <iomanip>
#include <limits>
#include <sensor_msgs/msg/range.hpp>
#include "rclcpp/serialization.hpp"
#include "rclcpp/serialized_message.hpp"
class BagReader : public rclcpp::Node {
public:
BagReader() : Node("playback_proximity_data_node") {
this->declare_parameter("bagpath");
rclcpp::Parameter bag_path_parameter = this->get_parameter("bagpath");
std::string bag_path = bag_path_parameter.as_string();
std::cout << "CURRENT_DIRECTORY: " << fs::current_path() << "\n";
rosbag2_cpp::readers::SequentialReader reader;
rosbag2_cpp::StorageOptions storage_options{};
// storage_options.uri = "/home/ros/rosbag2_2021_10_01-10_28_19";
// storage_options.uri = "src/rosbag2_test_data";
storage_options.uri = bag_path; //###"rosbag2_2021_10_04-09_04_44";
storage_options.storage_id = "sqlite3";
rosbag2_cpp::ConverterOptions converter_options{};
converter_options.input_serialization_format = "cdr";
converter_options.output_serialization_format = "cdr";
reader.open(storage_options, converter_options);
auto topics = reader.get_all_topics_and_types();
// about metadata
for (auto t : topics) {
std::cout << "meta name: " << t.name << std::endl;
std::cout << "meta type: " << t.type << std::endl;
std::cout << "meta serialization_format: " << t.serialization_format
<< std::endl;
}
// read and deserialize "serialized data"
int tof_i = 0;
int tof_counted_messages = 0;
int tof_counts[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int tof_inf_counts[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float tof_mins[8] = {1e6, 1e6, 1e6, 1e6, 1e6, 1e6, 1e6, 1e6};
float tof_maxs[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int sonar_i = 0;
int sonar_counted_messages = 0;
int sonar_counts[4] = {0, 0, 0, 0};
int sonar_inf_counts[4] = {0, 0, 0, 0};
float sonar_mins[4] = {1e6, 1e6, 1e6, 1e6};
float sonar_maxs[4] = {0, 0, 0, 0};
std::cout << "START READ" << std::endl;
while (reader.has_next()) {
// serialized data
auto serialized_message = reader.read_next();
if (serialized_message->topic_name == "/tofSensor") {
sensor_msgs::msg::Range msg;
rclcpp::Serialization<sensor_msgs::msg::Range> serialization;
rclcpp::SerializedMessage extracted_serialized_msg(
*serialized_message->serialized_data);
serialization.deserialize_message(&extracted_serialized_msg, &msg);
int32_t sec = (float)msg.header.stamp.sec;
uint32_t ns = (float)msg.header.stamp.nanosec;
int frame_id = tof_frameId(msg.header.frame_id);
float r = msg.range;
tof_counts[frame_id]++;
if (r == -std::numeric_limits<float>::infinity())
std::cout << "NEG_INFINITY" << std::endl;
bool is_infinity = (r == std::numeric_limits<float>::infinity()) ||
(r == -std::numeric_limits<float>::infinity());
if (is_infinity) tof_inf_counts[frame_id]++;
if (r < tof_mins[frame_id] &&
(r != -std::numeric_limits<float>::infinity()))
tof_mins[frame_id] = r;
if ((r > tof_maxs[frame_id]) &&
(r != std::numeric_limits<float>::infinity()))
tof_maxs[frame_id] = r;
char rline[256];
sprintf(rline, "[%d/%d] %10d.%-9d %5s %1.3f %f, %f", tof_i++, frame_id,
sec, ns, msg.header.frame_id.c_str(), r, tof_mins[frame_id],
tof_maxs[frame_id]);
std::cout << rline << std::endl;
tof_counted_messages++;
} else if (serialized_message->topic_name == "/sonarSensor") {
sensor_msgs::msg::Range msg;
rclcpp::Serialization<sensor_msgs::msg::Range> serialization;
rclcpp::SerializedMessage extracted_serialized_msg(
*serialized_message->serialized_data);
serialization.deserialize_message(&extracted_serialized_msg, &msg);
int32_t sec = (float)msg.header.stamp.sec;
uint32_t ns = (float)msg.header.stamp.nanosec;
int frame_id = sonar_frameId(msg.header.frame_id);
float r = msg.range;
sonar_counts[frame_id]++;
if (r == -std::numeric_limits<float>::infinity())
std::cout << "NEG_INFINITY" << std::endl;
bool is_infinity = (r == std::numeric_limits<float>::infinity()) ||
(r == -std::numeric_limits<float>::infinity());
if (is_infinity) sonar_inf_counts[frame_id]++;
if (r < sonar_mins[frame_id] &&
(r != -std::numeric_limits<float>::infinity()))
sonar_mins[frame_id] = r;
if ((r > sonar_maxs[frame_id]) &&
(r != std::numeric_limits<float>::infinity()))
sonar_maxs[frame_id] = r;
char rline[256];
sprintf(rline, "[%d/%d] %10d.%-9d %5s %1.3f %f, %f", tof_i++, frame_id,
sec, ns, msg.header.frame_id.c_str(), r, sonar_mins[frame_id],
sonar_maxs[frame_id]);
std::cout << rline << std::endl;
sonar_counted_messages++;
} else {
// std::cout << "Skipping topic: " << serialized_message->topic_name <<
// std::endl;
}
}
std::cout << "END READ, found: " << tof_counted_messages << std::endl;
std::cout << std::endl;
std::cout << "TOF message count: " << tof_counted_messages << std::endl;
for (int i = 0; i < 8; i++) {
std::cout << "tof_" << i;
std::cout << " count: " << std::setw(3) << tof_counts[i];
std::cout << ", inf_count: " << std::setw(3) << tof_inf_counts[i];
std::cout << (tof_counts[i] == tof_inf_counts[i] ? "*" : " ");
std::cout << ", min: " << std::setw(3) << std::setprecision(3)
<< tof_mins[i];
std::cout << ", max: " << std::setw(3) << std::setprecision(3)
<< tof_maxs[i];
std::cout << std::endl;
}
std::cout << std::endl;
std::cout << "SONAR message count: " << sonar_counted_messages << std::endl;
for (int i = 0; i < 4; i++) {
std::cout << "sonar_" << i;
std::cout << " count: " << std::setw(3) << sonar_counts[i];
std::cout << ", inf_count: " << std::setw(3) << sonar_inf_counts[i];
std::cout << (sonar_counts[i] == sonar_inf_counts[i] ? "*" : " ");
std::cout << ", min: " << std::setw(3) << std::setprecision(3)
<< sonar_mins[i];
std::cout << ", max: " << std::setw(3) << std::setprecision(3)
<< sonar_maxs[i];
std::cout << std::endl;
}
}
private:
std::unique_ptr<rosbag2_cpp::readers::SequentialReader> reader_;
int sonar_frameId(std::string frame_id) {
if (frame_id == "sonar_0") return 0;
if (frame_id == "sonar_1") return 1;
if (frame_id == "sonar_2") return 2;
if (frame_id == "sonar_3")
return 3;
else {
std::cout << "BAD SONAR FRAME ID: " << frame_id << std::endl;
return -0;
}
}
int tof_frameId(std::string frame_id) {
if (frame_id == "tof_0") return 0;
if (frame_id == "tof_1") return 1;
if (frame_id == "tof_2") return 2;
if (frame_id == "tof_3") return 3;
if (frame_id == "tof_4") return 4;
if (frame_id == "tof_5") return 5;
if (frame_id == "tof_6") return 6;
if (frame_id == "tof_7")
return 7;
else {
std::cout << "BAD TOF FRAME ID: " << frame_id << std::endl;
return -0;
}
}
};
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<BagReader>());
rclcpp::shutdown();
return 0;
}