From 2dd30a853b804be5751e3d0a4f07eadd1337d1fa Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 22 Mar 2017 13:47:49 +0100 Subject: [PATCH] Follow Cartographer API. (#283) 'laser_fan_3d' is renamed 'range_data_3d'. --- .../cartographer_ros/assets_writer.cc | 19 ++++++++++------- .../cartographer_ros/assets_writer_main.cc | 5 ++--- .../cartographer_ros/bag_reader.cc | 4 ++-- .../cartographer_ros/bag_reader.h | 4 ++-- .../cartographer_ros/node_main.cc | 21 ++++++++++--------- .../cartographer_ros/occupancy_grid.cc | 2 +- .../cartographer_ros/offline_node_main.cc | 7 +++---- .../cartographer_ros/urdf_reader.h | 5 +++-- 8 files changed, 35 insertions(+), 32 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 500cd9f..44aca6e 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -54,30 +54,33 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), {}, stem + "_xray_xy", file_writer_factory, &null_points_processor); carto::io::XRayPointsProcessor yz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor); carto::io::XRayPointsProcessor xz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor); carto::io::PlyWritingPointsProcessor ply_writing_points_processor( file_writer_factory(stem + ".ply"), &xz_xray_points_processor); for (const auto& node : trajectory_nodes) { const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan( - carto::sensor::Decompress(node.constant_data->laser_fan_3d), + carto::sensor::Decompress(node.constant_data->range_data_3d), node.pose.cast()); auto points_batch = carto::common::make_unique(); points_batch->origin = laser_fan.origin; points_batch->points = laser_fan.returns; for (const uint8 reflectivity : - node.constant_data->laser_fan_3d.reflectivities) { + node.constant_data->range_data_3d.reflectivities) { points_batch->colors.push_back( carto::io::Color{{reflectivity, reflectivity, reflectivity}}); } diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 9c3c0d2..79b3900 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -58,9 +58,8 @@ DEFINE_string(bag_filename, "", "Bag to process."); DEFINE_string( trajectory_filename, "", "Proto containing the trajectory written by /finish_trajectory service."); -DEFINE_bool( - use_bag_transforms, true, - "Whether to read and use the transforms from the bag."); +DEFINE_bool(use_bag_transforms, true, + "Whether to read and use the transforms from the bag."); namespace cartographer_ros { namespace { diff --git a/cartographer_ros/cartographer_ros/bag_reader.cc b/cartographer_ros/cartographer_ros/bag_reader.cc index e3c7b7e..bc515e1 100644 --- a/cartographer_ros/cartographer_ros/bag_reader.cc +++ b/cartographer_ros/cartographer_ros/bag_reader.cc @@ -29,8 +29,8 @@ namespace cartographer_ros { constexpr char kTfStaticTopic[] = "/tf_static"; -void ReadTransformsFromBag( - const string& bag_filename, tf2_ros::Buffer* const tf_buffer) { +void ReadTransformsFromBag(const string& bag_filename, + tf2_ros::Buffer* const tf_buffer) { rosbag::Bag bag; bag.open(bag_filename, rosbag::bagmode::Read); rosbag::View view(bag); diff --git a/cartographer_ros/cartographer_ros/bag_reader.h b/cartographer_ros/cartographer_ros/bag_reader.h index 48964ee..d1ce751 100644 --- a/cartographer_ros/cartographer_ros/bag_reader.h +++ b/cartographer_ros/cartographer_ros/bag_reader.h @@ -22,8 +22,8 @@ namespace cartographer_ros { -void ReadTransformsFromBag( - const string& bag_filename, tf2_ros::Buffer* const tf_buffer); +void ReadTransformsFromBag(const string& bag_filename, + tf2_ros::Buffer* const tf_buffer); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 68b638e..cbf524e 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -148,16 +148,17 @@ void Run() { kFinishTrajectoryServiceName, boost::function([&]( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response&) { - const int previous_trajectory_id = trajectory_id; - trajectory_id = node.map_builder_bridge()->AddTrajectory( - expected_sensor_ids, options.tracking_frame); - node.map_builder_bridge()->FinishTrajectory(previous_trajectory_id); - node.map_builder_bridge()->WriteAssets(request.stem); - return true; - })); + ::cartographer_ros_msgs::FinishTrajectory::Response&)>( + [&](::cartographer_ros_msgs::FinishTrajectory::Request& request, + ::cartographer_ros_msgs::FinishTrajectory::Response&) { + const int previous_trajectory_id = trajectory_id; + trajectory_id = node.map_builder_bridge()->AddTrajectory( + expected_sensor_ids, options.tracking_frame); + node.map_builder_bridge()->FinishTrajectory( + previous_trajectory_id); + node.map_builder_bridge()->WriteAssets(request.stem); + return true; + })); ::ros::spin(); diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index 1a82502..2ecc400 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -39,7 +39,7 @@ void BuildOccupancyGrid( carto::mapping_2d::LaserFanInserter laser_fan_inserter( submaps_options.laser_fan_inserter_options()); for (const auto& node : trajectory_nodes) { - CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet. + CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet. laser_fan_inserter.Insert( carto::sensor::TransformLaserFan(node.constant_data->laser_fan_2d, node.pose.cast()), diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index c30b136..165496e 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -47,16 +47,15 @@ DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process."); DEFINE_string( urdf_filename, "", "URDF file that contains static links for your sensor configuration."); -DEFINE_bool( - use_bag_transforms, true, - "Whether to read, use and republish the transforms from the bag."); +DEFINE_bool(use_bag_transforms, true, + "Whether to read, use and republish the transforms from the bag."); namespace cartographer_ros { namespace { constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; -constexpr char kTfTopic [] = "tf"; +constexpr char kTfTopic[] = "tf"; volatile std::sig_atomic_t sigint_triggered = 0; diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/cartographer_ros/urdf_reader.h index 7156137..2278c3a 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.h +++ b/cartographer_ros/cartographer_ros/urdf_reader.h @@ -17,14 +17,15 @@ #ifndef CARTOGRAPHER_ROS_URDF_READER_H_ #define CARTOGRAPHER_ROS_URDF_READER_H_ +#include + #include "cartographer/common/port.h" #include "tf2_ros/buffer.h" -#include namespace cartographer_ros { std::vector ReadStaticTransformsFromUrdf( - const string& urdf_filename, tf2_ros::Buffer* tf_buffer); + const string& urdf_filename, tf2_ros::Buffer* tf_buffer); } // namespace cartographer_ros