From c3a319cabf523d2d1a20ae66028a7befd263c41a Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Thu, 1 Dec 2016 11:45:25 +0100 Subject: [PATCH] Cleanups and preparation for handling /tf topic. (#206) --- .../cartographer_ros/assets_writer_main.cc | 91 ++++++++++--------- .../cartographer_ros/offline_node_main.cc | 10 +- 2 files changed, 55 insertions(+), 46 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 6a8eda0..e31cd2e 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -60,9 +60,6 @@ namespace carto = ::cartographer; void Run(const string& trajectory_filename, const string& bag_filename, const string& configuration_directory, const string& configuration_basename, const string& urdf_filename) { - ::tf2_ros::Buffer buffer; - ReadStaticTransformsFromUrdf(urdf_filename, &buffer); - std::ifstream stream(trajectory_filename.c_str()); carto::mapping::proto::Trajectory trajectory_proto; CHECK(trajectory_proto.ParseFromIstream(&stream)); @@ -88,53 +85,57 @@ void Run(const string& trajectory_filename, const string& bag_filename, builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); + tf2_ros::Buffer buffer; + // TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to + // support non-rigid sensor configurations. + if (!urdf_filename.empty()) { + ReadStaticTransformsFromUrdf(urdf_filename, &buffer); + } + + auto handle_point_cloud_2_message = [&]( + const sensor_msgs::PointCloud2::ConstPtr& msg) { + const carto::common::Time time = FromRos(msg->header.stamp); + if (!transform_interpolation_buffer->Has(time)) { + return; + } + carto::transform::Rigid3d tracking_to_map = + transform_interpolation_buffer->Lookup(time); + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(*msg, pcl_point_cloud); + + const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d( + buffer.lookupTransform(tracking_frame, msg->header.frame_id, + msg->header.stamp)); + + const carto::transform::Rigid3f sensor_to_map = + (tracking_to_map * sensor_to_tracking).cast(); + + auto batch = carto::common::make_unique(); + batch->time = time; + batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); + batch->frame_id = msg->header.frame_id; + + for (const auto& point : pcl_point_cloud) { + batch->points.push_back(sensor_to_map * + Eigen::Vector3f(point.x, point.y, point.z)); + } + pipeline.back()->Process(std::move(batch)); + }; + do { rosbag::Bag bag; bag.open(bag_filename, rosbag::bagmode::Read); - // TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to - // support non-rigid sensor configurations. - rosbag::View view( - bag, - rosbag::TypeQuery(std::vector{"sensor_msgs/PointCloud2"})); - const ros::Time bag_start_timestamp = view.getBeginTime(); - const ros::Time bag_end_timestamp = view.getEndTime(); + 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& m : view) { - auto point_cloud_msg = m.instantiate(); - if (point_cloud_msg != nullptr) { - const carto::common::Time time = FromRos(point_cloud_msg->header.stamp); - if (!transform_interpolation_buffer->Has(time)) { - continue; - } - carto::transform::Rigid3d tracking_to_map = - transform_interpolation_buffer->Lookup(time); - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*point_cloud_msg, pcl_point_cloud); - - const carto::transform::Rigid3d sensor_to_tracking = - ToRigid3d(buffer.lookupTransform(tracking_frame, - point_cloud_msg->header.frame_id, - point_cloud_msg->header.stamp)); - - const carto::transform::Rigid3f sensor_to_map = - (tracking_to_map * sensor_to_tracking).cast(); - - auto batch = carto::common::make_unique(); - batch->time = time; - batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); - batch->frame_id = point_cloud_msg->header.frame_id; - - for (const auto& point : pcl_point_cloud) { - batch->points.push_back(sensor_to_map * - Eigen::Vector3f(point.x, point.y, point.z)); - } - pipeline.back()->Process(std::move(batch)); + for (const rosbag::MessageInstance& msg : view) { + if (msg.isType()) { + handle_point_cloud_2_message(msg.instantiate()); } - ::ros::Time ros_time = m.getTime(); - LOG_EVERY_N(INFO, 1000) - << "Processed " << (ros_time - bag_start_timestamp).toSec() << " of " - << (bag_end_timestamp - bag_start_timestamp).toSec() - << " bag time seconds."; + LOG_EVERY_N(INFO, 100000) + << "Processed " << (msg.getTime() - begin_time).toSec() << " of " + << duration_in_seconds << " bag time seconds..."; } bag.close(); } while (pipeline.back()->Flush() == diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 330ce14..2e5d34b 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -126,10 +126,14 @@ void Run(std::vector bag_filenames) { const int trajectory_id = node.map_builder_bridge()->AddTrajectory( expected_sensor_ids, options.tracking_frame); + 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 : rosbag::View(bag)) { + for (const rosbag::MessageInstance& msg : view) { if (!::ros::ok()) { return; } @@ -169,6 +173,10 @@ void Run(std::vector bag_filenames) { clock_publisher.publish(clock); ::ros::spinOnce(); + + LOG_EVERY_N(INFO, 100000) + << "Processed " << (msg.getTime() - begin_time).toSec() << " of " + << duration_in_seconds << " bag time seconds..."; } bag.close();