diff --git a/cartographer_ros/configuration_files/3d_mapping.lua b/cartographer_ros/configuration_files/3d_mapping.lua deleted file mode 100644 index 1d42b17..0000000 --- a/cartographer_ros/configuration_files/3d_mapping.lua +++ /dev/null @@ -1,19 +0,0 @@ --- Copyright 2016 The Cartographer Authors --- --- Licensed under the Apache License, Version 2.0 (the "License"); --- you may not use this file except in compliance with the License. --- You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, --- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. --- See the License for the specific language governing permissions and --- limitations under the License. - -include "backpack_3d.lua" - -options.trajectory_builder.scans_per_accumulation = 2 - -return options diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 43a4a4c..ae099e4 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -29,4 +29,6 @@ options.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_ options.sparse_pose_graph.constraint_builder.min_score = 0.62 options.sparse_pose_graph.constraint_builder.log_matches = true +options.trajectory_builder.scans_per_accumulation = 2 + return options diff --git a/cartographer_ros/launch/mapping_2d.launch b/cartographer_ros/launch/backpack_2d.launch similarity index 100% rename from cartographer_ros/launch/mapping_2d.launch rename to cartographer_ros/launch/backpack_2d.launch diff --git a/cartographer_ros/launch/3d_mapping.launch b/cartographer_ros/launch/backpack_3d.launch similarity index 98% rename from cartographer_ros/launch/3d_mapping.launch rename to cartographer_ros/launch/backpack_3d.launch index 91099d0..6801ef2 100644 --- a/cartographer_ros/launch/3d_mapping.launch +++ b/cartographer_ros/launch/backpack_3d.launch @@ -90,7 +90,7 @@ configuration_files_directories: [ "$(find cartographer_ros)/configuration_files" ] - mapping_configuration_basename: "3d_mapping.lua" + mapping_configuration_basename: "backpack_3d.lua" imu_topic: "/imu" laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"] laser_min_range_m: 0. diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 94d43d8..942d027 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -80,7 +80,7 @@ constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds constexpr double kMaxTransformDelaySeconds = 1.; -Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) { +Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z), @@ -220,7 +220,7 @@ Node::Node() Rigid3d Node::LookupToTrackingTransformOrThrow( const ::cartographer::common::Time time, const string& frame_id) { - return ToRidig3d( + return ToRigid3d( tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time), ros::Duration(kMaxTransformDelaySeconds))); }