From 6b147dd44579bdd423df39be4c8970c2a7bdc744 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 3 May 2017 15:20:21 +0200 Subject: [PATCH] Remove dependency on proto::LaserScan. (#323) Also change the CMakeLists.txt to run all tests found by the glob. --- cartographer_ros/CMakeLists.txt | 37 ++-- .../cartographer_ros/assets_writer.cc | 15 +- .../cartographer_ros/assets_writer_main.cc | 36 +--- .../cartographer_ros/msg_conversion.cc | 197 ++++++------------ .../cartographer_ros/msg_conversion.h | 21 +- .../cartographer_ros/msg_conversion_test.cc | 74 +++++++ .../cartographer_ros/sensor_bridge.cc | 4 +- .../cartographer_ros/time_conversion_test.cc | 1 - 8 files changed, 177 insertions(+), 208 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/msg_conversion_test.cc diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 9daab4a..5cfdf87 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -132,29 +132,20 @@ set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(configuration_files_test "cartographer_ros/configuration_files_test.cc") - # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to - # target_link_libraries. That forces us to do the same. - target_include_directories(configuration_files_test SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - target_link_libraries(configuration_files_test ${LUA_LIBRARIES}) - target_include_directories(configuration_files_test SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(configuration_files_test ${catkin_LIBRARIES}) - add_dependencies(configuration_files_test ${catkin_EXPORTED_TARGETS}) - target_link_libraries(configuration_files_test cartographer) - target_link_libraries(configuration_files_test ${PROJECT_NAME}) - set_target_properties(configuration_files_test PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) - - catkin_add_gtest(time_conversion_test "cartographer_ros/time_conversion_test.cc") - # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to - # target_link_libraries. That forces us to do the same. - target_include_directories(time_conversion_test SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) - target_link_libraries(time_conversion_test ${LUA_LIBRARIES}) - target_include_directories(time_conversion_test SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) - target_link_libraries(time_conversion_test ${catkin_LIBRARIES}) - add_dependencies(time_conversion_test ${catkin_EXPORTED_TARGETS}) - target_link_libraries(time_conversion_test cartographer) - target_link_libraries(time_conversion_test ${PROJECT_NAME}) - set_target_properties(time_conversion_test PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + foreach(TEST_SOURCE_FILENAME ${ALL_TESTS}) + get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE) + catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME}) + # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to + # target_link_libraries. That forces us to do the same. + target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR}) + target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES}) + target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) + target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES}) + add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${TEST_NAME} cartographer) + target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) + set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + endforeach() endif() install(DIRECTORY launch urdf configuration_files diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 9ff3939..a92e5e4 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -55,16 +55,19 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), {}, stem + "_xray_xy", file_writer_factory, &null_points_processor); carto::io::XRayPointsProcessor yz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor); carto::io::XRayPointsProcessor xz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor); carto::io::PlyWritingPointsProcessor ply_writing_points_processor( file_writer_factory(stem + ".ply"), &xz_xray_points_processor); diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 0b16db5..fb0385f 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -66,30 +66,6 @@ namespace { namespace carto = ::cartographer; -carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::PointCloud2::ConstPtr& message) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*message, pcl_point_cloud); - carto::sensor::PointCloudWithIntensities point_cloud; - - // TODO(hrapp): How to get reflectivities from PCL? - for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z); - point_cloud.intensities.push_back(1.); - } - return point_cloud; -} - -carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::MultiEchoLaserScan::ConstPtr& message) { - return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message)); -} - -carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::LaserScan::ConstPtr& message) { - return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message)); -} - template void HandleMessage( const T& message, const string& tracking_frame, @@ -97,7 +73,7 @@ void HandleMessage( const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer, const std::vector>& pipeline) { - const carto::common::Time time = FromRos(message->header.stamp); + const carto::common::Time time = FromRos(message.header.stamp); if (!transform_interpolation_buffer.Has(time)) { return; } @@ -106,14 +82,14 @@ void HandleMessage( transform_interpolation_buffer.Lookup(time); const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d(tf_buffer.lookupTransform( - tracking_frame, message->header.frame_id, message->header.stamp)); + tracking_frame, message.header.frame_id, message.header.stamp)); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); auto batch = carto::common::make_unique(); batch->time = time; batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); - batch->frame_id = message->header.frame_id; + batch->frame_id = message.header.frame_id; carto::sensor::PointCloudWithIntensities point_cloud = ToPointCloudWithIntensities(message); @@ -179,17 +155,17 @@ void Run(const string& trajectory_filename, const string& bag_filename, for (const rosbag::MessageInstance& message : view) { if (message.isType()) { - HandleMessage(message.instantiate(), + HandleMessage(*message.instantiate(), tracking_frame, *tf_buffer, *transform_interpolation_buffer, pipeline); } if (message.isType()) { - HandleMessage(message.instantiate(), + HandleMessage(*message.instantiate(), tracking_frame, *tf_buffer, *transform_interpolation_buffer, pipeline); } if (message.isType()) { - HandleMessage(message.instantiate(), + HandleMessage(*message.instantiate(), tracking_frame, *tf_buffer, *transform_interpolation_buffer, pipeline); } diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index fef34fa..19c0615 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -18,7 +18,6 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" -#include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/time_conversion.h" @@ -46,6 +45,7 @@ constexpr float kPointCloudComponentFourMagic = 1.; using ::cartographer::transform::Rigid3d; using ::cartographer::kalman_filter::PoseCovariance; +using ::cartographer::sensor::PointCloudWithIntensities; sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp, const string& frame_id, @@ -76,76 +76,58 @@ sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp, return msg; } +// For sensor_msgs::LaserScan. +bool HasEcho(float) { return true; } + +float GetFirstEcho(float range) { return range; } + +// For sensor_msgs::MultiEchoLaserScan. +bool HasEcho(const sensor_msgs::LaserEcho& echo) { + return !echo.echoes.empty(); +} + +float GetFirstEcho(const sensor_msgs::LaserEcho& echo) { + return echo.echoes[0]; +} + +// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan. +template +PointCloudWithIntensities LaserScanToPointCloudWithIntensities( + const LaserMessageType& msg) { + CHECK_GE(msg.range_min, 0.f); + CHECK_GE(msg.range_max, msg.range_min); + if (msg.angle_increment > 0.f) { + CHECK_GT(msg.angle_max, msg.angle_min); + } else { + CHECK_GT(msg.angle_min, msg.angle_max); + } + PointCloudWithIntensities point_cloud; + float angle = msg.angle_min; + for (size_t i = 0; i < msg.ranges.size(); ++i) { + const auto& echoes = msg.ranges[i]; + if (HasEcho(echoes)) { + const float first_echo = GetFirstEcho(echoes); + if (msg.range_min <= first_echo && first_echo <= msg.range_max) { + const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); + point_cloud.points.push_back(rotation * + (first_echo * Eigen::Vector3f::UnitX())); + if (msg.intensities.size() > 0) { + CHECK_EQ(msg.intensities.size(), msg.ranges.size()); + const auto& echo_intensities = msg.intensities[i]; + CHECK(HasEcho(echo_intensities)); + point_cloud.intensities.push_back(GetFirstEcho(echo_intensities)); + } else { + point_cloud.intensities.push_back(0.f); + } + } + } + angle += msg.angle_increment; + } + return point_cloud; +} + } // namespace -sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage( - const int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::LaserScan& laser_scan) { - sensor_msgs::MultiEchoLaserScan msg; - msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); - msg.header.frame_id = frame_id; - - msg.angle_min = laser_scan.angle_min(); - msg.angle_max = laser_scan.angle_max(); - msg.angle_increment = laser_scan.angle_increment(); - msg.time_increment = laser_scan.time_increment(); - msg.scan_time = laser_scan.scan_time(); - msg.range_min = laser_scan.range_min(); - msg.range_max = laser_scan.range_max(); - - for (const auto& echoes : laser_scan.range()) { - msg.ranges.emplace_back(); - std::copy(echoes.value().begin(), echoes.value().end(), - std::back_inserter(msg.ranges.back().echoes)); - } - - for (const auto& echoes : laser_scan.intensity()) { - msg.intensities.emplace_back(); - std::copy(echoes.value().begin(), echoes.value().end(), - std::back_inserter(msg.intensities.back().echoes)); - } - return msg; -} - -sensor_msgs::LaserScan ToLaserScan( - const int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::LaserScan& laser_scan) { - sensor_msgs::LaserScan msg; - msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); - msg.header.frame_id = frame_id; - - msg.angle_min = laser_scan.angle_min(); - msg.angle_max = laser_scan.angle_max(); - msg.angle_increment = laser_scan.angle_increment(); - msg.time_increment = laser_scan.time_increment(); - msg.scan_time = laser_scan.scan_time(); - msg.range_min = laser_scan.range_min(); - msg.range_max = laser_scan.range_max(); - - for (const auto& echoes : laser_scan.range()) { - if (echoes.value_size() > 0) { - msg.ranges.push_back(echoes.value(0)); - } else { - msg.ranges.push_back(0.); - } - } - - bool has_intensities = false; - for (const auto& echoes : laser_scan.intensity()) { - if (echoes.value_size() > 0) { - msg.intensities.push_back(echoes.value(0)); - has_intensities = true; - } else { - msg.intensities.push_back(0); - } - } - if (!has_intensities) { - msg.intensities.clear(); - } - - return msg; -} - sensor_msgs::PointCloud2 ToPointCloud2Message( const int64 timestamp, const string& frame_id, const ::cartographer::sensor::PointCloud& point_cloud) { @@ -160,73 +142,28 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( return msg; } -sensor_msgs::PointCloud2 ToPointCloud2Message( - const int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::RangeData& range_data) { - CHECK(::cartographer::transform::ToEigen(range_data.origin()).norm() == 0) - << "Trying to convert a range_data that is not at the origin."; - - const auto& point_cloud = range_data.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(); - - auto msg = PreparePointCloud2Message(timestamp, frame_id, num_points); - ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); - for (int i = 0; i < num_points; ++i) { - stream.next(point_cloud.x(i)); - stream.next(point_cloud.y(i)); - stream.next(point_cloud.z(i)); - stream.next(kPointCloudComponentFourMagic); - } - return msg; -} - -::cartographer::sensor::proto::LaserScan ToCartographer( +PointCloudWithIntensities ToPointCloudWithIntensities( const sensor_msgs::LaserScan& msg) { - ::cartographer::sensor::proto::LaserScan proto; - proto.set_angle_min(msg.angle_min); - proto.set_angle_max(msg.angle_max); - proto.set_angle_increment(msg.angle_increment); - proto.set_time_increment(msg.time_increment); - proto.set_scan_time(msg.scan_time); - proto.set_range_min(msg.range_min); - proto.set_range_max(msg.range_max); - - for (const auto& range : msg.ranges) { - proto.add_range()->mutable_value()->Add(range); - } - - for (const auto& intensity : msg.intensities) { - proto.add_intensity()->mutable_value()->Add(intensity); - } - return proto; + return LaserScanToPointCloudWithIntensities(msg); } -::cartographer::sensor::proto::LaserScan ToCartographer( +PointCloudWithIntensities ToPointCloudWithIntensities( const sensor_msgs::MultiEchoLaserScan& msg) { - ::cartographer::sensor::proto::LaserScan proto; - proto.set_angle_min(msg.angle_min); - proto.set_angle_max(msg.angle_max); - proto.set_angle_increment(msg.angle_increment); - proto.set_time_increment(msg.time_increment); - proto.set_scan_time(msg.scan_time); - proto.set_range_min(msg.range_min); - proto.set_range_max(msg.range_max); + return LaserScanToPointCloudWithIntensities(msg); +} - for (const auto& range : msg.ranges) { - auto* proto_echoes = proto.add_range()->mutable_value(); - for (const auto& echo : range.echoes) { - proto_echoes->Add(echo); - } +PointCloudWithIntensities ToPointCloudWithIntensities( + const sensor_msgs::PointCloud2& message) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(message, pcl_point_cloud); + PointCloudWithIntensities point_cloud; + + // TODO(hrapp): How to get reflectivities from PCL? + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z); + point_cloud.intensities.push_back(1.); } - for (const auto& intensity : msg.intensities) { - auto* proto_echoes = proto.add_intensity()->mutable_value(); - for (const auto& echo : intensity.echoes) { - proto_echoes->Add(echo); - } - } - return proto; + return point_cloud; } Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index 91e564a..c7dae2c 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -20,7 +20,6 @@ #include "cartographer/common/port.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/sensor/point_cloud.h" -#include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/rigid_transform.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Transform.h" @@ -35,35 +34,25 @@ namespace cartographer_ros { -// Returns a laser scan consisting of the first echo of each beam. -sensor_msgs::LaserScan ToLaserScan( - int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::LaserScan& laser_scan); - -sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage( - int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::LaserScan& laser_scan); - sensor_msgs::PointCloud2 ToPointCloud2Message( int64 timestamp, const string& frame_id, const ::cartographer::sensor::PointCloud& point_cloud); -sensor_msgs::PointCloud2 ToPointCloud2Message( - int64 timestamp, const string& frame_id, - const ::cartographer::sensor::proto::RangeData& range_data); - geometry_msgs::Transform ToGeometryMsgTransform( const ::cartographer::transform::Rigid3d& rigid3d); geometry_msgs::Pose ToGeometryMsgPose( const ::cartographer::transform::Rigid3d& rigid3d); -::cartographer::sensor::proto::LaserScan ToCartographer( +::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( const sensor_msgs::LaserScan& msg); -::cartographer::sensor::proto::LaserScan ToCartographer( +::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( const sensor_msgs::MultiEchoLaserScan& msg); +::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( + const sensor_msgs::PointCloud2& message); + ::cartographer::transform::Rigid3d ToRigid3d( const geometry_msgs::TransformStamped& transform); diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc new file mode 100644 index 0000000..92b63a0 --- /dev/null +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -0,0 +1,74 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/msg_conversion.h" + +#include + +#include "gtest/gtest.h" +#include "sensor_msgs/LaserScan.h" + +namespace cartographer_ros { +namespace { + +TEST(MsgConversion, LaserScanToPointCloud) { + sensor_msgs::LaserScan laser_scan; + for (int i = 0; i < 8; ++i) { + laser_scan.ranges.push_back(1.f); + } + laser_scan.angle_min = 0.f; + laser_scan.angle_max = 8.f * static_cast(M_PI_4); + laser_scan.angle_increment = static_cast(M_PI_4); + laser_scan.range_min = 0.f; + laser_scan.range_max = 10.f; + + const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; + EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[1].isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[3].isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[5].isApprox( + Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), + 1e-6)); + EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[7].isApprox( + Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6)); +} + +TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { + sensor_msgs::LaserScan laser_scan; + laser_scan.ranges.push_back(1.f); + laser_scan.ranges.push_back(std::numeric_limits::infinity()); + laser_scan.ranges.push_back(2.f); + laser_scan.ranges.push_back(std::numeric_limits::quiet_NaN()); + laser_scan.ranges.push_back(3.f); + laser_scan.angle_min = 0.f; + laser_scan.angle_max = 3.f * static_cast(M_PI_4); + laser_scan.angle_increment = static_cast(M_PI_4); + laser_scan.range_min = 2.f; + laser_scan.range_max = 10.f; + + const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; + ASSERT_EQ(2, point_cloud.size()); + EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); +} + +} // namespace +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 5feef1f..5cd5b97 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -87,14 +87,14 @@ void SensorBridge::HandleImuMessage(const string& sensor_id, void SensorBridge::HandleLaserScanMessage( const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - carto::sensor::ToPointCloud(ToCartographer(*msg))); + ToPointCloudWithIntensities(*msg).points); } void SensorBridge::HandleMultiEchoLaserScanMessage( const string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - carto::sensor::ToPointCloud(ToCartographer(*msg))); + ToPointCloudWithIntensities(*msg).points); } void SensorBridge::HandlePointCloud2Message( diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/cartographer_ros/time_conversion_test.cc index 4443ca4..5f1b7bf 100644 --- a/cartographer_ros/cartographer_ros/time_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/time_conversion_test.cc @@ -23,7 +23,6 @@ #include "ros/ros.h" namespace cartographer_ros { - namespace { TEST(TimeConversion, testToRos) {