Remove dependency on proto::LaserScan. (#323)
Also change the CMakeLists.txt to run all tests found by the glob.master
Wolfgang Hess
GitHub
parent
1514f71b30
commit
6b147dd445
|
|
@ -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")
|
||||
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(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})
|
||||
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
|
||||
|
|
|
|||
|
|
@ -55,15 +55,18 @@ 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(
|
||||
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(
|
||||
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(
|
||||
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(
|
||||
|
|
|
|||
|
|
@ -66,30 +66,6 @@ namespace {
|
|||
|
||||
namespace carto = ::cartographer;
|
||||
|
||||
carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
|
||||
const sensor_msgs::PointCloud2::ConstPtr& message) {
|
||||
pcl::PointCloud<pcl::PointXYZ> 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 <typename T>
|
||||
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<std::unique_ptr<carto::io::PointsProcessor>>& 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<float>();
|
||||
|
||||
auto batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||
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<sensor_msgs::PointCloud2>()) {
|
||||
HandleMessage(message.instantiate<sensor_msgs::PointCloud2>(),
|
||||
HandleMessage(*message.instantiate<sensor_msgs::PointCloud2>(),
|
||||
tracking_frame, *tf_buffer,
|
||||
*transform_interpolation_buffer, pipeline);
|
||||
}
|
||||
if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
|
||||
HandleMessage(message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
|
||||
HandleMessage(*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
|
||||
tracking_frame, *tf_buffer,
|
||||
*transform_interpolation_buffer, pipeline);
|
||||
}
|
||||
if (message.isType<sensor_msgs::LaserScan>()) {
|
||||
HandleMessage(message.instantiate<sensor_msgs::LaserScan>(),
|
||||
HandleMessage(*message.instantiate<sensor_msgs::LaserScan>(),
|
||||
tracking_frame, *tf_buffer,
|
||||
*transform_interpolation_buffer, pipeline);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <typename LaserMessageType>
|
||||
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::PointXYZ> 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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <cmath>
|
||||
|
||||
#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<float>(M_PI_4);
|
||||
laser_scan.angle_increment = static_cast<float>(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<float>::infinity());
|
||||
laser_scan.ranges.push_back(2.f);
|
||||
laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
|
||||
laser_scan.ranges.push_back(3.f);
|
||||
laser_scan.angle_min = 0.f;
|
||||
laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
|
||||
laser_scan.angle_increment = static_cast<float>(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
|
||||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -23,7 +23,6 @@
|
|||
#include "ros/ros.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
namespace {
|
||||
|
||||
TEST(TimeConversion, testToRos) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue