Cleanups and preparation for handling /tf topic. (#206)
parent
42440452b6
commit
c3a319cabf
|
@ -60,9 +60,6 @@ namespace carto = ::cartographer;
|
||||||
void Run(const string& trajectory_filename, const string& bag_filename,
|
void Run(const string& trajectory_filename, const string& bag_filename,
|
||||||
const string& configuration_directory,
|
const string& configuration_directory,
|
||||||
const string& configuration_basename, const string& urdf_filename) {
|
const string& configuration_basename, const string& urdf_filename) {
|
||||||
::tf2_ros::Buffer buffer;
|
|
||||||
ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
|
|
||||||
|
|
||||||
std::ifstream stream(trajectory_filename.c_str());
|
std::ifstream stream(trajectory_filename.c_str());
|
||||||
carto::mapping::proto::Trajectory trajectory_proto;
|
carto::mapping::proto::Trajectory trajectory_proto;
|
||||||
CHECK(trajectory_proto.ParseFromIstream(&stream));
|
CHECK(trajectory_proto.ParseFromIstream(&stream));
|
||||||
|
@ -88,33 +85,27 @@ void Run(const string& trajectory_filename, const string& bag_filename,
|
||||||
builder.CreatePipeline(
|
builder.CreatePipeline(
|
||||||
lua_parameter_dictionary.GetDictionary("pipeline").get());
|
lua_parameter_dictionary.GetDictionary("pipeline").get());
|
||||||
|
|
||||||
do {
|
tf2_ros::Buffer buffer;
|
||||||
rosbag::Bag bag;
|
|
||||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
|
||||||
// TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to
|
// TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to
|
||||||
// support non-rigid sensor configurations.
|
// support non-rigid sensor configurations.
|
||||||
rosbag::View view(
|
if (!urdf_filename.empty()) {
|
||||||
bag,
|
ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
|
||||||
rosbag::TypeQuery(std::vector<std::string>{"sensor_msgs/PointCloud2"}));
|
}
|
||||||
const ros::Time bag_start_timestamp = view.getBeginTime();
|
|
||||||
const ros::Time bag_end_timestamp = view.getEndTime();
|
|
||||||
|
|
||||||
for (const rosbag::MessageInstance& m : view) {
|
auto handle_point_cloud_2_message = [&](
|
||||||
auto point_cloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
|
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||||
if (point_cloud_msg != nullptr) {
|
const carto::common::Time time = FromRos(msg->header.stamp);
|
||||||
const carto::common::Time time = FromRos(point_cloud_msg->header.stamp);
|
|
||||||
if (!transform_interpolation_buffer->Has(time)) {
|
if (!transform_interpolation_buffer->Has(time)) {
|
||||||
continue;
|
return;
|
||||||
}
|
}
|
||||||
carto::transform::Rigid3d tracking_to_map =
|
carto::transform::Rigid3d tracking_to_map =
|
||||||
transform_interpolation_buffer->Lookup(time);
|
transform_interpolation_buffer->Lookup(time);
|
||||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||||
pcl::fromROSMsg(*point_cloud_msg, pcl_point_cloud);
|
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
||||||
|
|
||||||
const carto::transform::Rigid3d sensor_to_tracking =
|
const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d(
|
||||||
ToRigid3d(buffer.lookupTransform(tracking_frame,
|
buffer.lookupTransform(tracking_frame, msg->header.frame_id,
|
||||||
point_cloud_msg->header.frame_id,
|
msg->header.stamp));
|
||||||
point_cloud_msg->header.stamp));
|
|
||||||
|
|
||||||
const carto::transform::Rigid3f sensor_to_map =
|
const carto::transform::Rigid3f sensor_to_map =
|
||||||
(tracking_to_map * sensor_to_tracking).cast<float>();
|
(tracking_to_map * sensor_to_tracking).cast<float>();
|
||||||
|
@ -122,19 +113,29 @@ void Run(const string& trajectory_filename, const string& bag_filename,
|
||||||
auto batch = carto::common::make_unique<carto::io::PointsBatch>();
|
auto batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||||
batch->time = time;
|
batch->time = time;
|
||||||
batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
|
batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
|
||||||
batch->frame_id = point_cloud_msg->header.frame_id;
|
batch->frame_id = msg->header.frame_id;
|
||||||
|
|
||||||
for (const auto& point : pcl_point_cloud) {
|
for (const auto& point : pcl_point_cloud) {
|
||||||
batch->points.push_back(sensor_to_map *
|
batch->points.push_back(sensor_to_map *
|
||||||
Eigen::Vector3f(point.x, point.y, point.z));
|
Eigen::Vector3f(point.x, point.y, point.z));
|
||||||
}
|
}
|
||||||
pipeline.back()->Process(std::move(batch));
|
pipeline.back()->Process(std::move(batch));
|
||||||
|
};
|
||||||
|
|
||||||
|
do {
|
||||||
|
rosbag::Bag bag;
|
||||||
|
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||||
|
rosbag::View view(bag);
|
||||||
|
const ::ros::Time begin_time = view.getBeginTime();
|
||||||
|
const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
|
||||||
|
|
||||||
|
for (const rosbag::MessageInstance& msg : view) {
|
||||||
|
if (msg.isType<sensor_msgs::PointCloud2>()) {
|
||||||
|
handle_point_cloud_2_message(msg.instantiate<sensor_msgs::PointCloud2>());
|
||||||
}
|
}
|
||||||
::ros::Time ros_time = m.getTime();
|
LOG_EVERY_N(INFO, 100000)
|
||||||
LOG_EVERY_N(INFO, 1000)
|
<< "Processed " << (msg.getTime() - begin_time).toSec() << " of "
|
||||||
<< "Processed " << (ros_time - bag_start_timestamp).toSec() << " of "
|
<< duration_in_seconds << " bag time seconds...";
|
||||||
<< (bag_end_timestamp - bag_start_timestamp).toSec()
|
|
||||||
<< " bag time seconds.";
|
|
||||||
}
|
}
|
||||||
bag.close();
|
bag.close();
|
||||||
} while (pipeline.back()->Flush() ==
|
} while (pipeline.back()->Flush() ==
|
||||||
|
|
|
@ -126,10 +126,14 @@ void Run(std::vector<string> bag_filenames) {
|
||||||
|
|
||||||
const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
|
const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
|
||||||
expected_sensor_ids, options.tracking_frame);
|
expected_sensor_ids, options.tracking_frame);
|
||||||
|
|
||||||
rosbag::Bag bag;
|
rosbag::Bag bag;
|
||||||
bag.open(bag_filename, rosbag::bagmode::Read);
|
bag.open(bag_filename, rosbag::bagmode::Read);
|
||||||
|
rosbag::View view(bag);
|
||||||
|
const ::ros::Time begin_time = view.getBeginTime();
|
||||||
|
const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
|
||||||
|
|
||||||
for (const rosbag::MessageInstance& msg : rosbag::View(bag)) {
|
for (const rosbag::MessageInstance& msg : view) {
|
||||||
if (!::ros::ok()) {
|
if (!::ros::ok()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -169,6 +173,10 @@ void Run(std::vector<string> bag_filenames) {
|
||||||
clock_publisher.publish(clock);
|
clock_publisher.publish(clock);
|
||||||
|
|
||||||
::ros::spinOnce();
|
::ros::spinOnce();
|
||||||
|
|
||||||
|
LOG_EVERY_N(INFO, 100000)
|
||||||
|
<< "Processed " << (msg.getTime() - begin_time).toSec() << " of "
|
||||||
|
<< duration_in_seconds << " bag time seconds...";
|
||||||
}
|
}
|
||||||
|
|
||||||
bag.close();
|
bag.close();
|
||||||
|
|
Loading…
Reference in New Issue