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

Also extrapolate based on odometry if available. (#479)

master
Wolfgang Hess 2017-08-09 12:36:50 +02:00 committed by GitHub
parent 9867aeb1a3
commit f43e05626e
3 changed files with 27 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer_ros/cartographer_ros/node.cc
View File

@ -448,8 +448,12 @@ void Node::HandleOdometryMessage(const int trajectory_id,
const string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_);
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleOdometryMessage(sensor_id, msg);
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
if (odometry_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id,

24
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -47,15 +47,27 @@ SensorBridge::SensorBridge(
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {}
void SensorBridge::HandleOdometryMessage(
const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::OdometryData>
SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddOdometerData(
sensor_id, time,
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse());
if (sensor_to_tracking == nullptr) {
return nullptr;
}
return ::cartographer::common::make_unique<
::cartographer::sensor::OdometryData>(
::cartographer::sensor::OdometryData{
time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}
void SensorBridge::HandleOdometryMessage(
const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
ToOdometryData(msg);
if (odometry_data != nullptr) {
trajectory_builder_->AddOdometerData(sensor_id, odometry_data->time,
odometry_data->pose);
}
}

3
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -21,6 +21,7 @@
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/tf_bridge.h"
@ -46,6 +47,8 @@ class SensorBridge {
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg);
void HandleOdometryMessage(const string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(