12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pass ROS landmark topic to the cartographer. (#746) 

[Landmark RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-03-02 11:37:10 +01:00 committed by GitHub
parent e583785720
commit 61dd57bd94
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
18 changed files with 165 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -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));

5
cartographer_ros/cartographer_ros/msg_conversion.h
View File

@ -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);

88
cartographer_ros/cartographer_ros/msg_conversion_test.cc
View File

@ -15,16 +15,28 @@
*/
#include "cartographer_ros/msg_conversion.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include <cmath>
#include <random>
#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<const LandmarkObservation&> 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"

28
cartographer_ros/cartographer_ros/node.cc
View File

@ -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<cartographer_ros_msgs::LandmarkList>(
&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) {

10
cartographer_ros/cartographer_ros/node.h
View File

@ -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'.

1
cartographer_ros/cartographer_ros/node_constants.h
View File

@ -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";

5
cartographer_ros/cartographer_ros/offline_node.cc
View File

@ -292,6 +292,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
node.HandleNavSatFixMessage(trajectory_id, sensor_id,
msg.instantiate<sensor_msgs::NavSatFix>());
}
if (msg.isType<cartographer_ros_msgs::LandmarkList>()) {
node.HandleLandmarkMessage(
trajectory_id, sensor_id,
msg.instantiate<cartographer_ros_msgs::LandmarkList>());
}
}
clock.clock = msg.getTime();
clock_publisher.publish(clock);

6
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -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<carto::sensor::ImuData> SensorBridge::ToImuData(
const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->linear_acceleration_covariance[0], -1)

5
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -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,

7
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

@ -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;

2
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -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

2
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -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

2
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -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

2
cartographer_ros/configuration_files/pr2.lua
View File

@ -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

2
cartographer_ros/configuration_files/revo_lds.lua
View File

@ -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

2
cartographer_ros/configuration_files/taurob_tracker.lua
View File

@ -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

1
cartographer_ros_msgs/msg/SensorTopics.msg
View File

@ -18,3 +18,4 @@ string point_cloud2_topic
string imu_topic
string odometry_topic
string nav_sat_fix_topic
string landmark_topic

2
cartographer_ros_msgs/msg/TrajectoryOptions.msg
View File

@ -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.