12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove direct use of sensor::Collator. (#132) 

Collating the sensor data is now handled by the MapBuilder.
master
Wolfgang Hess 2016-10-19 17:55:20 +02:00 committed by GitHub
parent e4abb77835
commit b8d9ac585e
3 changed files with 46 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -26,7 +26,6 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
@ -93,7 +92,6 @@ using carto::kalman_filter::PoseCovariance;
constexpr int64 kTrajectoryBuilderId = 0;
constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
// Unique default topic names. Expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan";
@ -121,8 +119,6 @@ class Node {
void Initialize();
private:
void HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data);
bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response);
@ -146,8 +142,7 @@ class Node {
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
GUARDED_BY(mutex_);
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
carto::sensor::Collator sensor_collator_ GUARDED_BY(mutex_);
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
std::unique_ptr<SensorBridge> sensor_bridge_ GUARDED_BY(mutex_);
::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_;
@ -165,10 +160,6 @@ class Node {
std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_);
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_;
std::map<string, carto::common::RateTimer<>> rate_timers_;
// We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire.
std::vector<::ros::WallTimer> wall_timers_;
@ -180,9 +171,7 @@ Node::Node(const NodeOptions& options)
tf_(tf_buffer_),
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
&tf_buffer_),
map_builder_(options.map_builder_options, &constant_data_),
sensor_bridge_(options.sensor_bridge_options, &tf_bridge_,
kTrajectoryBuilderId, &sensor_collator_) {}
map_builder_(options.map_builder_options, &constant_data_) {}
Node::~Node() {
{
@ -197,7 +186,7 @@ Node::~Node() {
void Node::Initialize() {
// Set of all topics we subscribe to. We use the non-remapped default names
// which are unique.
std::unordered_set<string> expected_sensor_identifiers;
std::unordered_set<string> expected_sensor_ids;
// For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_horizontal_laser) {
@ -205,19 +194,19 @@ void Node::Initialize() {
kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
sensor_bridge_.HandleLaserScanMessage(kLaserScanTopic, msg);
sensor_bridge_->HandleLaserScanMessage(kLaserScanTopic, msg);
}));
expected_sensor_identifiers.insert(kLaserScanTopic);
expected_sensor_ids.insert(kLaserScanTopic);
}
if (options_.use_horizontal_multi_echo_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
sensor_bridge_.HandleMultiEchoLaserScanMessage(
sensor_bridge_->HandleMultiEchoLaserScanMessage(
kMultiEchoLaserScanTopic, msg);
}));
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
}
// For 3D SLAM, subscribe to all 3D lasers.
@ -231,9 +220,9 @@ void Node::Initialize() {
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
sensor_bridge_.HandlePointCloud2Message(topic, msg);
sensor_bridge_->HandlePointCloud2Message(topic, msg);
})));
expected_sensor_identifiers.insert(topic);
expected_sensor_ids.insert(topic);
}
}
@ -247,9 +236,9 @@ void Node::Initialize() {
kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](const sensor_msgs::Imu::ConstPtr& msg) {
sensor_bridge_.HandleImuMessage(kImuTopic, msg);
sensor_bridge_->HandleImuMessage(kImuTopic, msg);
}));
expected_sensor_identifiers.insert(kImuTopic);
expected_sensor_ids.insert(kImuTopic);
}
if (options_.use_odometry_data) {
@ -257,19 +246,17 @@ void Node::Initialize() {
kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) {
sensor_bridge_.HandleOdometryMessage(kOdometryTopic, msg);
sensor_bridge_->HandleOdometryMessage(kOdometryTopic, msg);
}));
expected_sensor_identifiers.insert(kOdometryTopic);
expected_sensor_ids.insert(kOdometryTopic);
}
// TODO(damonkohler): Add multi-trajectory support.
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
sensor_collator_.AddTrajectory(
kTrajectoryBuilderId, expected_sensor_identifiers,
[this](const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data) {
HandleSensorData(sensor_id, std::move(sensor_data));
});
CHECK_EQ(kTrajectoryBuilderId,
map_builder_.AddTrajectoryBuilder(expected_sensor_ids));
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
options_.sensor_bridge_options, &tf_bridge_,
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId));
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
@ -310,7 +297,7 @@ bool Node::HandleSubmapQuery(
carto::common::MutexLocker lock(&mutex_);
// TODO(hrapp): return error messages and extract common code from MapBuilder.
carto::mapping::Submaps* submaps =
carto::mapping::Submaps* const submaps =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
return false;
@ -361,7 +348,7 @@ bool Node::HandleFinishTrajectory(
::ros::shutdown();
carto::common::MutexLocker lock(&mutex_);
// TODO(whess): Add multi-trajectory support.
sensor_collator_.FinishTrajectory(kTrajectoryBuilderId);
map_builder_.FinishTrajectory(kTrajectoryBuilderId);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
const auto trajectory_nodes =
@ -490,50 +477,6 @@ void Node::SpinOccupancyGridThreadForever() {
}
}
void Node::HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data) {
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) {
it =
rate_timers_
.emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(carto::common::FromSeconds(
kSensorDataRatesLoggingPeriodSeconds)))
.first;
}
it->second.Pulse(sensor_data->time);
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
for (const auto& pair : rate_timers_) {
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
}
auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
switch (sensor_data->type) {
case carto::sensor::Data::Type::kImu:
trajectory_builder->AddImuData(sensor_data->time,
sensor_data->imu.linear_acceleration,
sensor_data->imu.angular_velocity);
return;
case carto::sensor::Data::Type::kLaserFan:
trajectory_builder->AddLaserFan(sensor_data->time,
sensor_data->laser_fan);
return;
case carto::sensor::Data::Type::kOdometry:
trajectory_builder->AddOdometerPose(sensor_data->time,
sensor_data->odometry.pose,
sensor_data->odometry.covariance);
return;
}
LOG(FATAL);
}
void Node::SpinForever() { ::ros::spin(); }
void Run() {

56
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -56,14 +56,12 @@ SensorBridgeOptions CreateSensorBridgeOptions(
return options;
}
SensorBridge::SensorBridge(const SensorBridgeOptions& options,
const TfBridge* const tf_bridge,
const int trajectory_id,
carto::sensor::Collator* const sensor_collator)
SensorBridge::SensorBridge(
const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
carto::mapping::TrajectoryBuilder* const trajectory_builder)
: options_(options),
tf_bridge_(tf_bridge),
trajectory_id_(trajectory_id),
sensor_collator_(sensor_collator) {}
trajectory_builder_(trajectory_builder) {}
void SensorBridge::HandleOdometryMessage(
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
@ -83,12 +81,9 @@ void SensorBridge::HandleOdometryMessage(
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>(
time, ::cartographer::sensor::Data::Odometry{
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance}));
trajectory_builder_->AddOdometerPose(
topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance);
}
}
@ -104,15 +99,10 @@ void SensorBridge::HandleImuMessage(const string& topic,
<< "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise.";
sensor_collator_->AddSensorData(
trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>(
time,
::cartographer::sensor::Data::Imu{
sensor_to_tracking->rotation() *
ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
}));
trajectory_builder_->AddImuData(
topic, time,
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
}
}
@ -136,19 +126,17 @@ void SensorBridge::HandlePointCloud2Message(
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>(
time, carto::sensor::TransformLaserFan(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>())));
trajectory_builder_->AddLaserFan(
topic, time,
carto::sensor::TransformLaserFan(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>()));
}
}
void SensorBridge::HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
const string& topic, const carto::common::Time time, const string& frame_id,
const carto::sensor::proto::LaserScan& laser_scan) {
const auto laser_fan = carto::sensor::ToLaserFan(
laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range,
@ -156,11 +144,9 @@ void SensorBridge::HandleLaserScanProto(
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData(
trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>(
time, carto::sensor::TransformLaserFan(
laser_fan, sensor_to_tracking->cast<float>())));
trajectory_builder_->AddLaserFan(
topic, time, carto::sensor::TransformLaserFan(
laser_fan, sensor_to_tracking->cast<float>()));
}
}

12
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -17,8 +17,7 @@
#ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/tf_bridge.h"
@ -47,9 +46,9 @@ SensorBridgeOptions CreateSensorBridgeOptions(
// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
class SensorBridge {
public:
explicit SensorBridge(const SensorBridgeOptions& options,
const TfBridge* tf_bridge, int trajectory_id,
::cartographer::sensor::Collator* sensor_collator);
explicit SensorBridge(
const SensorBridgeOptions& options, const TfBridge* tf_bridge,
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
@ -74,8 +73,7 @@ class SensorBridge {
const SensorBridgeOptions options_;
const TfBridge* const tf_bridge_;
const int trajectory_id_;
::cartographer::sensor::Collator* const sensor_collator_;
::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
};
} // namespace cartographer_ros