DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix Windows warnings

Browse Source
This commit is contained in:
Martin Idel 2018-03-14 16:47:52 +01:00
parent 48e676ff7f
commit c985f5defc
2 changed files with 69 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/laser_geometry.cpp
View File

@ -82,7 +82,7 @@ void LaserProjection::projectLaser_(
// Set the output cloud accordingly // Set the output cloud accordingly
cloud_out.header = scan_in.header; cloud_out.header = scan_in.header;
cloud_out.height = 1; cloud_out.height = 1;
cloud_out.width = scan_in.ranges.size(); cloud_out.width = static_cast<uint32_t>(scan_in.ranges.size());
cloud_out.fields.resize(3); cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x"; cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0; cloud_out.fields[0].offset = 0;
@ -102,53 +102,53 @@ void LaserProjection::projectLaser_(
idx_vpy = -1, idx_vpz = -1; idx_vpy = -1, idx_vpz = -1;
// now, we need to check what fields we need to store // now, we need to check what fields we need to store
int offset = 12; uint32_t offset = 12;
if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) { if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) {
int field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity"; cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
idx_intensity = field_size; idx_intensity = static_cast<int>(field_size);
} }
if ((channel_options & channel_option::Index)) { if ((channel_options & channel_option::Index)) {
int field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index"; cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = POINT_FIELD::INT32; cloud_out.fields[field_size].datatype = POINT_FIELD::INT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
idx_index = field_size; idx_index = static_cast<int>(field_size);
} }
if ((channel_options & channel_option::Distance)) { if ((channel_options & channel_option::Distance)) {
int field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances"; cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
idx_distance = field_size; idx_distance = static_cast<int>(field_size);
} }
if ((channel_options & channel_option::Timestamp)) { if ((channel_options & channel_option::Timestamp)) {
int field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps"; cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
idx_timestamp = field_size; idx_timestamp = static_cast<int>(field_size);
} }
if ((channel_options & channel_option::Viewpoint)) { if ((channel_options & channel_option::Viewpoint)) {
int field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 3); cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x"; cloud_out.fields[field_size].name = "vp_x";
@ -169,9 +169,9 @@ void LaserProjection::projectLaser_(
cloud_out.fields[field_size + 2].count = 1; cloud_out.fields[field_size + 2].count = 1;
offset += 4; offset += 4;
idx_vpx = field_size; idx_vpx = static_cast<int>(field_size);
idx_vpy = field_size + 1; idx_vpy = static_cast<int>(field_size + 1);
idx_vpz = field_size + 2; idx_vpz = static_cast<int>(field_size + 2);
} }
cloud_out.point_step = offset; cloud_out.point_step = offset;
@ -191,8 +191,8 @@ void LaserProjection::projectLaser_(
auto pstep = reinterpret_cast<float *>(&cloud_out.data[count * cloud_out.point_step]); auto pstep = reinterpret_cast<float *>(&cloud_out.data[count * cloud_out.point_step]);
// Copy XYZ // Copy XYZ
pstep[0] = output(i, 0); pstep[0] = static_cast<float>(output(i, 0));
pstep[1] = output(i, 1); pstep[1] = static_cast<float>(output(i, 1));
pstep[2] = 0; pstep[2] = 0;
// Copy intensity // Copy intensity
@ -202,7 +202,7 @@ void LaserProjection::projectLaser_(
// Copy index // Copy index
if (idx_index != -1) { if (idx_index != -1) {
reinterpret_cast<int *>(pstep)[idx_index] = i; reinterpret_cast<int *>(pstep)[idx_index] = static_cast<int>(i);
} }
// Copy distance // Copy distance
@ -345,9 +345,9 @@ void LaserProjection::transformLaserScanToPointCloud_(
tf2::Vector3 point_out = cur_transform * point_in; tf2::Vector3 point_out = cur_transform * point_in;
// Copy transformed point into cloud // Copy transformed point into cloud
pstep[0] = point_out.x(); pstep[0] = static_cast<float>(point_out.x());
pstep[1] = point_out.y(); pstep[1] = static_cast<float>(point_out.y());
pstep[2] = point_out.z(); pstep[2] = static_cast<float>(point_out.z());
// Convert the viewpoint as well // Convert the viewpoint as well
if (has_viewpoint) { if (has_viewpoint) {
@ -357,9 +357,9 @@ void LaserProjection::transformLaserScanToPointCloud_(
point_out = cur_transform * point_in; point_out = cur_transform * point_in;
// Copy transformed point into cloud // Copy transformed point into cloud
vpstep[0] = point_out.x(); vpstep[0] = static_cast<float>(point_out.x());
vpstep[1] = point_out.y(); vpstep[1] = static_cast<float>(point_out.y());
vpstep[2] = point_out.z(); vpstep[2] = static_cast<float>(point_out.z());
} }
} }
@ -426,7 +426,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
TIME end_time = scan_in.header.stamp; TIME end_time = scan_in.header.stamp;
// TODO(anonymous): reconcile all the different time constructs // TODO(anonymous): reconcile all the different time constructs
if (!scan_in.ranges.empty()) { if (!scan_in.ranges.empty()) {
end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); end_time = end_time + rclcpp::Duration(
static_cast<int>((scan_in.ranges.size() - 1) * scan_in.time_increment), 0);
} }
std::chrono::nanoseconds start(start_time.nanoseconds()); std::chrono::nanoseconds start(start_time.nanoseconds());

84
test/projection_test.cpp
View File

@ -27,8 +27,9 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#define _USE_MATH_DEFINES
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <math.h> #include <cmath>
#include <vector> #include <vector>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
@ -36,9 +37,10 @@
#include "laser_geometry/laser_geometry.hpp" #include "laser_geometry/laser_geometry.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_cloud2.hpp"
#define PROJECTION_TEST_RANGE_MIN (0.23) #define PROJECTION_TEST_RANGE_MIN (0.23f)
#define PROJECTION_TEST_RANGE_MAX (40.0) #define PROJECTION_TEST_RANGE_MAX (40.0f)
#define PI static_cast<float>(M_PI)
class BuildScanException class BuildScanException
{ {
@ -46,16 +48,16 @@ class BuildScanException
struct ScanOptions struct ScanOptions
{ {
double range_; float range_;
double intensity_; float intensity_;
double ang_min_; float ang_min_;
double ang_max_; float ang_max_;
double ang_increment_; float ang_increment_;
rclcpp::Duration scan_time_; rclcpp::Duration scan_time_;
ScanOptions( ScanOptions(
double range, double intensity, float range, float intensity,
double ang_min, double ang_max, double ang_increment, float ang_min, float ang_max, float ang_increment,
rclcpp::Duration scan_time) rclcpp::Duration scan_time)
: range_(range), : range_(range),
intensity_(intensity), intensity_(intensity),
@ -77,7 +79,7 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.angle_min = options.ang_min_; scan.angle_min = options.ang_min_;
scan.angle_max = options.ang_max_; scan.angle_max = options.ang_max_;
scan.angle_increment = options.ang_increment_; scan.angle_increment = options.ang_increment_;
scan.scan_time = options.scan_time_.nanoseconds(); scan.scan_time = static_cast<float>(options.scan_time_.nanoseconds());
scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX; scan.range_max = PROJECTION_TEST_RANGE_MAX;
uint32_t i = 0; uint32_t i = 0;
@ -86,7 +88,8 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.intensities.push_back(options.intensity_); scan.intensities.push_back(options.intensity_);
} }
scan.time_increment = options.scan_time_.nanoseconds() / static_cast<double>(i); scan.time_increment =
static_cast<float>(options.scan_time_.nanoseconds() / static_cast<double>(i));
return scan; return scan;
} }
@ -101,39 +104,39 @@ TEST(laser_geometry, projectLaser2) {
double tolerance = 1e-12; double tolerance = 1e-12;
laser_geometry::LaserProjection projector; laser_geometry::LaserProjection projector;
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments; std::vector<float> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times; std::vector<rclcpp::Duration> increment_times, scan_times;
ranges.push_back(-1.0); ranges.push_back(-1.0f);
ranges.push_back(1.0); ranges.push_back(1.0f);
ranges.push_back(2.0); ranges.push_back(2.0f);
ranges.push_back(3.0); ranges.push_back(3.0f);
ranges.push_back(4.0); ranges.push_back(4.0f);
ranges.push_back(5.0); ranges.push_back(5.0f);
ranges.push_back(100.0); ranges.push_back(100.0f);
intensities.push_back(1.0); intensities.push_back(1.0f);
intensities.push_back(2.0); intensities.push_back(2.0f);
intensities.push_back(3.0); intensities.push_back(3.0f);
intensities.push_back(4.0); intensities.push_back(4.0f);
intensities.push_back(5.0); intensities.push_back(5.0f);
min_angles.push_back(-M_PI); min_angles.push_back(-PI);
min_angles.push_back(-M_PI / 1.5); min_angles.push_back(-PI / 1.5f);
min_angles.push_back(-M_PI / 2); min_angles.push_back(-PI / 2);
min_angles.push_back(-M_PI / 4); min_angles.push_back(-PI / 4);
min_angles.push_back(-M_PI / 8); min_angles.push_back(-PI / 8);
max_angles.push_back(M_PI); max_angles.push_back(PI);
max_angles.push_back(M_PI / 1.5); max_angles.push_back(PI / 1.5f);
max_angles.push_back(M_PI / 2); max_angles.push_back(PI / 2);
max_angles.push_back(M_PI / 4); max_angles.push_back(PI / 4);
max_angles.push_back(M_PI / 8); max_angles.push_back(PI / 8);
angle_increments.push_back(-M_PI / 180); // -one degree angle_increments.push_back(-PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree angle_increments.push_back(PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree angle_increments.push_back(PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree angle_increments.push_back(PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration(1 / 40)); scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20)); scan_times.push_back(rclcpp::Duration(1 / 20));
@ -234,7 +237,8 @@ TEST(laser_geometry, projectLaser2) {
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset), EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset),
(float)i * scan.time_increment, tolerance); // timestamps (float)i * scan.time_increment, tolerance); // timestamps
} }
} catch (BuildScanException & ex) { } catch (const BuildScanException & ex) {
(void) ex;
// make sure it is not a false exception // make sure it is not a false exception
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL(); FAIL();