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

Follow googlecartographer/cartographer#736 (#620)

master
Christoph Schütte 2017-12-08 13:09:15 +01:00 committed by Wally B. Feed
parent 7bcdda4d37
commit 88609432f8
4 changed files with 19 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -43,7 +43,7 @@ SensorBridge::SensorBridge(
const int num_subdivisions_per_laser_scan, const int num_subdivisions_per_laser_scan,
const std::string& tracking_frame, const std::string& tracking_frame,
const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer, const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilder* const trajectory_builder) carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {} trajectory_builder_(trajectory_builder) {}
@ -67,8 +67,9 @@ void SensorBridge::HandleOdometryMessage(
std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data = std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
ToOdometryData(msg); ToOdometryData(msg);
if (odometry_data != nullptr) { if (odometry_data != nullptr) {
trajectory_builder_->AddOdometerData(sensor_id, odometry_data->time, trajectory_builder_->AddSensorData(
odometry_data->pose); sensor_id, cartographer::sensor::OdometryData{odometry_data->time,
odometry_data->pose});
} }
} }
@ -106,9 +107,10 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) { if (imu_data != nullptr) {
trajectory_builder_->AddImuData(sensor_id, imu_data->time, trajectory_builder_->AddSensorData(
sensor_id, cartographer::sensor::ImuData{imu_data->time,
imu_data->linear_acceleration, imu_data->linear_acceleration,
imu_data->angular_velocity); imu_data->angular_velocity});
} }
} }
@ -169,10 +171,11 @@ void SensorBridge::HandleRangefinder(
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) {
trajectory_builder_->AddRangefinderData( trajectory_builder_->AddSensorData(
sensor_id, time, sensor_to_tracking->translation().cast<float>(), sensor_id, cartographer::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud( carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())); ranges, sensor_to_tracking->cast<float>())});
} }
} }

7
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -19,7 +19,7 @@
#include <memory> #include <memory>
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -42,7 +42,7 @@ class SensorBridge {
explicit SensorBridge( explicit SensorBridge(
int num_subdivisions_per_laser_scan, const std::string& tracking_frame, int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer, double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilder* trajectory_builder); ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
SensorBridge(const SensorBridge&) = delete; SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete; SensorBridge& operator=(const SensorBridge&) = delete;
@ -77,7 +77,8 @@ class SensorBridge {
const int num_subdivisions_per_laser_scan_; const int num_subdivisions_per_laser_scan_;
const TfBridge tf_bridge_; const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_; ::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

2
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

@ -16,7 +16,7 @@
#include "cartographer_ros/trajectory_options.h" #include "cartographer_ros/trajectory_options.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder_interface.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/time_conversion.h" #include "cartographer_ros/time_conversion.h"