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/make_unique.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/kalman_filter/pose_tracker.h"
@ -93,7 +92,6 @@ using carto::kalman_filter::PoseCovariance;
constexpr int64 kTrajectoryBuilderId = 0; constexpr int64 kTrajectoryBuilderId = 0;
constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kInfiniteSubscriberQueueSize = 0;
constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
// Unique default topic names. Expected to be remapped as needed. // Unique default topic names. Expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "scan"; constexpr char kLaserScanTopic[] = "scan";
@ -121,8 +119,6 @@ class Node {
void Initialize(); void Initialize();
private: private:
void HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data);
bool HandleSubmapQuery( bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response); ::cartographer_ros_msgs::SubmapQuery::Response& response);
@ -146,8 +142,7 @@ class Node {
std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_ std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_); carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
carto::sensor::Collator sensor_collator_ GUARDED_BY(mutex_); std::unique_ptr<SensorBridge> sensor_bridge_ GUARDED_BY(mutex_);
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
::ros::NodeHandle node_handle_; ::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_; ::ros::Subscriber imu_subscriber_;
@ -165,10 +160,6 @@ class Node {
std::thread occupancy_grid_thread_; std::thread occupancy_grid_thread_;
bool terminating_ = false GUARDED_BY(mutex_); 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 // We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire. // they do not fire.
std::vector<::ros::WallTimer> wall_timers_; std::vector<::ros::WallTimer> wall_timers_;
@ -180,9 +171,7 @@ Node::Node(const NodeOptions& options)
tf_(tf_buffer_), tf_(tf_buffer_),
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec, tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec,
&tf_buffer_), &tf_buffer_),
map_builder_(options.map_builder_options, &constant_data_), map_builder_(options.map_builder_options, &constant_data_) {}
sensor_bridge_(options.sensor_bridge_options, &tf_bridge_,
kTrajectoryBuilderId, &sensor_collator_) {}
Node::~Node() { Node::~Node() {
{ {
@ -197,7 +186,7 @@ Node::~Node() {
void Node::Initialize() { void Node::Initialize() {
// Set of all topics we subscribe to. We use the non-remapped default names // Set of all topics we subscribe to. We use the non-remapped default names
// which are unique. // 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. // For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_horizontal_laser) { if (options_.use_horizontal_laser) {
@ -205,19 +194,19 @@ void Node::Initialize() {
kLaserScanTopic, kInfiniteSubscriberQueueSize, kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) { [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) { if (options_.use_horizontal_multi_echo_laser) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe( horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
sensor_bridge_.HandleMultiEchoLaserScanMessage( sensor_bridge_->HandleMultiEchoLaserScanMessage(
kMultiEchoLaserScanTopic, msg); kMultiEchoLaserScanTopic, msg);
})); }));
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
} }
// For 3D SLAM, subscribe to all 3D lasers. // For 3D SLAM, subscribe to all 3D lasers.
@ -231,9 +220,9 @@ void Node::Initialize() {
topic, kInfiniteSubscriberQueueSize, topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>( boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { [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, kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>( boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](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) { if (options_.use_odometry_data) {
@ -257,19 +246,17 @@ void Node::Initialize() {
kOdometryTopic, kInfiniteSubscriberQueueSize, kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>( boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) { [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. // TODO(damonkohler): Add multi-trajectory support.
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder()); CHECK_EQ(kTrajectoryBuilderId,
sensor_collator_.AddTrajectory( map_builder_.AddTrajectoryBuilder(expected_sensor_ids));
kTrajectoryBuilderId, expected_sensor_identifiers, sensor_bridge_ = carto::common::make_unique<SensorBridge>(
[this](const string& sensor_id, options_.sensor_bridge_options, &tf_bridge_,
std::unique_ptr<carto::sensor::Data> sensor_data) { map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId));
HandleSensorData(sensor_id, std::move(sensor_data));
});
submap_list_publisher_ = submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
@ -310,7 +297,7 @@ bool Node::HandleSubmapQuery(
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
// TODO(hrapp): return error messages and extract common code from MapBuilder. // 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(); map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
if (request.submap_id < 0 || request.submap_id >= submaps->size()) { if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
return false; return false;
@ -361,7 +348,7 @@ bool Node::HandleFinishTrajectory(
::ros::shutdown(); ::ros::shutdown();
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
// TODO(whess): Add multi-trajectory support. // TODO(whess): Add multi-trajectory support.
sensor_collator_.FinishTrajectory(kTrajectoryBuilderId); map_builder_.FinishTrajectory(kTrajectoryBuilderId);
map_builder_.sparse_pose_graph()->RunFinalOptimization(); map_builder_.sparse_pose_graph()->RunFinalOptimization();
const auto trajectory_nodes = 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 Node::SpinForever() { ::ros::spin(); }
void Run() { void Run() {

54
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -56,14 +56,12 @@ SensorBridgeOptions CreateSensorBridgeOptions(
return options; return options;
} }
SensorBridge::SensorBridge(const SensorBridgeOptions& options, SensorBridge::SensorBridge(
const TfBridge* const tf_bridge, const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
const int trajectory_id, carto::mapping::TrajectoryBuilder* const trajectory_builder)
carto::sensor::Collator* const sensor_collator)
: options_(options), : options_(options),
tf_bridge_(tf_bridge), tf_bridge_(tf_bridge),
trajectory_id_(trajectory_id), trajectory_builder_(trajectory_builder) {}
sensor_collator_(sensor_collator) {}
void SensorBridge::HandleOdometryMessage( void SensorBridge::HandleOdometryMessage(
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) { const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
@ -83,12 +81,9 @@ void SensorBridge::HandleOdometryMessage(
const auto sensor_to_tracking = tf_bridge_->LookupToTracking( const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id)); time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( trajectory_builder_->AddOdometerPose(
trajectory_id_, topic, topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
carto::common::make_unique<::cartographer::sensor::Data>( covariance);
time, ::cartographer::sensor::Data::Odometry{
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. " << "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will " "Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise."; "otherwise be imprecise.";
sensor_collator_->AddSensorData( trajectory_builder_->AddImuData(
trajectory_id_, topic, topic, time,
carto::common::make_unique<::cartographer::sensor::Data>( sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
time, sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
::cartographer::sensor::Data::Imu{
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( const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id)); time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( trajectory_builder_->AddLaserFan(
trajectory_id_, topic, topic, time,
carto::common::make_unique<::cartographer::sensor::Data>( carto::sensor::TransformLaserFan(
time, carto::sensor::TransformLaserFan(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>()))); sensor_to_tracking->cast<float>()));
} }
} }
void SensorBridge::HandleLaserScanProto( void SensorBridge::HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time, const string& topic, const carto::common::Time time, const string& frame_id,
const string& frame_id, const carto::sensor::proto::LaserScan& laser_scan) {
const ::cartographer::sensor::proto::LaserScan& laser_scan) {
const auto laser_fan = carto::sensor::ToLaserFan( const auto laser_fan = carto::sensor::ToLaserFan(
laser_scan, options_.horizontal_laser_min_range, laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range, options_.horizontal_laser_max_range,
@ -156,11 +144,9 @@ void SensorBridge::HandleLaserScanProto(
const auto sensor_to_tracking = const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id)); tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( trajectory_builder_->AddLaserFan(
trajectory_id_, topic, topic, time, carto::sensor::TransformLaserFan(
carto::common::make_unique<::cartographer::sensor::Data>( laser_fan, sensor_to_tracking->cast<float>()));
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_ #ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
#include "cartographer/sensor/collator.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/tf_bridge.h"
@ -47,9 +46,9 @@ SensorBridgeOptions CreateSensorBridgeOptions(
// Converts ROS messages into SensorData in tracking frame for the MapBuilder. // Converts ROS messages into SensorData in tracking frame for the MapBuilder.
class SensorBridge { class SensorBridge {
public: public:
explicit SensorBridge(const SensorBridgeOptions& options, explicit SensorBridge(
const TfBridge* tf_bridge, int trajectory_id, const SensorBridgeOptions& options, const TfBridge* tf_bridge,
::cartographer::sensor::Collator* sensor_collator); ::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
SensorBridge(const SensorBridge&) = delete; SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete; SensorBridge& operator=(const SensorBridge&) = delete;
@ -74,8 +73,7 @@ class SensorBridge {
const SensorBridgeOptions options_; const SensorBridgeOptions options_;
const TfBridge* const tf_bridge_; const TfBridge* const tf_bridge_;
const int trajectory_id_; ::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
::cartographer::sensor::Collator* const sensor_collator_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros