diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 4733384..f5096c8 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -46,6 +46,23 @@ google_library(msg_conversion time_conversion ) +google_library(node + USES_CARTOGRAPHER + USES_EIGEN + USES_GLOG + SRCS + node.cc + HDRS + node.h + DEPENDS + map_builder_bridge + msg_conversion + node_options + sensor_bridge + tf_bridge + time_conversion +) + google_library(node_options USES_CARTOGRAPHER USES_GLOG @@ -137,19 +154,11 @@ google_binary(cartographer_assets_writer google_binary(cartographer_node USES_CARTOGRAPHER - USES_EIGEN - USES_GFLAGS - USES_GLOG SRCS node_main.cc DEPENDS - map_builder_bridge - msg_conversion - node_options + node ros_log_sink - sensor_bridge - tf_bridge - time_conversion ) install(TARGETS cartographer_node diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 7763322..712ae09 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -14,6 +14,9 @@ * limitations under the License. */ +#ifndef CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ +#define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ + #include #include @@ -69,3 +72,5 @@ class MapBuilderBridge { }; } // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc new file mode 100644 index 0000000..df696f1 --- /dev/null +++ b/cartographer_ros/cartographer_ros/node.cc @@ -0,0 +1,291 @@ +/* + * 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 "cartographer_ros/node.h" + +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/sensor_bridge.h" +#include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros/time_conversion.h" +#include "glog/logging.h" +#include "nav_msgs/Odometry.h" +#include "ros/serialization.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_eigen/tf2_eigen.h" + +namespace cartographer_ros { + +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; + +constexpr int kInfiniteSubscriberQueueSize = 0; +constexpr int kLatestOnlyPublisherQueueSize = 1; +constexpr double kTfBufferCacheTimeInSeconds = 1e6; + +// Unique 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 kOccupancyGridTopic[] = "map"; +constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; +constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kSubmapQueryServiceName[] = "submap_query"; +constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; + +Node::Node(const NodeOptions& options) + : options_(options), + tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)), + tf_(tf_buffer_) {} + +Node::~Node() { + { + carto::common::MutexLocker lock(&mutex_); + terminating_ = true; + } + if (occupancy_grid_thread_.joinable()) { + occupancy_grid_thread_.join(); + } +} + +void Node::Initialize() { + carto::common::MutexLocker lock(&mutex_); + std::unordered_set expected_sensor_ids; + + // For 2D SLAM, subscribe to exactly one horizontal laser. + if (options_.use_laser_scan) { + horizontal_laser_scan_subscriber_ = node_handle_.subscribe( + kLaserScanTopic, kInfiniteSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::LaserScan::ConstPtr& msg) { + map_builder_bridge_->sensor_bridge()->HandleLaserScanMessage( + kLaserScanTopic, msg); + })); + expected_sensor_ids.insert(kLaserScanTopic); + } + if (options_.use_multi_echo_laser_scan) { + horizontal_laser_scan_subscriber_ = node_handle_.subscribe( + kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + map_builder_bridge_->sensor_bridge() + ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, + msg); + })); + expected_sensor_ids.insert(kMultiEchoLaserScanTopic); + } + + // 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); + } + point_cloud_subscribers_.push_back(node_handle_.subscribe( + topic, kInfiniteSubscriberQueueSize, + boost::function( + [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { + map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message( + topic, msg); + }))); + expected_sensor_ids.insert(topic); + } + } + + // 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())) { + imu_subscriber_ = node_handle_.subscribe( + kImuTopic, kInfiniteSubscriberQueueSize, + boost::function( + [this](const sensor_msgs::Imu::ConstPtr& msg) { + map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic, + msg); + })); + expected_sensor_ids.insert(kImuTopic); + } + + if (options_.use_odometry) { + odometry_subscriber_ = node_handle_.subscribe( + kOdometryTopic, kInfiniteSubscriberQueueSize, + boost::function( + [this](const nav_msgs::Odometry::ConstPtr& msg) { + map_builder_bridge_->sensor_bridge()->HandleOdometryMessage( + kOdometryTopic, msg); + })); + expected_sensor_ids.insert(kOdometryTopic); + } + + map_builder_bridge_ = carto::common::make_unique( + options_, expected_sensor_ids, &tf_buffer_); + + submap_list_publisher_ = + node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( + kSubmapListTopic, kLatestOnlyPublisherQueueSize); + submap_query_server_ = node_handle_.advertiseService( + kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); + + if (options_.map_builder_options.use_trajectory_builder_2d()) { + occupancy_grid_publisher_ = + node_handle_.advertise<::nav_msgs::OccupancyGrid>( + kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, + true /* latched */); + occupancy_grid_thread_ = + std::thread(&Node::SpinOccupancyGridThreadForever, this); + } + + scan_matched_point_cloud_publisher_ = + node_handle_.advertise( + kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); + + finish_trajectory_server_ = node_handle_.advertiseService( + kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this); + + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(options_.submap_publish_period_sec), + &Node::PublishSubmapList, this)); + wall_timers_.push_back(node_handle_.createWallTimer( + ::ros::WallDuration(options_.pose_publish_period_sec), + &Node::PublishPoseAndScanMatchedPointCloud, this)); +} + +bool Node::HandleSubmapQuery( + ::cartographer_ros_msgs::SubmapQuery::Request& request, + ::cartographer_ros_msgs::SubmapQuery::Response& response) { + carto::common::MutexLocker lock(&mutex_); + return map_builder_bridge_->HandleSubmapQuery(request, response); +} + +bool Node::HandleFinishTrajectory( + ::cartographer_ros_msgs::FinishTrajectory::Request& request, + ::cartographer_ros_msgs::FinishTrajectory::Response& response) { + carto::common::MutexLocker lock(&mutex_); + return map_builder_bridge_->HandleFinishTrajectory(request, response); +} + +void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { + carto::common::MutexLocker lock(&mutex_); + submap_list_publisher_.publish(map_builder_bridge_->GetSubmapList()); +} + +void Node::PublishPoseAndScanMatchedPointCloud( + const ::ros::WallTimerEvent& timer_event) { + carto::common::MutexLocker lock(&mutex_); + const carto::mapping::TrajectoryBuilder* trajectory_builder = + map_builder_bridge_->map_builder()->GetTrajectoryBuilder( + map_builder_bridge_->trajectory_id()); + const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = + trajectory_builder->pose_estimate(); + if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { + return; + } + + const Rigid3d tracking_to_local = last_pose_estimate.pose; + const Rigid3d local_to_map = + map_builder_bridge_->map_builder() + ->sparse_pose_graph() + ->GetLocalToGlobalTransform(*trajectory_builder->submaps()); + const Rigid3d tracking_to_map = local_to_map * tracking_to_local; + + geometry_msgs::TransformStamped stamped_transform; + stamped_transform.header.stamp = ToRos(last_pose_estimate.time); + + // We only publish a point cloud if it has changed. It is not needed at high + // frequency, and republishing it would be computationally wasteful. + if (last_pose_estimate.time != last_scan_matched_point_cloud_time_) { + scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( + carto::common::ToUniversal(last_pose_estimate.time), + options_.tracking_frame, + carto::sensor::TransformPointCloud( + last_pose_estimate.point_cloud, + tracking_to_local.inverse().cast()))); + last_scan_matched_point_cloud_time_ = last_pose_estimate.time; + } else { + // If we do not publish a new point cloud, we still allow time of the + // published poses to advance. + stamped_transform.header.stamp = ros::Time::now(); + } + + const auto published_to_tracking = + map_builder_bridge_->tf_bridge()->LookupToTracking( + last_pose_estimate.time, options_.published_frame); + if (published_to_tracking != nullptr) { + if (options_.provide_odom_frame) { + std::vector stamped_transforms; + + stamped_transform.header.frame_id = options_.map_frame; + stamped_transform.child_frame_id = options_.odom_frame; + stamped_transform.transform = ToGeometryMsgTransform(local_to_map); + stamped_transforms.push_back(stamped_transform); + + stamped_transform.header.frame_id = options_.odom_frame; + stamped_transform.child_frame_id = options_.published_frame; + stamped_transform.transform = + ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking)); + stamped_transforms.push_back(stamped_transform); + + tf_broadcaster_.sendTransform(stamped_transforms); + } else { + stamped_transform.header.frame_id = options_.map_frame; + stamped_transform.child_frame_id = options_.published_frame; + stamped_transform.transform = + ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking)); + tf_broadcaster_.sendTransform(stamped_transform); + } + } +} + +void Node::SpinOccupancyGridThreadForever() { + for (;;) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + { + carto::common::MutexLocker lock(&mutex_); + if (terminating_) { + return; + } + } + if (occupancy_grid_publisher_.getNumSubscribers() == 0) { + continue; + } + const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid(); + if (occupancy_grid != nullptr) { + occupancy_grid_publisher_.publish(*occupancy_grid); + } + } +} + +void Node::SpinForever() { ::ros::spin(); } + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h new file mode 100644 index 0000000..2be8a15 --- /dev/null +++ b/cartographer_ros/cartographer_ros/node.h @@ -0,0 +1,94 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_ROS_NODE_H_ +#define CARTOGRAPHER_ROS_NODE_H_ + +#include +#include + +#include "cartographer/common/mutex.h" +#include "cartographer_ros/map_builder_bridge.h" +#include "cartographer_ros/node_options.h" +#include "cartographer_ros_msgs/FinishTrajectory.h" +#include "cartographer_ros_msgs/SubmapEntry.h" +#include "cartographer_ros_msgs/SubmapList.h" +#include "cartographer_ros_msgs/SubmapQuery.h" +#include "cartographer_ros_msgs/TrajectorySubmapList.h" +#include "ros/ros.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +namespace cartographer_ros { + +// Wires up ROS topics to SLAM. +class Node { + public: + Node(const NodeOptions& options); + ~Node(); + + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + + void SpinForever(); + void Initialize(); + + private: + bool HandleSubmapQuery( + cartographer_ros_msgs::SubmapQuery::Request& request, + cartographer_ros_msgs::SubmapQuery::Response& response); + bool HandleFinishTrajectory( + cartographer_ros_msgs::FinishTrajectory::Request& request, + cartographer_ros_msgs::FinishTrajectory::Response& response); + + void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); + void PublishPoseAndScanMatchedPointCloud( + const ::ros::WallTimerEvent& timer_event); + void SpinOccupancyGridThreadForever(); + + const NodeOptions options_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + + cartographer::common::Mutex mutex_; + std::unique_ptr map_builder_bridge_ GUARDED_BY(mutex_); + + ::ros::NodeHandle node_handle_; + ::ros::Subscriber imu_subscriber_; + ::ros::Subscriber horizontal_laser_scan_subscriber_; + std::vector<::ros::Subscriber> point_cloud_subscribers_; + ::ros::Subscriber odometry_subscriber_; + ::ros::Publisher submap_list_publisher_; + ::ros::ServiceServer submap_query_server_; + ::ros::Publisher scan_matched_point_cloud_publisher_; + cartographer::common::Time last_scan_matched_point_cloud_time_ = + cartographer::common::Time::min(); + ::ros::ServiceServer finish_trajectory_server_; + + ::ros::Publisher occupancy_grid_publisher_; + std::thread occupancy_grid_thread_; + bool terminating_ = false GUARDED_BY(mutex_); + + // We have to keep the timer handles of ::ros::WallTimers around, otherwise + // they do not fire. + std::vector<::ros::WallTimer> wall_timers_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_NODE_H_ diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 8a1d969..619a0e1 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -14,46 +14,15 @@ * limitations under the License. */ -#include -#include -#include #include #include -#include "Eigen/Core" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" -#include "cartographer/common/mutex.h" #include "cartographer/common/port.h" -#include "cartographer/common/time.h" -#include "cartographer/mapping/map_builder.h" -#include "cartographer/mapping/proto/submap_visualization.pb.h" -#include "cartographer/mapping/sparse_pose_graph.h" -#include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer/transform/transform.h" -#include "cartographer_ros/map_builder_bridge.h" -#include "cartographer_ros/msg_conversion.h" -#include "cartographer_ros/node_options.h" +#include "cartographer_ros/node.h" #include "cartographer_ros/ros_log_sink.h" -#include "cartographer_ros/sensor_bridge.h" -#include "cartographer_ros/tf_bridge.h" -#include "cartographer_ros/time_conversion.h" -#include "cartographer_ros_msgs/FinishTrajectory.h" -#include "cartographer_ros_msgs/SubmapEntry.h" -#include "cartographer_ros_msgs/SubmapList.h" -#include "cartographer_ros_msgs/SubmapQuery.h" -#include "cartographer_ros_msgs/TrajectorySubmapList.h" -#include "gflags/gflags.h" -#include "glog/logging.h" -#include "nav_msgs/Odometry.h" -#include "ros/ros.h" -#include "ros/serialization.h" -#include "sensor_msgs/PointCloud2.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" DEFINE_string(configuration_directory, "", "First directory in which configuration files are searched, " @@ -66,316 +35,13 @@ DEFINE_string(configuration_basename, "", namespace cartographer_ros { namespace { -namespace carto = ::cartographer; - -using carto::transform::Rigid3d; - -constexpr int kInfiniteSubscriberQueueSize = 0; -constexpr int kLatestOnlyPublisherQueueSize = 1; -constexpr double kTfBufferCacheTimeInSeconds = 1e6; - -// Unique 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 kOccupancyGridTopic[] = "map"; -constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; -constexpr char kSubmapListTopic[] = "submap_list"; -constexpr char kSubmapQueryServiceName[] = "submap_query"; -constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; - -// Node that listens to all the sensor data that we are interested in and wires -// it up to the SLAM. -class Node { - public: - Node(const NodeOptions& options); - ~Node(); - - Node(const Node&) = delete; - Node& operator=(const Node&) = delete; - - void SpinForever(); - void Initialize(); - - private: - bool HandleSubmapQuery( - ::cartographer_ros_msgs::SubmapQuery::Request& request, - ::cartographer_ros_msgs::SubmapQuery::Response& response); - bool HandleFinishTrajectory( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response& response); - - void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); - void PublishPoseAndScanMatchedPointCloud( - const ::ros::WallTimerEvent& timer_event); - void SpinOccupancyGridThreadForever(); - - const NodeOptions options_; - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_; - tf2_ros::TransformBroadcaster tf_broadcaster_; - - carto::common::Mutex mutex_; - std::unique_ptr map_builder_bridge_ GUARDED_BY(mutex_); - - ::ros::NodeHandle node_handle_; - ::ros::Subscriber imu_subscriber_; - ::ros::Subscriber horizontal_laser_scan_subscriber_; - std::vector<::ros::Subscriber> point_cloud_subscribers_; - ::ros::Subscriber odometry_subscriber_; - ::ros::Publisher submap_list_publisher_; - ::ros::ServiceServer submap_query_server_; - ::ros::Publisher scan_matched_point_cloud_publisher_; - carto::common::Time last_scan_matched_point_cloud_time_ = - carto::common::Time::min(); - ::ros::ServiceServer finish_trajectory_server_; - - ::ros::Publisher occupancy_grid_publisher_; - std::thread occupancy_grid_thread_; - bool terminating_ = false GUARDED_BY(mutex_); - - // We have to keep the timer handles of ::ros::WallTimers around, otherwise - // they do not fire. - std::vector<::ros::WallTimer> wall_timers_; -}; - -Node::Node(const NodeOptions& options) - : options_(options), - tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)), - tf_(tf_buffer_) {} - -Node::~Node() { - { - carto::common::MutexLocker lock(&mutex_); - terminating_ = true; - } - if (occupancy_grid_thread_.joinable()) { - occupancy_grid_thread_.join(); - } -} - -void Node::Initialize() { - carto::common::MutexLocker lock(&mutex_); - std::unordered_set expected_sensor_ids; - - // For 2D SLAM, subscribe to exactly one horizontal laser. - if (options_.use_laser_scan) { - horizontal_laser_scan_subscriber_ = node_handle_.subscribe( - kLaserScanTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::LaserScan::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandleLaserScanMessage( - kLaserScanTopic, msg); - })); - expected_sensor_ids.insert(kLaserScanTopic); - } - if (options_.use_multi_echo_laser_scan) { - horizontal_laser_scan_subscriber_ = node_handle_.subscribe( - kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge() - ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, - msg); - })); - expected_sensor_ids.insert(kMultiEchoLaserScanTopic); - } - - // 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); - } - point_cloud_subscribers_.push_back(node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message( - topic, msg); - }))); - expected_sensor_ids.insert(topic); - } - } - - // 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())) { - imu_subscriber_ = node_handle_.subscribe( - kImuTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const sensor_msgs::Imu::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic, - msg); - })); - expected_sensor_ids.insert(kImuTopic); - } - - if (options_.use_odometry) { - odometry_subscriber_ = node_handle_.subscribe( - kOdometryTopic, kInfiniteSubscriberQueueSize, - boost::function( - [this](const nav_msgs::Odometry::ConstPtr& msg) { - map_builder_bridge_->sensor_bridge()->HandleOdometryMessage( - kOdometryTopic, msg); - })); - expected_sensor_ids.insert(kOdometryTopic); - } - - map_builder_bridge_ = carto::common::make_unique( - options_, expected_sensor_ids, &tf_buffer_); - - submap_list_publisher_ = - node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( - kSubmapListTopic, kLatestOnlyPublisherQueueSize); - submap_query_server_ = node_handle_.advertiseService( - kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); - - if (options_.map_builder_options.use_trajectory_builder_2d()) { - occupancy_grid_publisher_ = - node_handle_.advertise<::nav_msgs::OccupancyGrid>( - kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, - true /* latched */); - occupancy_grid_thread_ = - std::thread(&Node::SpinOccupancyGridThreadForever, this); - } - - scan_matched_point_cloud_publisher_ = - node_handle_.advertise( - kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize); - - finish_trajectory_server_ = node_handle_.advertiseService( - kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this); - - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(options_.submap_publish_period_sec), - &Node::PublishSubmapList, this)); - wall_timers_.push_back(node_handle_.createWallTimer( - ::ros::WallDuration(options_.pose_publish_period_sec), - &Node::PublishPoseAndScanMatchedPointCloud, this)); -} - -bool Node::HandleSubmapQuery( - ::cartographer_ros_msgs::SubmapQuery::Request& request, - ::cartographer_ros_msgs::SubmapQuery::Response& response) { - carto::common::MutexLocker lock(&mutex_); - return map_builder_bridge_->HandleSubmapQuery(request, response); -} - -bool Node::HandleFinishTrajectory( - ::cartographer_ros_msgs::FinishTrajectory::Request& request, - ::cartographer_ros_msgs::FinishTrajectory::Response& response) { - carto::common::MutexLocker lock(&mutex_); - return map_builder_bridge_->HandleFinishTrajectory(request, response); -} - -void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { - carto::common::MutexLocker lock(&mutex_); - submap_list_publisher_.publish(map_builder_bridge_->GetSubmapList()); -} - -void Node::PublishPoseAndScanMatchedPointCloud( - const ::ros::WallTimerEvent& timer_event) { - carto::common::MutexLocker lock(&mutex_); - const carto::mapping::TrajectoryBuilder* trajectory_builder = - map_builder_bridge_->map_builder()->GetTrajectoryBuilder( - map_builder_bridge_->trajectory_id()); - const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = - trajectory_builder->pose_estimate(); - if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { - return; - } - - const Rigid3d tracking_to_local = last_pose_estimate.pose; - const Rigid3d local_to_map = - map_builder_bridge_->map_builder() - ->sparse_pose_graph() - ->GetLocalToGlobalTransform(*trajectory_builder->submaps()); - const Rigid3d tracking_to_map = local_to_map * tracking_to_local; - - geometry_msgs::TransformStamped stamped_transform; - stamped_transform.header.stamp = ToRos(last_pose_estimate.time); - - // We only publish a point cloud if it has changed. It is not needed at high - // frequency, and republishing it would be computationally wasteful. - if (last_pose_estimate.time != last_scan_matched_point_cloud_time_) { - scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( - carto::common::ToUniversal(last_pose_estimate.time), - options_.tracking_frame, - carto::sensor::TransformPointCloud( - last_pose_estimate.point_cloud, - tracking_to_local.inverse().cast()))); - last_scan_matched_point_cloud_time_ = last_pose_estimate.time; - } else { - // If we do not publish a new point cloud, we still allow time of the - // published poses to advance. - stamped_transform.header.stamp = ros::Time::now(); - } - - const auto published_to_tracking = - map_builder_bridge_->tf_bridge()->LookupToTracking( - last_pose_estimate.time, options_.published_frame); - if (published_to_tracking != nullptr) { - if (options_.provide_odom_frame) { - std::vector stamped_transforms; - - stamped_transform.header.frame_id = options_.map_frame; - stamped_transform.child_frame_id = options_.odom_frame; - stamped_transform.transform = ToGeometryMsgTransform(local_to_map); - stamped_transforms.push_back(stamped_transform); - - stamped_transform.header.frame_id = options_.odom_frame; - stamped_transform.child_frame_id = options_.published_frame; - stamped_transform.transform = - ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking)); - stamped_transforms.push_back(stamped_transform); - - tf_broadcaster_.sendTransform(stamped_transforms); - } else { - stamped_transform.header.frame_id = options_.map_frame; - stamped_transform.child_frame_id = options_.published_frame; - stamped_transform.transform = - ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking)); - tf_broadcaster_.sendTransform(stamped_transform); - } - } -} - -void Node::SpinOccupancyGridThreadForever() { - for (;;) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - { - carto::common::MutexLocker lock(&mutex_); - if (terminating_) { - return; - } - } - if (occupancy_grid_publisher_.getNumSubscribers() == 0) { - continue; - } - const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid(); - if (occupancy_grid != nullptr) { - occupancy_grid_publisher_.publish(*occupancy_grid); - } - } -} - -void Node::SpinForever() { ::ros::spin(); } - void Run() { - auto file_resolver = - carto::common::make_unique( - std::vector{FLAGS_configuration_directory}); + auto file_resolver = cartographer::common::make_unique< + cartographer::common::ConfigurationFileResolver>( + std::vector{FLAGS_configuration_directory}); const string code = file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); - carto::common::LuaParameterDictionary lua_parameter_dictionary( + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); Node node(CreateNodeOptions(&lua_parameter_dictionary)); @@ -398,7 +64,7 @@ int main(int argc, char** argv) { ::ros::init(argc, argv, "cartographer_node"); ::ros::start(); - ::cartographer_ros::ScopedRosLogSink ros_log_sink; - ::cartographer_ros::Run(); + cartographer_ros::ScopedRosLogSink ros_log_sink; + cartographer_ros::Run(); ::ros::shutdown(); }