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

39
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -33,7 +33,6 @@
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/map_builder.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/local_trajectory_builder.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_options.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/sensor/collator.h"
#include "cartographer/sensor/data.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
@ -121,7 +121,7 @@ class Node {
void Initialize();
private:
void HandleSensorData(int64 timestamp,
void HandleSensorData(const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data);
bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
@ -266,9 +266,9 @@ void Node::Initialize() {
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
sensor_collator_.AddTrajectory(
kTrajectoryBuilderId, expected_sensor_identifiers,
[this](const int64 timestamp,
[this](const string& sensor_id,
std::unique_ptr<carto::sensor::Data> sensor_data) {
HandleSensorData(timestamp, std::move(sensor_data));
HandleSensorData(sensor_id, std::move(sensor_data));
});
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) {
auto it = rate_timers_.find(sensor_data->frame_id);
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end()) {
it = rate_timers_
.emplace(std::piecewise_construct,
std::forward_as_tuple(sensor_data->frame_id),
std::forward_as_tuple(carto::common::FromSeconds(
kSensorDataRatesLoggingPeriodSeconds)))
.first;
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(carto::common::FromUniversal(timestamp));
it->second.Pulse(sensor_data->time);
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
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();
}
const auto time = carto::common::FromUniversal(timestamp);
auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
switch (sensor_data->type) {
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);
return;
case carto::sensor::Data::Type::kLaserFan:
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 {
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;
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);
return;
}

44
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

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