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

New Cartographer API. (#178)

master
Damon Kohler 2016-11-21 10:56:50 +01:00 committed by GitHub
parent 797ee08c5b
commit d09c33e8f8
4 changed files with 22 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -229,24 +229,6 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
return proto;
}
::cartographer::sensor::proto::LaserFan ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
::cartographer::sensor::proto::LaserFan proto;
auto* origin = proto.mutable_origin();
origin->set_x(0.);
origin->set_y(0.);
origin->set_z(0.);
auto* point_cloud = proto.mutable_point_cloud();
for (const auto& point : pcl_points) {
point_cloud->add_x(point.x);
point_cloud->add_y(point.y);
point_cloud->add_z(point.z);
}
return proto;
}
Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
return Rigid3d(ToEigen(transform.transform.translation),
ToEigen(transform.transform.rotation));

3
cartographer_ros/cartographer_ros/msg_conversion.h
View File

@ -64,9 +64,6 @@ geometry_msgs::Pose ToGeometryMsgPose(
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::MultiEchoLaserScan& msg);
::cartographer::sensor::proto::LaserFan ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points);
::cartographer::transform::Rigid3d ToRigid3d(
const geometry_msgs::TransformStamped& transform);

51
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -74,7 +74,7 @@ void SensorBridge::HandleOdometryMessage(
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddOdometerPose(
trajectory_builder_->AddOdometerData(
topic, time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
covariance);
}
@ -101,47 +101,40 @@ void SensorBridge::HandleImuMessage(const string& topic,
void SensorBridge::HandleLaserScanMessage(
const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) {
HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
ToCartographer(*msg));
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id,
carto::sensor::ToPointCloud(ToCartographer(*msg)));
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id,
ToCartographer(*msg));
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id,
carto::sensor::ToPointCloud(ToCartographer(*msg)));
}
void SensorBridge::HandlePointCloud2Message(
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
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, CheckNoLeadingSlash(msg->header.frame_id));
carto::sensor::PointCloud point_cloud;
for (const auto& point : pcl_point_cloud) {
point_cloud.emplace_back(point.x, point.y, point.z);
}
HandleRangefinder(topic, FromRos(msg->header.stamp), msg->header.frame_id,
point_cloud);
}
void SensorBridge::HandleRangefinder(const string& topic,
const carto::common::Time time,
const string& frame_id,
const carto::sensor::PointCloud& ranges) {
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddLaserFan(
topic, time,
carto::sensor::TransformLaserFan(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
trajectory_builder_->AddRangefinderData(
topic, time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformPointCloud(ranges,
sensor_to_tracking->cast<float>()));
}
}
void SensorBridge::HandleLaserScanProto(
const string& topic, const carto::common::Time time, const string& frame_id,
const carto::sensor::proto::LaserScan& laser_scan) {
const carto::sensor::LaserFan laser_fan = {
Eigen::Vector3f::Zero(),
carto::sensor::ToPointCloud(laser_scan),
{},
{}};
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddLaserFan(
topic, time, carto::sensor::TransformLaserFan(
laser_fan, sensor_to_tracking->cast<float>()));
}
}
} // namespace cartographer_ros

6
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -63,10 +63,10 @@ class SensorBridge {
const sensor_msgs::PointCloud2::ConstPtr& msg);
private:
void HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time,
void HandleRangefinder(const string& topic,
const ::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan);
const ::cartographer::sensor::PointCloud& ranges);
const SensorBridgeOptions options_;
const TfBridge* const tf_bridge_;