From 7e6f150285257050280febb9bb54374d800f33b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 31 Jul 2018 18:12:45 +0200 Subject: [PATCH 1/9] Follow cartographer#1357 --- .../cartographer_ros/assets_writer.cc | 6 +-- .../cartographer_ros/msg_conversion.cc | 43 +++++++++++-------- cartographer_ros/cartographer_ros/node.cc | 7 ++- .../cartographer_ros/rosbag_validate_main.cc | 11 ++--- .../cartographer_ros/sensor_bridge.cc | 12 +++--- 5 files changed, 42 insertions(+), 37 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index f9b68edce..6ebd7d78b 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -110,7 +110,8 @@ std::unique_ptr HandleMessage( for (size_t i = 0; i < point_cloud.points.size(); ++i) { const carto::common::Time time = - point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); + point_cloud_time + + carto::common::FromSeconds(point_cloud.points[i].time()); if (!transform_interpolation_buffer.Has(time)) { continue; } @@ -121,8 +122,7 @@ std::unique_ptr HandleMessage( tracking_frame, message.header.frame_id, ToRos(time))); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); - points_batch->points.push_back(sensor_to_map * - point_cloud.points[i].head<3>()); + points_batch->points.push_back(sensor_to_map * point_cloud.points[i]); points_batch->intensities.push_back(point_cloud.intensities[i]); // We use the last transform for the origin, which is approximately correct. points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 2c996ca20..898aa8c34 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -147,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { 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()); - Eigen::Vector4f point; - point << rotation * (first_echo * Eigen::Vector3f::UnitX()), - i * msg.time_increment; + const cartographer::sensor::TimedRangefinderPoint point( + rotation * (first_echo * Eigen::Vector3f::UnitX()), + i * msg.time_increment); point_cloud.points.push_back(point); if (msg.intensities.size() > 0) { CHECK_EQ(msg.intensities.size(), msg.ranges.size()); @@ -165,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { } ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back()[3]; + const double duration = point_cloud.points.back().time(); timestamp += cartographer::common::FromSeconds(duration); - for (Eigen::Vector4f& point : point_cloud.points) { - point[3] -= duration; + for (auto& point : point_cloud.points) { + point.set_time(point.time() - duration); } } return std::make_tuple(point_cloud, timestamp); @@ -191,10 +191,10 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( const ::cartographer::sensor::TimedPointCloud& point_cloud) { auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); - for (const Eigen::Vector4f& point : point_cloud) { - stream.next(point.x()); - stream.next(point.y()); - stream.next(point.z()); + for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) { + stream.next(point.position().x()); + stream.next(point.position().y()); + stream.next(point.position().z()); stream.next(kPointCloudComponentFourMagic); } return msg; @@ -225,7 +225,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, point.time); + point_cloud.points.emplace_back( + Eigen::Vector3f{point.x, point.y, point.z}, point.time); point_cloud.intensities.push_back(point.intensity); } } else { @@ -234,7 +235,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); + point_cloud.points.emplace_back( + Eigen::Vector3f{point.x, point.y, point.z}, 0.f); point_cloud.intensities.push_back(point.intensity); } } @@ -246,7 +248,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, point.time); + point_cloud.points.emplace_back( + Eigen::Vector3f{point.x, point.y, point.z}, point.time); point_cloud.intensities.push_back(1.0f); } } else { @@ -255,19 +258,21 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); + point_cloud.points.emplace_back( + Eigen::Vector3f{point.x, point.y, point.z}, 0.f); point_cloud.intensities.push_back(1.0f); } } } ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back()[3]; + const double duration = point_cloud.points.back().time(); timestamp += cartographer::common::FromSeconds(duration); - for (Eigen::Vector4f& point : point_cloud.points) { - point[3] -= duration; - CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than " - "the last point in the cloud."; + for (auto& point : point_cloud.points) { + point.set_time(point.time() - duration); + CHECK_LE(point.time(), 0) + << "Encountered a point with a larger stamp than " + "the last point in the cloud."; } } return std::make_tuple(point_cloud, timestamp); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 720934b3d..de309f52b 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -208,11 +208,10 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { carto::sensor::TimedPointCloud point_cloud; point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local .returns.size()); - for (const Eigen::Vector3f point : + for (const cartographer::sensor::RangefinderPoint point : trajectory_data.local_slam_data->range_data_in_local.returns) { - Eigen::Vector4f point_time; - point_time << point, 0.f; - point_cloud.push_back(point_time); + point_cloud.push_back(cartographer::sensor::TimedRangefinderPoint( + point, 0.f /* time */)); } scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( carto::common::ToUniversal(trajectory_data.local_slam_data->time), diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index 40bd754d3..0b5f18d9c 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -219,14 +219,15 @@ class RangeDataChecker { const cartographer::sensor::TimedPointCloud& point_cloud = std::get<0>(point_cloud_time).points; *to = std::get<1>(point_cloud_time); - *from = *to + cartographer::common::FromSeconds(point_cloud[0][3]); + *from = *to + cartographer::common::FromSeconds(point_cloud[0].time()); Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); - for (const Eigen::Vector4f& point : point_cloud) { - points_sum += point; + for (const auto& point : point_cloud) { + points_sum.head<3>() += point.position(); + points_sum[3] += point.time(); } if (point_cloud.size() > 0) { - double first_point_relative_time = point_cloud[0][3]; - double last_point_relative_time = (*point_cloud.rbegin())[3]; + double first_point_relative_time = point_cloud[0].time(); + double last_point_relative_time = (*point_cloud.rbegin()).time(); if (first_point_relative_time > 0) { LOG_FIRST_N(ERROR, 1) << "Sensor with frame_id \"" << message.header.frame_id diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 1bd382299..ea1cf7ee6 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -177,7 +177,7 @@ void SensorBridge::HandleLaserScan( if (points.points.empty()) { return; } - CHECK_LE(points.points.back()[3], 0); + CHECK_LE(points.points.back().time(), 0); // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = @@ -189,7 +189,7 @@ void SensorBridge::HandleLaserScan( if (start_index == end_index) { continue; } - const double time_to_subdivision_end = subdivision.back()[3]; + const double time_to_subdivision_end = subdivision.back().time(); // `subdivision_time` is the end of the measurement so sensor::Collator will // send all other sensor data first. const carto::common::Time subdivision_time = @@ -204,10 +204,10 @@ void SensorBridge::HandleLaserScan( continue; } sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time; - for (Eigen::Vector4f& point : subdivision) { - point[3] -= time_to_subdivision_end; + for (auto& point : subdivision) { + point.set_time(point.time() - time_to_subdivision_end); } - CHECK_EQ(subdivision.back()[3], 0); + CHECK_EQ(subdivision.back().time(), 0); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } } @@ -216,7 +216,7 @@ void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { if (!ranges.empty()) { - CHECK_LE(ranges.back()[3], 0); + CHECK_LE(ranges.back().time(), 0); } const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); From 1927273052a1626d8eafe23fd093e572d026aea9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 2 Aug 2018 23:41:56 +0200 Subject: [PATCH 2/9] update msg_conversion test --- .../cartographer_ros/msg_conversion_test.cc | 42 ++++++++++--------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 0ba87ae02..baf9bf238 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -50,25 +50,25 @@ TEST(MsgConversion, LaserScanToPointCloud) { const auto point_cloud = std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; EXPECT_TRUE( - point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[1].isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); - EXPECT_TRUE( - point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[3].isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); - EXPECT_TRUE( - point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[5].isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), - kEps)); + point_cloud[0].position().isApprox(Eigen::Vector4f(1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[1].position().isApprox( + Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[7].isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f), + point_cloud[2].position().isApprox(Eigen::Vector4f(0.f, 1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[3].position().isApprox( + Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); + EXPECT_TRUE(point_cloud[4].position().isApprox( + Eigen::Vector4f(-1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[5].position().isApprox( + Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); + EXPECT_TRUE(point_cloud[6].position().isApprox( + Eigen::Vector4f(0.f, -1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[7].position().isApprox( + Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); + for (int i = 0; i < 8; ++i) { + EXPECT_NEAR(point_cloud[i].time(), kEps); + } } TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { @@ -88,9 +88,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; ASSERT_EQ(2, point_cloud.size()); EXPECT_TRUE( - point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), kEps)); - EXPECT_TRUE( - point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps)); + point_cloud[0].position().isApprox(Eigen::Vector4f(0.f, 2.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[1].position().isApprox( + Eigen::Vector4f(-3.f, 0.f, 0.f), kEps)); + EXPECT_NEAR(point_cloud[0].time(), kEps); + EXPECT_NEAR(point_cloud[1].time(), kEps); } ::testing::Matcher EqualsLandmark( From a16c902e294e02b5816f6022e27d894964302372 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 3 Aug 2018 00:18:40 +0200 Subject: [PATCH 3/9] fix test once more --- cartographer_ros/cartographer_ros/msg_conversion_test.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index baf9bf238..8456fcb8c 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -67,7 +67,7 @@ TEST(MsgConversion, LaserScanToPointCloud) { EXPECT_TRUE(point_cloud[7].position().isApprox( Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); for (int i = 0; i < 8; ++i) { - EXPECT_NEAR(point_cloud[i].time(), kEps); + EXPECT_NEAR(point_cloud[i].time(), 0.f, kEps); } } @@ -91,8 +91,8 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { point_cloud[0].position().isApprox(Eigen::Vector4f(0.f, 2.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[1].position().isApprox( Eigen::Vector4f(-3.f, 0.f, 0.f), kEps)); - EXPECT_NEAR(point_cloud[0].time(), kEps); - EXPECT_NEAR(point_cloud[1].time(), kEps); + EXPECT_NEAR(point_cloud[0].time(), 0.f, kEps); + EXPECT_NEAR(point_cloud[1].time(), 0.f, kEps); } ::testing::Matcher EqualsLandmark( From a89b5ffbc9b9ad2aeddbb563be7271953ff26c70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 3 Aug 2018 00:22:37 +0200 Subject: [PATCH 4/9] vector4->vector3 --- .../cartographer_ros/msg_conversion_test.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 8456fcb8c..40e16dcd1 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -50,22 +50,22 @@ TEST(MsgConversion, LaserScanToPointCloud) { const auto point_cloud = std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; EXPECT_TRUE( - point_cloud[0].position().isApprox(Eigen::Vector4f(1.f, 0.f, 0.f), kEps)); + point_cloud[0].position().isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[1].position().isApprox( Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[2].position().isApprox(Eigen::Vector4f(0.f, 1.f, 0.f), kEps)); + point_cloud[2].position().isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[3].position().isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); + Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE(point_cloud[4].position().isApprox( - Eigen::Vector4f(-1.f, 0.f, 0.f), kEps)); + Eigen::Vector3f(-1.f, 0.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[5].position().isApprox( - Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), + Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE(point_cloud[6].position().isApprox( - Eigen::Vector4f(0.f, -1.f, 0.f), kEps)); + Eigen::Vector3f(0.f, -1.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[7].position().isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); + Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); for (int i = 0; i < 8; ++i) { EXPECT_NEAR(point_cloud[i].time(), 0.f, kEps); } @@ -88,9 +88,9 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; ASSERT_EQ(2, point_cloud.size()); EXPECT_TRUE( - point_cloud[0].position().isApprox(Eigen::Vector4f(0.f, 2.f, 0.f), kEps)); + point_cloud[0].position().isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[1].position().isApprox( - Eigen::Vector4f(-3.f, 0.f, 0.f), kEps)); + Eigen::Vector3f(-3.f, 0.f, 0.f), kEps)); EXPECT_NEAR(point_cloud[0].time(), 0.f, kEps); EXPECT_NEAR(point_cloud[1].time(), 0.f, kEps); } From 3218f8bb1d14431bb6cbc9f0f8611e6f96ea28ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 3 Aug 2018 00:24:45 +0200 Subject: [PATCH 5/9] one more --- cartographer_ros/cartographer_ros/msg_conversion_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 40e16dcd1..d568522ed 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -52,7 +52,7 @@ TEST(MsgConversion, LaserScanToPointCloud) { EXPECT_TRUE( point_cloud[0].position().isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[1].position().isApprox( - Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); + Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( point_cloud[2].position().isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); EXPECT_TRUE(point_cloud[3].position().isApprox( From 1f04062511066ece9d51b87024b541e88fe3dfd6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Wed, 8 Aug 2018 23:42:49 +0200 Subject: [PATCH 6/9] follow changes in cartographer PR --- .../cartographer_ros/assets_writer.cc | 6 ++- .../cartographer_ros/msg_conversion.cc | 39 +++++++++---------- cartographer_ros/cartographer_ros/node.cc | 4 +- .../cartographer_ros/rosbag_validate_main.cc | 10 ++--- .../cartographer_ros/sensor_bridge.cc | 10 ++--- 5 files changed, 35 insertions(+), 34 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 6ebd7d78b..51875b541 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -111,7 +111,7 @@ std::unique_ptr HandleMessage( for (size_t i = 0; i < point_cloud.points.size(); ++i) { const carto::common::Time time = point_cloud_time + - carto::common::FromSeconds(point_cloud.points[i].time()); + carto::common::FromSeconds(point_cloud.points[i].time); if (!transform_interpolation_buffer.Has(time)) { continue; } @@ -122,7 +122,9 @@ std::unique_ptr HandleMessage( tracking_frame, message.header.frame_id, ToRos(time))); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); - points_batch->points.push_back(sensor_to_map * point_cloud.points[i]); + points_batch->points.push_back( + sensor_to_map * + carto::sensor::ToRangefinderPoint(point_cloud.points[i])); points_batch->intensities.push_back(point_cloud.intensities[i]); // We use the last transform for the origin, which is approximately correct. points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 898aa8c34..f9efb2fca 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -147,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { 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()); - const cartographer::sensor::TimedRangefinderPoint point( + const cartographer::sensor::TimedRangefinderPoint point{ rotation * (first_echo * Eigen::Vector3f::UnitX()), - i * msg.time_increment); + i * msg.time_increment}; point_cloud.points.push_back(point); if (msg.intensities.size() > 0) { CHECK_EQ(msg.intensities.size(), msg.ranges.size()); @@ -165,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { } ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back().time(); + const double duration = point_cloud.points.back().time; timestamp += cartographer::common::FromSeconds(duration); for (auto& point : point_cloud.points) { - point.set_time(point.time() - duration); + point.time -= duration; } } return std::make_tuple(point_cloud, timestamp); @@ -192,9 +192,9 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) { - stream.next(point.position().x()); - stream.next(point.position().y()); - stream.next(point.position().z()); + stream.next(point.position.x()); + stream.next(point.position.y()); + stream.next(point.position.z()); stream.next(kPointCloudComponentFourMagic); } return msg; @@ -225,8 +225,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back( - Eigen::Vector3f{point.x, point.y, point.z}, point.time); + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); point_cloud.intensities.push_back(point.intensity); } } else { @@ -235,8 +235,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back( - Eigen::Vector3f{point.x, point.y, point.z}, 0.f); + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); point_cloud.intensities.push_back(point.intensity); } } @@ -248,8 +248,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back( - Eigen::Vector3f{point.x, point.y, point.z}, point.time); + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, point.time}); point_cloud.intensities.push_back(1.0f); } } else { @@ -258,21 +258,20 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { point_cloud.points.reserve(pcl_point_cloud.size()); point_cloud.intensities.reserve(pcl_point_cloud.size()); for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back( - Eigen::Vector3f{point.x, point.y, point.z}, 0.f); + point_cloud.points.push_back( + {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); point_cloud.intensities.push_back(1.0f); } } } ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); if (!point_cloud.points.empty()) { - const double duration = point_cloud.points.back().time(); + const double duration = point_cloud.points.back().time; timestamp += cartographer::common::FromSeconds(duration); for (auto& point : point_cloud.points) { - point.set_time(point.time() - duration); - CHECK_LE(point.time(), 0) - << "Encountered a point with a larger stamp than " - "the last point in the cloud."; + point.time -= duration; + CHECK_LE(point.time, 0) << "Encountered a point with a larger stamp than " + "the last point in the cloud."; } } return std::make_tuple(point_cloud, timestamp); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index de309f52b..835b140a6 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -210,8 +210,8 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { .returns.size()); for (const cartographer::sensor::RangefinderPoint point : trajectory_data.local_slam_data->range_data_in_local.returns) { - point_cloud.push_back(cartographer::sensor::TimedRangefinderPoint( - point, 0.f /* time */)); + point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint( + point, 0. /* time */)); } scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( carto::common::ToUniversal(trajectory_data.local_slam_data->time), diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index 0b5f18d9c..c52c70049 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -219,15 +219,15 @@ class RangeDataChecker { const cartographer::sensor::TimedPointCloud& point_cloud = std::get<0>(point_cloud_time).points; *to = std::get<1>(point_cloud_time); - *from = *to + cartographer::common::FromSeconds(point_cloud[0].time()); + *from = *to + cartographer::common::FromSeconds(point_cloud[0].time); Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); for (const auto& point : point_cloud) { - points_sum.head<3>() += point.position(); - points_sum[3] += point.time(); + points_sum.head<3>() += point.position; + points_sum[3] += point.time; } if (point_cloud.size() > 0) { - double first_point_relative_time = point_cloud[0].time(); - double last_point_relative_time = (*point_cloud.rbegin()).time(); + double first_point_relative_time = point_cloud[0].time; + double last_point_relative_time = (*point_cloud.rbegin()).time; if (first_point_relative_time > 0) { LOG_FIRST_N(ERROR, 1) << "Sensor with frame_id \"" << message.header.frame_id diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index ea1cf7ee6..c2bbe3479 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -177,7 +177,7 @@ void SensorBridge::HandleLaserScan( if (points.points.empty()) { return; } - CHECK_LE(points.points.back().time(), 0); + CHECK_LE(points.points.back().time, 0); // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = @@ -189,7 +189,7 @@ void SensorBridge::HandleLaserScan( if (start_index == end_index) { continue; } - const double time_to_subdivision_end = subdivision.back().time(); + const double time_to_subdivision_end = subdivision.back().time; // `subdivision_time` is the end of the measurement so sensor::Collator will // send all other sensor data first. const carto::common::Time subdivision_time = @@ -205,9 +205,9 @@ void SensorBridge::HandleLaserScan( } sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time; for (auto& point : subdivision) { - point.set_time(point.time() - time_to_subdivision_end); + point.time -= time_to_subdivision_end; } - CHECK_EQ(subdivision.back().time(), 0); + CHECK_EQ(subdivision.back().time, 0); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } } @@ -216,7 +216,7 @@ void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { if (!ranges.empty()) { - CHECK_LE(ranges.back().time(), 0); + CHECK_LE(ranges.back().time, 0); } const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); From ceb6bac85846693e4ea03fc72f835c4ca10f39a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 9 Aug 2018 00:28:30 +0200 Subject: [PATCH 7/9] fix test --- .../cartographer_ros/msg_conversion_test.cc | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index d568522ed..89c8246d3 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -50,24 +50,24 @@ TEST(MsgConversion, LaserScanToPointCloud) { const auto point_cloud = std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; EXPECT_TRUE( - point_cloud[0].position().isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[1].position().isApprox( + point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[1].position.isApprox( Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); EXPECT_TRUE( - point_cloud[2].position().isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[3].position().isApprox( + point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[3].position.isApprox( Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps)); - EXPECT_TRUE(point_cloud[4].position().isApprox( - Eigen::Vector3f(-1.f, 0.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[5].position().isApprox( + EXPECT_TRUE( + point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[5].position.isApprox( Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); - EXPECT_TRUE(point_cloud[6].position().isApprox( - Eigen::Vector3f(0.f, -1.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[7].position().isApprox( + EXPECT_TRUE( + point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps)); + EXPECT_TRUE(point_cloud[7].position.isApprox( Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps)); for (int i = 0; i < 8; ++i) { - EXPECT_NEAR(point_cloud[i].time(), 0.f, kEps); + EXPECT_NEAR(point_cloud[i].time, 0.f, kEps); } } @@ -88,11 +88,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; ASSERT_EQ(2, point_cloud.size()); EXPECT_TRUE( - point_cloud[0].position().isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); - EXPECT_TRUE(point_cloud[1].position().isApprox( - Eigen::Vector3f(-3.f, 0.f, 0.f), kEps)); - EXPECT_NEAR(point_cloud[0].time(), 0.f, kEps); - EXPECT_NEAR(point_cloud[1].time(), 0.f, kEps); + point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); + EXPECT_TRUE( + point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps)); + EXPECT_NEAR(point_cloud[0].time, 0.f, kEps); + EXPECT_NEAR(point_cloud[1].time, 0.f, kEps); } ::testing::Matcher EqualsLandmark( From cd4fe50e224f3daf4482441e4457872f75c064d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 23 Mar 2018 21:37:38 +0100 Subject: [PATCH 8/9] use larics --- Dockerfile.indigo | 5 +---- Dockerfile.kinetic | 5 +---- Dockerfile.lunar | 5 +---- cartographer_ros.rosinstall | 4 ++-- jenkins/Dockerfile.kinetic | 2 +- jenkins/worker.py | 2 +- 6 files changed, 7 insertions(+), 16 deletions(-) diff --git a/Dockerfile.indigo b/Dockerfile.indigo index 2ea9ed60e..2422827c5 100644 --- a/Dockerfile.indigo +++ b/Dockerfile.indigo @@ -16,13 +16,10 @@ FROM ros:indigo ARG CARTOGRAPHER_VERSION=master -# We require a GitHub access token to be passed. -ARG github_token - # First, we invalidate the entire cache if googlecartographer/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/Dockerfile.kinetic b/Dockerfile.kinetic index d7a26c373..f67e956a5 100644 --- a/Dockerfile.kinetic +++ b/Dockerfile.kinetic @@ -16,16 +16,13 @@ FROM ros:kinetic ARG CARTOGRAPHER_VERSION=master -# We require a GitHub access token to be passed. -ARG github_token - # Xenial's base image doesn't ship with sudo. RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* # First, we invalidate the entire cache if googlecartographer/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/Dockerfile.lunar b/Dockerfile.lunar index f5af22ad1..8f3c996a4 100644 --- a/Dockerfile.lunar +++ b/Dockerfile.lunar @@ -16,16 +16,13 @@ FROM ros:lunar ARG CARTOGRAPHER_VERSION=master -# We require a GitHub access token to be passed. -ARG github_token - # Xenial's base image doesn't ship with sudo. RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* # First, we invalidate the entire cache if googlecartographer/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \ +ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/cartographer_ros.rosinstall b/cartographer_ros.rosinstall index 89dae9bd4..d8d2530a5 100644 --- a/cartographer_ros.rosinstall +++ b/cartographer_ros.rosinstall @@ -1,3 +1,3 @@ -- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'} -- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'} +- git: {local-name: cartographer, uri: 'https://github.com/larics/cartographer.git'} +- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git'} - git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'} diff --git a/jenkins/Dockerfile.kinetic b/jenkins/Dockerfile.kinetic index 227aa79f3..8e9a31131 100644 --- a/jenkins/Dockerfile.kinetic +++ b/jenkins/Dockerfile.kinetic @@ -21,7 +21,7 @@ RUN apt-get update && apt-get install -y sudo time && rm -rf /var/lib/apt/lists/ # First, we invalidate the entire cache if googlecartographer/cartographer has # changed. This file's content changes whenever master changes. See: # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone -ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \ +ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \ cartographer_ros/cartographer_version.json # wstool needs the updated rosinstall file to clone the correct repos. diff --git a/jenkins/worker.py b/jenkins/worker.py index e23213e34..b71eb1568 100644 --- a/jenkins/worker.py +++ b/jenkins/worker.py @@ -104,7 +104,7 @@ def get_head_git_sha1(): """Returns the SHA-1 hash of the commit tagged HEAD.""" output = subprocess.check_output([ 'git', 'ls-remote', - 'https://github.com/googlecartographer/cartographer.git' + 'https://github.com/larics/cartographer.git' ]) parsed = GIT_SHA1_PATTERN.extract(output) return parsed['sha1'] From 66166ba86a18ec2c04d915f70d6d96b6a6fcb5ae Mon Sep 17 00:00:00 2001 From: "azure-pipelines[bot]" Date: Mon, 19 Nov 2018 13:26:17 +0000 Subject: [PATCH 9/9] Set up CI with Azure Pipelines --- azure-pipelines.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 azure-pipelines.yml diff --git a/azure-pipelines.yml b/azure-pipelines.yml new file mode 100644 index 000000000..3b9370d1d --- /dev/null +++ b/azure-pipelines.yml @@ -0,0 +1,16 @@ +# Starter pipeline +# Start with a minimal pipeline that you can customize to build and deploy your code. +# Add steps that build, run tests, deploy, and more: +# https://aka.ms/yaml + +pool: + vmImage: 'Ubuntu 16.04' + +steps: +- script: echo Hello, world! + displayName: 'Run a one-line script' + +- script: | + echo Add other tasks to build, test, and deploy your project. + echo See https://aka.ms/yaml + displayName: 'Run a multi-line script'