diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index f9b68ed..51875b5 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,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].head<3>()); + 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 2c996ca..7e19088 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.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.push_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.push_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.push_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.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()[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.time -= duration; + CHECK_LE(point.time, 0.f) + << "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/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 0ba87ae..89c8246 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), + 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( + 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( + Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.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)); - 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), - kEps)); + 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); + } } 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)); + point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps)); EXPECT_TRUE( - point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps)); + 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( diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 720934b..8c988b3 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::ToTimedRangefinderPoint( + 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 40bd754..c52c700 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 1bd3822..525c585 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.f); // 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.time -= time_to_subdivision_end; } - CHECK_EQ(subdivision.back()[3], 0); + CHECK_EQ(subdivision.back().time, 0.f); 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.f); } const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); diff --git a/scripts/install_debs.sh b/scripts/install_debs.sh index 6916377..9cc45d7 100755 --- a/scripts/install_debs.sh +++ b/scripts/install_debs.sh @@ -43,3 +43,6 @@ apt-get install -y ninja-build # Install rosdep dependencies. rosdep update rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y + +# Update rosconsole-bridge to fix build issue with Docker image for Kinetic +sudo apt-get install ros-${ROS_DISTRO}-rosconsole-bridge -y