diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index fd60213..007bf4e 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -23,10 +23,12 @@ set(PACKAGE_DEPENDENCIES nav_msgs pcl_conversions roscpp + rosbag sensor_msgs tf2 tf2_eigen tf2_ros + urdf ) diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 418180f..f05b5d6 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -109,6 +109,18 @@ google_catkin_test(time_conversion_test time_conversion ) +google_binary(cartographer_assets_writer + USES_CARTOGRAPHER + USES_GFLAGS + USES_GLOG + USES_ROS + SRCS + assets_writer_main.cc + DEPENDS + msg_conversion + time_conversion +) + google_binary(cartographer_node USES_CARTOGRAPHER USES_EIGEN @@ -133,3 +145,9 @@ install(TARGETS cartographer_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(TARGETS cartographer_assets_writer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc new file mode 100644 index 0000000..762ffa8 --- /dev/null +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -0,0 +1,187 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_ros/buffer.h" +#include "urdf/model.h" + +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); +DEFINE_string( + urdf, "", + "URDF file that contains static links for your sensor configuration."); +DEFINE_string(bag_filename, "", "Bag to process."); +DEFINE_string( + trajectory_filename, "", + "Proto containing the trajectory written by /finish_trajectory service."); + +namespace cartographer_ros { +namespace { + +namespace carto = ::cartographer; + +std::unique_ptr ReadTrajectory( + const std::string& trajectory_filename) { + std::ifstream stream(trajectory_filename.c_str()); + carto::proto::Trajectory trajectory_proto; + CHECK(trajectory_proto.ParseFromIstream(&stream)); + return carto::transform::TransformInterpolationBuffer::FromTrajectory( + trajectory_proto); +} + +void ReadStaticTransformsFromUrdf(const string& urdf_filename, + ::tf2_ros::Buffer* const buffer) { + urdf::Model model; + CHECK(model.initFile(urdf_filename)); + std::vector> links; + model.getLinks(links); + for (const auto& link : links) { + if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) { + continue; + } + + const urdf::Pose& pose = + link->parent_joint->parent_to_joint_origin_transform; + geometry_msgs::TransformStamped transform; + transform.transform = ToGeometryMsgTransform(carto::transform::Rigid3d( + Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), + Eigen::Quaterniond(pose.rotation.w, pose.rotation.x, pose.rotation.y, + pose.rotation.z))); + transform.child_frame_id = link->name; + transform.header.frame_id = link->getParent()->name; + buffer->setTransform(transform, "urdf", true /* is_static */); + } +} + +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); + + auto transform_interpolation_buffer = ReadTrajectory(trajectory_filename); + + auto* builder = carto::io::PointsProcessorPipelineBuilder::instance(); + + auto file_resolver = + carto::common::make_unique( + std::vector{configuration_directory}); + const string code = + file_resolver->GetFileContentOrDie(configuration_basename); + carto::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver), nullptr); + const string tracking_frame = + lua_parameter_dictionary.GetString("tracking_frame"); + + std::vector> pipeline = + builder->CreatePipeline( + lua_parameter_dictionary.GetDictionary("pipeline").get()); + + 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(); + + 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)); + } + ::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."; + } + bag.close(); + } while (pipeline.back()->Flush() == + carto::io::PointsProcessor::FlushResult::kRestartStream); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + CHECK(!FLAGS_trajectory_filename.empty()) + << "-trajectory_filename is missing."; + + ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename, + FLAGS_configuration_directory, + FLAGS_configuration_basename, FLAGS_urdf); +} diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua new file mode 100644 index 0000000..72197f1 --- /dev/null +++ b/cartographer_ros/configuration_files/assets_writer.lua @@ -0,0 +1,69 @@ +VOXEL_SIZE = 5e-2 +options = { + tracking_frame = "base_link", + pipeline = { + -- { + -- action = "fixed_ratio_sampler", + -- sampling_ratio = 1., + -- }, + -- { + -- action = "min_max_range_filter", + -- min_range = 1., + -- max_range = 150., + -- }, + -- { + -- action = "voxel_filter_and_remove_moving_objects", + -- voxel_size = VOXEL_SIZE, + -- }, + -- { + -- action = "dump_num_points", + -- }, + -- { + -- action = "color_with_panos", + -- run_ids = { + -- "20120119_193543_L19220", + -- }, + -- }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_yz.png", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., math.pi, }, + }, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xy.png", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0., -math.pi / 2., 0., }, + }, + }, + { + action = "write_xray_image", + voxel_size = VOXEL_SIZE, + filename = "xray_xz.png", + transform = { + translation = { 0., 0., 0. }, + rotation = { 0. , 0., -math.pi / 2, }, + }, + }, + -- { + -- action = "write_xyz", + -- filename = "points.xyz", + -- }, + -- { + -- action = "write_octree", + -- directory = "octree", + -- }, + -- { + -- action = "write_ply", + -- filename = "points.ply", + -- }, + } +} + +return options diff --git a/cartographer_ros/package.xml b/cartographer_ros/package.xml index 791dd29..b8cbec7 100644 --- a/cartographer_ros/package.xml +++ b/cartographer_ros/package.xml @@ -56,6 +56,7 @@ tf2 tf2_eigen tf2_ros + urdf yaml-cpp rosunit