Pulls out node.h/cc (#189)
Damon Kohler
GitHub
parent
d5652c6985
commit
94fc589141
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -14,6 +14,9 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
|
||||
#define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_set>
|
||||
|
||||
|
|
@ -69,3 +72,5 @@ class MapBuilderBridge {
|
|||
};
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
|
||||
|
|
|
|||
|
|
@ -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 <chrono>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<string> 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<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
||||
[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<void(const nav_msgs::Odometry::ConstPtr&)>(
|
||||
[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<MapBuilderBridge>(
|
||||
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<sensor_msgs::PointCloud2>(
|
||||
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<float>())));
|
||||
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<geometry_msgs::TransformStamped> 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
|
||||
|
|
@ -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 <memory>
|
||||
#include <vector>
|
||||
|
||||
#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<MapBuilderBridge> 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_
|
||||
|
|
@ -14,46 +14,15 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<MapBuilderBridge> 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<string> 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<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||
[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<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
||||
[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<void(const nav_msgs::Odometry::ConstPtr&)>(
|
||||
[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<MapBuilderBridge>(
|
||||
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<sensor_msgs::PointCloud2>(
|
||||
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<float>())));
|
||||
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<geometry_msgs::TransformStamped> 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<carto::common::ConfigurationFileResolver>(
|
||||
auto file_resolver = cartographer::common::make_unique<
|
||||
cartographer::common::ConfigurationFileResolver>(
|
||||
std::vector<string>{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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue