From 61dd57bd9436910f889d463e36f4c30a1354b2cf Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 2 Mar 2018 11:37:10 +0100 Subject: [PATCH] Pass ROS landmark topic to the cartographer. (#746) [Landmark RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md) --- .../cartographer_ros/msg_conversion.cc | 16 +++- .../cartographer_ros/msg_conversion.h | 5 ++ .../cartographer_ros/msg_conversion_test.cc | 88 +++++++++++++++---- cartographer_ros/cartographer_ros/node.cc | 28 +++++- cartographer_ros/cartographer_ros/node.h | 10 ++- .../cartographer_ros/node_constants.h | 1 + .../cartographer_ros/offline_node.cc | 5 ++ .../cartographer_ros/sensor_bridge.cc | 6 ++ .../cartographer_ros/sensor_bridge.h | 5 ++ .../cartographer_ros/trajectory_options.cc | 7 ++ .../cartographer_ros/trajectory_options.h | 2 + .../configuration_files/backpack_2d.lua | 2 + .../configuration_files/backpack_3d.lua | 2 + cartographer_ros/configuration_files/pr2.lua | 2 + .../configuration_files/revo_lds.lua | 2 + .../configuration_files/taurob_tracker.lua | 2 + cartographer_ros_msgs/msg/SensorTopics.msg | 1 + .../msg/TrajectoryOptions.msg | 2 + 18 files changed, 165 insertions(+), 21 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 448c4aa..2548005 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -40,7 +40,6 @@ #include "sensor_msgs/PointCloud2.h" namespace cartographer_ros { - namespace { // The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each @@ -48,8 +47,12 @@ namespace { // properly. constexpr float kPointCloudComponentFourMagic = 1.; +using ::cartographer::sensor::LandmarkData; +using ::cartographer::sensor::LandmarkObservation; using ::cartographer::sensor::PointCloudWithIntensities; using ::cartographer::transform::Rigid3d; +using ::cartographer_ros_msgs::LandmarkEntry; +using ::cartographer_ros_msgs::LandmarkList; sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, const std::string& frame_id, @@ -205,6 +208,17 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) { return std::make_tuple(point_cloud, FromRos(message.header.stamp)); } +LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { + LandmarkData landmark_data; + landmark_data.time = FromRos(landmark_list.header.stamp); + for (const LandmarkEntry& entry : landmark_list.landmark) { + landmark_data.landmark_observations.push_back( + {entry.id, ToRigid3d(entry.tracking_from_landmark_transform), + entry.translation_weight, entry.rotation_weight}); + } + return landmark_data; +} + Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { return Rigid3d(ToEigen(transform.transform.translation), ToEigen(transform.transform.rotation)); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index 3941d1e..b0467ce 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -20,8 +20,10 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/io/submap_painter.h" +#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" +#include "cartographer_ros_msgs/LandmarkList.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" @@ -63,6 +65,9 @@ std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message); +::cartographer::sensor::LandmarkData ToLandmarkData( + const cartographer_ros_msgs::LandmarkList& landmark_list); + ::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 index 576bcdd..4111deb 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -15,16 +15,28 @@ */ #include "cartographer_ros/msg_conversion.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" #include #include +#include "cartographer_ros/time_conversion.h" +#include "gmock/gmock.h" #include "gtest/gtest.h" #include "sensor_msgs/LaserScan.h" namespace cartographer_ros { namespace { +using ::cartographer::sensor::LandmarkData; +using ::cartographer::sensor::LandmarkObservation; +using ::testing::AllOf; +using ::testing::DoubleNear; +using ::testing::ElementsAre; +using ::testing::Field; + +constexpr double kEps = 1e-6; + TEST(MsgConversion, LaserScanToPointCloud) { sensor_msgs::LaserScan laser_scan; for (int i = 0; i < 8; ++i) { @@ -39,25 +51,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), 1e-6)); + 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), - 1e-6)); + kEps)); EXPECT_TRUE( - point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), 1e-6)); + 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), - 1e-6)); + kEps)); EXPECT_TRUE( - point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), 1e-6)); + 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), - 1e-6)); + kEps)); EXPECT_TRUE( - point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), 1e-6)); + 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), - 1e-6)); + kEps)); } TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { @@ -77,9 +89,53 @@ 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), 1e-6)); + 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), 1e-6)); + point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps)); +} + +::testing::Matcher EqualsLandmark( + const LandmarkObservation& expected) { + return ::testing::AllOf( + Field(&LandmarkObservation::id, expected.id), + Field(&LandmarkObservation::landmark_to_tracking_transform, + ::cartographer::transform::IsNearly( + expected.landmark_to_tracking_transform, kEps)), + Field(&LandmarkObservation::translation_weight, + DoubleNear(expected.translation_weight, kEps)), + Field(&LandmarkObservation::rotation_weight, + DoubleNear(expected.rotation_weight, kEps))); +} + +TEST(MsgConversion, LandmarkListToLandmarkData) { + cartographer_ros_msgs::LandmarkList message; + message.header.stamp.fromSec(10); + + cartographer_ros_msgs::LandmarkEntry landmark_0; + landmark_0.id = "landmark_0"; + landmark_0.tracking_from_landmark_transform.position.x = 1.0; + landmark_0.tracking_from_landmark_transform.position.y = 2.0; + landmark_0.tracking_from_landmark_transform.position.z = 3.0; + landmark_0.tracking_from_landmark_transform.orientation.w = 1.0; + landmark_0.tracking_from_landmark_transform.orientation.x = 0.0; + landmark_0.tracking_from_landmark_transform.orientation.y = 0.0; + landmark_0.tracking_from_landmark_transform.orientation.z = 0.0; + landmark_0.translation_weight = 1.0; + landmark_0.rotation_weight = 2.0; + message.landmark.push_back(landmark_0); + + LandmarkData actual_landmark_data = ToLandmarkData(message); + EXPECT_THAT(actual_landmark_data, + AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)), + Field(&LandmarkData::landmark_observations, + ElementsAre(EqualsLandmark(LandmarkObservation{ + "landmark_0", + ::cartographer::transform::Rigid3d( + Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 0., 0., 0.)), + 1.f, + 2.f, + }))))); } TEST(MsgConversion, LatLongAltToEcef) { @@ -90,23 +146,23 @@ TEST(MsgConversion, LatLongAltToEcef) { EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0))) << plus_ten_meters; Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0); - EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9)) + EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps)) << north_pole; Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0); EXPECT_TRUE( - also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9)) + also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps)) << also_north_pole; Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0); - EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), 1e-9)) + EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps)) << south_pole; Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20); EXPECT_TRUE( - above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), 1e-9)) + above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps)) << above_south_pole; Eigen::Vector3d somewhere_on_earth = LatLongAltToEcef(48.1372149, 11.5748024, 517.1); EXPECT_TRUE(somewhere_on_earth.isApprox( - Eigen::Vector3d(4177983, 855702, 4727457), 1e-6)) + Eigen::Vector3d(4177983, 855702, 4727457), kEps)) << somewhere_on_earth; } @@ -143,7 +199,7 @@ TEST(MsgConversion, ComputeLocalFrameFromLatLong) { ComputeLocalFrameFromLatLong(latitude, longitude); EXPECT_TRUE((transform_to_local_frame * LatLongAltToEcef(latitude, longitude, altitude)) - .isApprox(altitude * Eigen::Vector3d::UnitZ(), 1e-6)) + .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps)) << transform_to_local_frame * LatLongAltToEcef(latitude, longitude, altitude) << "\n" diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 6acc3be..b39f98a 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -55,6 +55,7 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { topics.imu_topic = kImuTopic; topics.odometry_topic = kOdometryTopic; topics.nav_sat_fix_topic = kNavSatFixTopic; + topics.landmark_topic = kLandmarkTopic; return topics; } @@ -171,7 +172,8 @@ void Node::AddSensorSamplers(const int trajectory_id, std::piecewise_construct, std::forward_as_tuple(trajectory_id), std::forward_as_tuple( options.rangefinder_sampling_ratio, options.odometry_sampling_ratio, - options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio)); + options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio, + options.landmarks_sampling_ratio)); } void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { @@ -318,7 +320,10 @@ Node::ComputeExpectedSensorIds( expected_topics.insert( SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); } - + // Landmark is optional. + if (options.use_landmarks) { + expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic}); + } return expected_topics; } @@ -397,6 +402,14 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, this), topic}); } + if (options.use_landmarks) { + std::string topic = topics.landmark_topic; + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, + this), + topic}); + } } bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { @@ -603,6 +616,17 @@ void Node::HandleNavSatFixMessage(const int trajectory_id, ->HandleNavSatFixMessage(sensor_id, msg); } +void Node::HandleLandmarkMessage( + const int trajectory_id, const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleLandmarkMessage(sensor_id, msg); +} + void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 4c61e10..accbc7b 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -95,6 +95,9 @@ class Node { const nav_msgs::Odometry::ConstPtr& msg); void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg); + void HandleLandmarkMessage( + int trajectory_id, const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, @@ -180,16 +183,19 @@ class Node { TrajectorySensorSamplers(const double rangefinder_sampling_ratio, const double odometry_sampling_ratio, const double fixed_frame_pose_sampling_ratio, - const double imu_sampling_ratio) + const double imu_sampling_ratio, + const double landmark_sampling_ratio) : rangefinder_sampler(rangefinder_sampling_ratio), odometry_sampler(odometry_sampling_ratio), fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio), - imu_sampler(imu_sampling_ratio) {} + imu_sampler(imu_sampling_ratio), + landmark_sampler(landmark_sampling_ratio) {} ::cartographer::common::FixedRatioSampler rangefinder_sampler; ::cartographer::common::FixedRatioSampler odometry_sampler; ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler; ::cartographer::common::FixedRatioSampler imu_sampler; + ::cartographer::common::FixedRatioSampler landmark_sampler; }; // These are keyed with 'trajectory_id'. diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index 5c0164e..6f08506 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -29,6 +29,7 @@ constexpr char kPointCloud2Topic[] = "points2"; constexpr char kImuTopic[] = "imu"; constexpr char kOdometryTopic[] = "odom"; constexpr char kNavSatFixTopic[] = "fix"; +constexpr char kLandmarkTopic[] = "landmark"; constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 96f1c53..389753b 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -292,6 +292,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { node.HandleNavSatFixMessage(trajectory_id, sensor_id, msg.instantiate()); } + if (msg.isType()) { + node.HandleLandmarkMessage( + trajectory_id, sensor_id, + msg.instantiate()); + } } clock.clock = msg.getTime(); clock_publisher.publish(clock); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 204a829..181453b 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -98,6 +98,12 @@ void SensorBridge::HandleNavSatFixMessage( msg->altitude)))}); } +void SensorBridge::HandleLandmarkMessage( + const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { + trajectory_builder_->AddSensorData(sensor_id, ToLandmarkData(*msg)); +} + std::unique_ptr SensorBridge::ToImuData( const sensor_msgs::Imu::ConstPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 906c3e3..c2ab811 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -26,6 +26,7 @@ #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/tf_bridge.h" +#include "cartographer_ros_msgs/LandmarkList.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" #include "nav_msgs/OccupancyGrid.h" @@ -55,6 +56,10 @@ class SensorBridge { const nav_msgs::Odometry::ConstPtr& msg); void HandleNavSatFixMessage(const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg); + void HandleLandmarkMessage( + const std::string& sensor_id, + const cartographer_ros_msgs::LandmarkList::ConstPtr& msg); + std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( const sensor_msgs::Imu::ConstPtr& msg); void HandleImuMessage(const std::string& sensor_id, diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index daec52b..0c092ab 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -54,6 +54,7 @@ TrajectoryOptions CreateTrajectoryOptions( lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); + options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks"); options.publish_frame_projected_to_2d = lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d"); options.num_laser_scans = @@ -73,6 +74,8 @@ TrajectoryOptions CreateTrajectoryOptions( lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio"); options.imu_sampling_ratio = lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); + options.landmarks_sampling_ratio = + lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio"); CheckTrajectoryOptions(options); return options; } @@ -110,6 +113,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->provide_odom_frame = msg.provide_odom_frame; options->use_odometry = msg.use_odometry; options->use_nav_sat = msg.use_nav_sat; + options->use_landmarks = msg.use_landmarks; options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d; options->num_laser_scans = msg.num_laser_scans; options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans; @@ -121,6 +125,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->fixed_frame_pose_sampling_ratio = msg.fixed_frame_pose_sampling_ratio; options->imu_sampling_ratio = msg.imu_sampling_ratio; + options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio; if (!options->trajectory_builder_options.ParseFromString( msg.trajectory_builder_options_proto)) { LOG(ERROR) << "Failed to parse protobuf"; @@ -139,6 +144,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage( msg.provide_odom_frame = options.provide_odom_frame; msg.use_odometry = options.use_odometry; msg.use_nav_sat = options.use_nav_sat; + msg.use_landmarks = options.use_landmarks; msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d; msg.num_laser_scans = options.num_laser_scans; msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans; @@ -148,6 +154,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage( msg.odometry_sampling_ratio = options.odometry_sampling_ratio; msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio; msg.imu_sampling_ratio = options.imu_sampling_ratio; + msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio; options.trajectory_builder_options.SerializeToString( &msg.trajectory_builder_options_proto); return msg; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index fa69f08..c39b039 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -35,6 +35,7 @@ struct TrajectoryOptions { bool provide_odom_frame; bool use_odometry; bool use_nav_sat; + bool use_landmarks; bool publish_frame_projected_to_2d; int num_laser_scans; int num_multi_echo_laser_scans; @@ -44,6 +45,7 @@ struct TrajectoryOptions { double odometry_sampling_ratio; double fixed_frame_pose_sampling_ratio; double imu_sampling_ratio; + double landmarks_sampling_ratio; }; ::cartographer::mapping::proto::InitialTrajectoryPose diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 841b11a..85699db 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -26,6 +26,7 @@ options = { publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, + use_landmarks = false, num_laser_scans = 0, num_multi_echo_laser_scans = 1, num_subdivisions_per_laser_scan = 10, @@ -38,6 +39,7 @@ options = { odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 3dc466c..ad730ba 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -26,6 +26,7 @@ options = { publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, + use_landmarks = false, num_laser_scans = 0, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -38,6 +39,7 @@ options = { odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., } TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160 diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index a6c4f4a..31ec614 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -26,6 +26,7 @@ options = { publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, + use_landmarks = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -38,6 +39,7 @@ options = { odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index c16426b..f6935e9 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -26,6 +26,7 @@ options = { publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, + use_landmarks = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -38,6 +39,7 @@ options = { odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index a66a5c6..d1cd4c3 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -26,6 +26,7 @@ options = { publish_frame_projected_to_2d = false, use_odometry = true, use_nav_sat = false, + use_landmarks = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -38,6 +39,7 @@ options = { odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., } TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180 diff --git a/cartographer_ros_msgs/msg/SensorTopics.msg b/cartographer_ros_msgs/msg/SensorTopics.msg index ab13fbb..1db9686 100644 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ b/cartographer_ros_msgs/msg/SensorTopics.msg @@ -18,3 +18,4 @@ string point_cloud2_topic string imu_topic string odometry_topic string nav_sat_fix_topic +string landmark_topic diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index 21017a6..b41810c 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/cartographer_ros_msgs/msg/TrajectoryOptions.msg @@ -18,6 +18,7 @@ string odom_frame bool provide_odom_frame bool use_odometry bool use_nav_sat +bool use_landmarks bool publish_frame_projected_to_2d int32 num_laser_scans int32 num_multi_echo_laser_scans @@ -27,6 +28,7 @@ float64 rangefinder_sampling_ratio float64 odometry_sampling_ratio float64 fixed_frame_pose_sampling_ratio float64 imu_sampling_ratio +float64 landmarks_sampling_ratio # This is a binary-encoded # 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.