diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 2c3f216..6ab6a6c 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -162,11 +162,11 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message( const int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) { - CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0) - << "Trying to convert a laser_scan_3d that is not at the origin."; + const ::cartographer::sensor::proto::LaserFan& laser_fan) { + CHECK(::cartographer::transform::ToEigen(laser_fan.origin()).norm() == 0) + << "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.z_size()); const auto num_points = point_cloud.x_size(); @@ -229,9 +229,9 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( return proto; } -::cartographer::sensor::proto::LaserFan3D ToCartographer( +::cartographer::sensor::proto::LaserFan ToCartographer( const pcl::PointCloud& pcl_points) { - ::cartographer::sensor::proto::LaserFan3D proto; + ::cartographer::sensor::proto::LaserFan proto; auto* origin = proto.mutable_origin(); origin->set_x(0.); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index 2d3d329..b3d9770 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -50,7 +50,7 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message( 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( const ::cartographer::transform::Rigid3d& rigid3d); @@ -64,7 +64,7 @@ geometry_msgs::Pose ToGeometryMsgPose( ::cartographer::sensor::proto::LaserScan ToCartographer( const sensor_msgs::MultiEchoLaserScan& msg); -::cartographer::sensor::proto::LaserFan3D ToCartographer( +::cartographer::sensor::proto::LaserFan ToCartographer( const pcl::PointCloud& pcl_points); ::cartographer::transform::Rigid3d ToRigid3d( diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 3966297..6cbf3b8 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -511,13 +511,12 @@ void Node::HandleSensorData(const int64 timestamp, sensor_data->imu.angular_velocity); return; - case carto::sensor::Data::Type::kLaserFan3D: + case carto::sensor::Data::Type::kLaserFan: if (options_.map_builder_options.use_trajectory_builder_2d()) { - trajectory_builder->AddHorizontalLaserFan(time, - sensor_data->laser_fan_3d); + trajectory_builder->AddHorizontalLaserFan(time, sensor_data->laser_fan); } else { 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; diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index 68041c9..1a82502 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -41,9 +41,8 @@ void BuildOccupancyGrid( for (const auto& node : trajectory_nodes) { CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet. laser_fan_inserter.Insert( - carto::sensor::TransformLaserFan( - node.constant_data->laser_fan, - carto::transform::Project2D(node.pose).cast()), + carto::sensor::TransformLaserFan(node.constant_data->laser_fan_2d, + node.pose.cast()), &probability_grid); } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 0f764ad..f36873a 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -141,7 +141,7 @@ void SensorBridge::HandlePointCloud2Message( trajectory_id_, carto::common::ToUniversal(time), topic, carto::common::make_unique<::cartographer::sensor::Data>( CheckNoLeadingSlash(msg->header.frame_id), - carto::sensor::TransformLaserFan3D( + carto::sensor::TransformLaserFan( carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), sensor_to_tracking->cast()))); } @@ -151,7 +151,7 @@ void SensorBridge::HandleLaserScanProto( const string& topic, const ::cartographer::common::Time time, const string& frame_id, 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, options_.horizontal_laser_max_range, options_.horizontal_laser_missing_echo_ray_length); @@ -161,7 +161,7 @@ void SensorBridge::HandleLaserScanProto( trajectory_id_, carto::common::ToUniversal(time), topic, carto::common::make_unique<::cartographer::sensor::Data>( CheckNoLeadingSlash(frame_id), - carto::sensor::TransformLaserFan3D( + carto::sensor::TransformLaserFan( laser_fan, sensor_to_tracking->cast()))); } }