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

Improve parameter names. (#185)

master
Damon Kohler 2016-11-25 11:31:57 +01:00 committed by GitHub
parent e5b63e377a
commit 516c86fa5a
2 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -43,17 +43,18 @@ SensorBridge::SensorBridge(
: tf_bridge_(tf_bridge), trajectory_builder_(trajectory_builder) {} : tf_bridge_(tf_bridge), trajectory_builder_(trajectory_builder) {}
void SensorBridge::HandleOdometryMessage( void SensorBridge::HandleOdometryMessage(
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) { const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp); const carto::common::Time time = FromRos(msg->header.stamp);
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) {
trajectory_builder_->AddOdometerData( trajectory_builder_->AddOdometerData(
topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()); sensor_id, time,
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse());
} }
} }
void SensorBridge::HandleImuMessage(const string& topic, void SensorBridge::HandleImuMessage(const string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
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);
@ -66,37 +67,38 @@ void SensorBridge::HandleImuMessage(const string& topic,
"Transforming linear acceleration into the tracking frame will " "Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise."; "otherwise be imprecise.";
trajectory_builder_->AddImuData( trajectory_builder_->AddImuData(
topic, time, sensor_id, time,
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)); sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
} }
} }
void SensorBridge::HandleLaserScanMessage( void SensorBridge::HandleLaserScanMessage(
const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) { const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id, HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
carto::sensor::ToPointCloud(ToCartographer(*msg))); carto::sensor::ToPointCloud(ToCartographer(*msg)));
} }
void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const string& sensor_id,
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
carto::sensor::ToPointCloud(ToCartographer(*msg))); carto::sensor::ToPointCloud(ToCartographer(*msg)));
} }
void SensorBridge::HandlePointCloud2Message( void SensorBridge::HandlePointCloud2Message(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) {
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);
carto::sensor::PointCloud point_cloud; carto::sensor::PointCloud point_cloud;
for (const auto& point : pcl_point_cloud) { for (const auto& point : pcl_point_cloud) {
point_cloud.emplace_back(point.x, point.y, point.z); point_cloud.emplace_back(point.x, point.y, point.z);
} }
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id, HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
point_cloud); point_cloud);
} }
void SensorBridge::HandleRangefinder(const string& topic, void SensorBridge::HandleRangefinder(const string& sensor_id,
const carto::common::Time time, const carto::common::Time time,
const string& frame_id, const string& frame_id,
const carto::sensor::PointCloud& ranges) { const carto::sensor::PointCloud& ranges) {
@ -104,7 +106,7 @@ void SensorBridge::HandleRangefinder(const string& topic,
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_->AddRangefinderData(
topic, time, sensor_to_tracking->translation().cast<float>(), sensor_id, time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformPointCloud(ranges, carto::sensor::TransformPointCloud(ranges,
sensor_to_tracking->cast<float>())); sensor_to_tracking->cast<float>()));
} }

12
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -42,20 +42,20 @@ class SensorBridge {
SensorBridge(const SensorBridge&) = delete; SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete; SensorBridge& operator=(const SensorBridge&) = delete;
void HandleOdometryMessage(const string& topic, void HandleOdometryMessage(const string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg); const nav_msgs::Odometry::ConstPtr& msg);
void HandleImuMessage(const string& topic, void HandleImuMessage(const string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg); const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(const string& topic, void HandleLaserScanMessage(const string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg); const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage( void HandleMultiEchoLaserScanMessage(
const string& topic, const string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(const string& topic, void HandlePointCloud2Message(const string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg); const sensor_msgs::PointCloud2::ConstPtr& msg);
private: private:
void HandleRangefinder(const string& topic, void HandleRangefinder(const string& sensor_id,
const ::cartographer::common::Time time, const ::cartographer::common::Time time,
const string& frame_id, const string& frame_id,
const ::cartographer::sensor::PointCloud& ranges); const ::cartographer::sensor::PointCloud& ranges);