diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index ca076c4..c1f38ba 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -183,3 +183,12 @@ install(TARGETS cartographer_assets_writer LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +google_binary(cartographer_offline_node + USES_CARTOGRAPHER + SRCS + offline_node_main.cc + DEPENDS + node + ros_log_sink + urdf_reader +) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 3f5b45d..01925ac 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -47,12 +47,6 @@ using carto::transform::Rigid3d; constexpr int kLatestOnlyPublisherQueueSize = 1; -// Default topic names; expected to be remapped as needed. -constexpr char kOccupancyGridTopic[] = "map"; -constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; -constexpr char kSubmapListTopic[] = "submap_list"; -constexpr char kSubmapQueryServiceName[] = "submap_query"; - Node::Node(const NodeOptions& options, tf2_ros::Buffer* const tf_buffer) : options_(options), map_builder_bridge_(options_, tf_buffer) {} @@ -96,8 +90,6 @@ void Node::Initialize() { &Node::PublishTrajectoryStates, this)); } -void Node::Spin() { ::ros::spin(); } - ::ros::NodeHandle* Node::node_handle() { return &node_handle_; } MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 4788bb0..150b264 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -33,6 +33,18 @@ namespace cartographer_ros { +// Default topic names; expected to be remapped as needed. +constexpr char kLaserScanTopic[] = "scan"; +constexpr char kMultiEchoLaserScanTopic[] = "echoes"; +constexpr char kPointCloud2Topic[] = "points2"; +constexpr char kImuTopic[] = "imu"; +constexpr char kOdometryTopic[] = "odom"; +constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; +constexpr char kOccupancyGridTopic[] = "map"; +constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; +constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kSubmapQueryServiceName[] = "submap_query"; + // Wires up ROS topics to SLAM. class Node { public: @@ -43,7 +55,6 @@ class Node { Node& operator=(const Node&) = delete; void Initialize(); - void Spin(); ::ros::NodeHandle* node_handle(); MapBuilderBridge* map_builder_bridge(); diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 8b72818..5dda6d3 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -38,14 +38,6 @@ namespace { constexpr int kInfiniteSubscriberQueueSize = 0; -// Default topic names; expected to be remapped as needed. -constexpr char kLaserScanTopic[] = "scan"; -constexpr char kMultiEchoLaserScanTopic[] = "echoes"; -constexpr char kPointCloud2Topic[] = "points2"; -constexpr char kImuTopic[] = "imu"; -constexpr char kOdometryTopic[] = "odom"; -constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; - void Run() { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( @@ -60,6 +52,7 @@ void Run() { tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; tf2_ros::TransformListener tf(tf_buffer); Node node(options, &tf_buffer); + node.Initialize(); int trajectory_id = -1; std::unordered_set expected_sensor_ids; @@ -162,8 +155,7 @@ void Run() { return true; })); - node.Initialize(); - node.Spin(); + ::ros::spin(); node.map_builder_bridge()->FinishTrajectory(trajectory_id); } diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc new file mode 100644 index 0000000..65d7f58 --- /dev/null +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -0,0 +1,188 @@ +/* + * 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 "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" +#include "cartographer_ros/node.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/urdf_reader.h" +#include "ros/callback_queue.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "rosgraph_msgs/Clock.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."); +// TODO(damonkohler): Support multi-trajectory. +DEFINE_string(bag_filename, "", "Bag to process."); +DEFINE_string( + urdf_filename, "", + "URDF file that contains static links for your sensor configuration."); + +namespace cartographer_ros { +namespace { + +constexpr int kLatestOnlyPublisherQueueSize = 1; +constexpr char kClockTopic[] = "clock"; + +void Run() { + // TODO(damonkohler): Pull out this common code across binaries. + auto file_resolver = cartographer::common::make_unique< + cartographer::common::ConfigurationFileResolver>( + std::vector{FLAGS_configuration_directory}); + const string code = + file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver)); + + tf2_ros::Buffer tf_buffer; + tf_buffer.setUsingDedicatedThread(true); + ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer); + const auto options = CreateNodeOptions(&lua_parameter_dictionary); + Node node(options, &tf_buffer); + node.Initialize(); + + std::unordered_set expected_sensor_ids; + + // For 2D SLAM, subscribe to exactly one horizontal laser. + if (options.use_laser_scan) { + expected_sensor_ids.insert( + node.node_handle()->resolveName(kLaserScanTopic, false /* remap */)); + } + if (options.use_multi_echo_laser_scan) { + expected_sensor_ids.insert(node.node_handle()->resolveName( + kMultiEchoLaserScanTopic, false /* remap */)); + } + + // For 3D SLAM, subscribe to all point clouds topics. + if (options.num_point_clouds > 0) { + for (int i = 0; i < options.num_point_clouds; ++i) { + string topic = kPointCloud2Topic; + if (options.num_point_clouds > 1) { + topic += "_" + std::to_string(i + 1); + } + expected_sensor_ids.insert( + node.node_handle()->resolveName(topic, false /* remap */)); + } + } + + // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is + // required. + if (options.map_builder_options.use_trajectory_builder_3d() || + (options.map_builder_options.use_trajectory_builder_2d() && + options.map_builder_options.trajectory_builder_2d_options() + .use_imu_data())) { + expected_sensor_ids.insert( + node.node_handle()->resolveName(kImuTopic, false /* remap */)); + } + + // For both 2D and 3D SLAM, odometry is optional. + if (options.use_odometry) { + expected_sensor_ids.insert( + node.node_handle()->resolveName(kOdometryTopic, false /* remap */)); + } + + // TODO(damonkohler): Support multi-trajectory. + const int trajectory_id = node.map_builder_bridge()->AddTrajectory( + expected_sensor_ids, options.tracking_frame); + + ::ros::Publisher clock_publisher = + node.node_handle()->advertise( + kClockTopic, kLatestOnlyPublisherQueueSize); + + rosbag::Bag bag; + bag.open(FLAGS_bag_filename, rosbag::bagmode::Read); + for (const rosbag::MessageInstance& msg : rosbag::View(bag)) { + if (!::ros::ok()) { + break; + } + + const string& topic = node.node_handle()->resolveName(msg.getTopic()); + if (expected_sensor_ids.count( + node.node_handle()->resolveName(msg.getTopic())) == 0) { + continue; + } + if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleLaserScanMessage(topic, + msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage( + topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandlePointCloud2Message( + topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleImuMessage(topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleOdometryMessage(topic, msg.instantiate()); + } + + rosgraph_msgs::Clock clock; + clock.clock = msg.getTime(); + clock_publisher.publish(clock); + + ::ros::spinOnce(); + } + bag.close(); + + node.map_builder_bridge()->FinishTrajectory(trajectory_id); + node.map_builder_bridge()->WriteAssets(FLAGS_bag_filename); +} + +} // 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_urdf_filename.empty()) << "-urdf_filename is missing."; + + ::ros::init(argc, argv, "cartographer_offline_node"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + cartographer_ros::Run(); + + ::ros::shutdown(); +}