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

Updated to work with new APIs. (#125)

master
Damon Kohler 2016-10-18 18:04:46 +02:00 committed by Wolfgang Hess
parent caa77c8588
commit b63611b2c3
2 changed files with 42 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -33,7 +33,6 @@
#include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/proto/submaps.pb.h" #include "cartographer/mapping/proto/submaps.pb.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/mapping_2d/global_trajectory_builder.h" #include "cartographer/mapping_2d/global_trajectory_builder.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h" #include "cartographer/mapping_2d/local_trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/sparse_pose_graph.h"
@ -41,6 +40,7 @@
#include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping_3d/local_trajectory_builder.h"
#include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
@ -121,7 +121,7 @@ class Node {
void Initialize(); void Initialize();
private: private:
void HandleSensorData(int64 timestamp, void HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data); std::unique_ptr<carto::sensor::Data> sensor_data);
bool HandleSubmapQuery( bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
@ -266,9 +266,9 @@ void Node::Initialize() {
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder()); CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
sensor_collator_.AddTrajectory( sensor_collator_.AddTrajectory(
kTrajectoryBuilderId, expected_sensor_identifiers, kTrajectoryBuilderId, expected_sensor_identifiers,
[this](const int64 timestamp, [this](const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data) { std::unique_ptr<carto::sensor::Data> sensor_data) {
HandleSensorData(timestamp, std::move(sensor_data)); HandleSensorData(sensor_id, std::move(sensor_data));
}); });
submap_list_publisher_ = submap_list_publisher_ =
@ -490,18 +490,18 @@ void Node::SpinOccupancyGridThreadForever() {
} }
} }
void Node::HandleSensorData(const int64 timestamp, void Node::HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data) { std::unique_ptr<carto::sensor::Data> sensor_data) {
auto it = rate_timers_.find(sensor_data->frame_id); auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) { if (it == rate_timers_.end()) {
it = rate_timers_ it =
.emplace(std::piecewise_construct, rate_timers_
std::forward_as_tuple(sensor_data->frame_id), .emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(carto::common::FromSeconds( std::forward_as_tuple(carto::common::FromSeconds(
kSensorDataRatesLoggingPeriodSeconds))) kSensorDataRatesLoggingPeriodSeconds)))
.first; .first;
} }
it->second.Pulse(carto::common::FromUniversal(timestamp)); it->second.Pulse(sensor_data->time);
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ > if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
@ -511,26 +511,29 @@ void Node::HandleSensorData(const int64 timestamp,
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now(); last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
} }
const auto time = carto::common::FromUniversal(timestamp);
auto* const trajectory_builder = auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId); map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
switch (sensor_data->type) { switch (sensor_data->type) {
case carto::sensor::Data::Type::kImu: case carto::sensor::Data::Type::kImu:
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration, trajectory_builder->AddImuData(sensor_data->time,
sensor_data->imu.linear_acceleration,
sensor_data->imu.angular_velocity); sensor_data->imu.angular_velocity);
return; return;
case carto::sensor::Data::Type::kLaserFan: case carto::sensor::Data::Type::kLaserFan:
if (options_.map_builder_options.use_trajectory_builder_2d()) { if (options_.map_builder_options.use_trajectory_builder_2d()) {
trajectory_builder->AddHorizontalLaserFan(time, sensor_data->laser_fan); trajectory_builder->AddHorizontalLaserFan(sensor_data->time,
sensor_data->laser_fan);
} else { } else {
CHECK(options_.map_builder_options.use_trajectory_builder_3d()); CHECK(options_.map_builder_options.use_trajectory_builder_3d());
trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan); trajectory_builder->AddLaserFan3D(sensor_data->time,
sensor_data->laser_fan);
} }
return; return;
case carto::sensor::Data::Type::kOdometry: case carto::sensor::Data::Type::kOdometry:
trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose, trajectory_builder->AddOdometerPose(sensor_data->time,
sensor_data->odometry.pose,
sensor_data->odometry.covariance); sensor_data->odometry.covariance);
return; return;
} }

34
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -80,14 +80,13 @@ void SensorBridge::HandleOdometryMessage(
translational, Eigen::Matrix3d::Zero(), translational, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotational; Eigen::Matrix3d::Zero(), rotational;
// clang-format on // clang-format on
const auto sensor_to_tracking = const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
tf_bridge_->LookupToTracking(time, msg->child_frame_id); time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->child_frame_id), time, ::cartographer::sensor::Data::Odometry{
::cartographer::sensor::Data::Odometry{
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(), ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance})); covariance}));
} }
@ -98,17 +97,17 @@ void SensorBridge::HandleImuMessage(const string& topic,
CHECK_NE(msg->linear_acceleration_covariance[0], -1); CHECK_NE(msg->linear_acceleration_covariance[0], -1);
CHECK_NE(msg->angular_velocity_covariance[0], -1); CHECK_NE(msg->angular_velocity_covariance[0], -1);
const carto::common::Time time = FromRos(msg->header.stamp); const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
tf_bridge_->LookupToTracking(time, msg->header.frame_id); time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
CHECK(sensor_to_tracking->translation().norm() < 1e-5) CHECK(sensor_to_tracking->translation().norm() < 1e-5)
<< "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( sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->header.frame_id), time,
::cartographer::sensor::Data::Imu{ ::cartographer::sensor::Data::Imu{
sensor_to_tracking->rotation() * sensor_to_tracking->rotation() *
ToEigen(msg->linear_acceleration), ToEigen(msg->linear_acceleration),
@ -134,14 +133,13 @@ void SensorBridge::HandlePointCloud2Message(
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud; pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(*msg, pcl_point_cloud); pcl::fromROSMsg(*msg, pcl_point_cloud);
const carto::common::Time time = FromRos(msg->header.stamp); const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
tf_bridge_->LookupToTracking(time, msg->header.frame_id); time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->header.frame_id), time, carto::sensor::TransformLaserFan(
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>())));
} }
@ -155,13 +153,13 @@ void SensorBridge::HandleLaserScanProto(
laser_scan, options_.horizontal_laser_min_range, laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range, options_.horizontal_laser_max_range,
options_.horizontal_laser_missing_echo_ray_length); options_.horizontal_laser_missing_echo_ray_length);
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(time, frame_id); const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
sensor_collator_->AddSensorData( sensor_collator_->AddSensorData(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(frame_id), time, carto::sensor::TransformLaserFan(
carto::sensor::TransformLaserFan(
laser_fan, sensor_to_tracking->cast<float>()))); laser_fan, sensor_to_tracking->cast<float>())));
} }
} }