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

Updates use of LaserFan following cartographer. (#114) 

This follows changes from googlecartographer/cartographer#62 and
googlecartographer/cartographer#64.
master
Wolfgang Hess 2016-10-14 12:41:00 +02:00 committed by GitHub
parent 4557d9fd12
commit 8d36c633b8
5 changed files with 16 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -162,11 +162,11 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message(
const int64 timestamp, const string& frame_id, const int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) { const ::cartographer::sensor::proto::LaserFan& laser_fan) {
CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0) CHECK(::cartographer::transform::ToEigen(laser_fan.origin()).norm() == 0)
<< "Trying to convert a laser_scan_3d that is not at the origin."; << "Trying to convert a laser_fan that is not at the origin.";
const auto& point_cloud = laser_scan_3d.point_cloud(); const auto& point_cloud = laser_fan.point_cloud();
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size()); CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size()); CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
const auto num_points = point_cloud.x_size(); const auto num_points = point_cloud.x_size();
@ -229,9 +229,9 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
return proto; return proto;
} }
::cartographer::sensor::proto::LaserFan3D ToCartographer( ::cartographer::sensor::proto::LaserFan ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) { const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
::cartographer::sensor::proto::LaserFan3D proto; ::cartographer::sensor::proto::LaserFan proto;
auto* origin = proto.mutable_origin(); auto* origin = proto.mutable_origin();
origin->set_x(0.); origin->set_x(0.);

4
cartographer_ros/cartographer_ros/msg_conversion.h
View File

@ -50,7 +50,7 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message(
int64 timestamp, const string& frame_id, int64 timestamp, const string& frame_id,
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d); const ::cartographer::sensor::proto::LaserFan& laser_fan);
geometry_msgs::Transform ToGeometryMsgTransform( geometry_msgs::Transform ToGeometryMsgTransform(
const ::cartographer::transform::Rigid3d& rigid3d); const ::cartographer::transform::Rigid3d& rigid3d);
@ -64,7 +64,7 @@ geometry_msgs::Pose ToGeometryMsgPose(
::cartographer::sensor::proto::LaserScan ToCartographer( ::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::MultiEchoLaserScan& msg); const sensor_msgs::MultiEchoLaserScan& msg);
::cartographer::sensor::proto::LaserFan3D ToCartographer( ::cartographer::sensor::proto::LaserFan ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points); const pcl::PointCloud<pcl::PointXYZ>& pcl_points);
::cartographer::transform::Rigid3d ToRigid3d( ::cartographer::transform::Rigid3d ToRigid3d(

7
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -511,13 +511,12 @@ void Node::HandleSensorData(const int64 timestamp,
sensor_data->imu.angular_velocity); sensor_data->imu.angular_velocity);
return; return;
case carto::sensor::Data::Type::kLaserFan3D: case carto::sensor::Data::Type::kLaserFan:
if (options_.map_builder_options.use_trajectory_builder_2d()) { if (options_.map_builder_options.use_trajectory_builder_2d()) {
trajectory_builder->AddHorizontalLaserFan(time, trajectory_builder->AddHorizontalLaserFan(time, sensor_data->laser_fan);
sensor_data->laser_fan_3d);
} else { } else {
CHECK(options_.map_builder_options.use_trajectory_builder_3d()); CHECK(options_.map_builder_options.use_trajectory_builder_3d());
trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan_3d); trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan);
} }
return; return;

5
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -41,9 +41,8 @@ void BuildOccupancyGrid(
for (const auto& node : trajectory_nodes) { for (const auto& node : trajectory_nodes) {
CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet. CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet.
laser_fan_inserter.Insert( laser_fan_inserter.Insert(
carto::sensor::TransformLaserFan( carto::sensor::TransformLaserFan(node.constant_data->laser_fan_2d,
node.constant_data->laser_fan, node.pose.cast<float>()),
carto::transform::Project2D(node.pose).cast<float>()),
&probability_grid); &probability_grid);
} }

6
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -141,7 +141,7 @@ void SensorBridge::HandlePointCloud2Message(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(msg->header.frame_id), CheckNoLeadingSlash(msg->header.frame_id),
carto::sensor::TransformLaserFan3D( carto::sensor::TransformLaserFan(
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
sensor_to_tracking->cast<float>()))); sensor_to_tracking->cast<float>())));
} }
@ -151,7 +151,7 @@ void SensorBridge::HandleLaserScanProto(
const string& topic, const ::cartographer::common::Time time, const string& topic, const ::cartographer::common::Time time,
const string& frame_id, const string& frame_id,
const ::cartographer::sensor::proto::LaserScan& laser_scan) { const ::cartographer::sensor::proto::LaserScan& laser_scan) {
const auto laser_fan = carto::sensor::ToLaserFan3D( const auto laser_fan = carto::sensor::ToLaserFan(
laser_scan, options_.horizontal_laser_min_range, laser_scan, options_.horizontal_laser_min_range,
options_.horizontal_laser_max_range, options_.horizontal_laser_max_range,
options_.horizontal_laser_missing_echo_ray_length); options_.horizontal_laser_missing_echo_ray_length);
@ -161,7 +161,7 @@ void SensorBridge::HandleLaserScanProto(
trajectory_id_, carto::common::ToUniversal(time), topic, trajectory_id_, carto::common::ToUniversal(time), topic,
carto::common::make_unique<::cartographer::sensor::Data>( carto::common::make_unique<::cartographer::sensor::Data>(
CheckNoLeadingSlash(frame_id), CheckNoLeadingSlash(frame_id),
carto::sensor::TransformLaserFan3D( carto::sensor::TransformLaserFan(
laser_fan, sensor_to_tracking->cast<float>()))); laser_fan, sensor_to_tracking->cast<float>())));
} }
} }