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

Adds sensor data rate logging. (#26) 

Fixes a few CMakeLists.txt errors.
Strips slashes from sensor data frames in case the data is provided
using tf instead of tf2.
master
Damon Kohler 2016-08-24 16:24:32 +02:00 committed by GitHub
parent 428cd6f62d
commit 008b5ef377
2 changed files with 132 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer_ros/CMakeLists.txt
View File

@ -96,6 +96,10 @@ target_link_libraries(cartographer_node
gflags # TODO(whess): Use or remove gflags_catkin. gflags # TODO(whess): Use or remove gflags_catkin.
) )
add_dependencies(cartographer_node
${catkin_EXPORTED_TARGETS}
)
add_library(cartographer_rviz_submaps_visualization add_library(cartographer_rviz_submaps_visualization
src/drawable_submap.cc src/drawable_submap.cc
src/drawable_submap.h src/drawable_submap.h
@ -111,6 +115,10 @@ target_link_libraries(cartographer_rviz_submaps_visualization
${ZLIB_LIBRARIES} ${ZLIB_LIBRARIES}
) )
add_dependencies(cartographer_rviz_submaps_visualization
${catkin_EXPORTED_TARGETS}
)
add_executable(time_conversion_test add_executable(time_conversion_test
src/time_conversion_test.cc src/time_conversion_test.cc
src/time_conversion.h src/time_conversion.h
@ -123,6 +131,10 @@ target_link_libraries(time_conversion_test
${catkin_LIBRARIES} ${catkin_LIBRARIES}
) )
add_dependencies(time_conversion_test
${catkin_EXPORTED_TARGETS}
)
add_test(time_conversion_test time_conversion_test) add_test(time_conversion_test time_conversion_test)
install(DIRECTORY launch/ install(DIRECTORY launch/
@ -138,17 +150,17 @@ install(DIRECTORY configuration_files/
) )
install(TARGETS install(TARGETS
cartographer_rviz_submaps_visualization cartographer_rviz_submaps_visualization cartographer_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
) )
install(FILES install(FILES
plugin_description.xml rviz_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
) )
install(DIRECTORY ogre_media/ install(DIRECTORY ogre_media/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ogre_media} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ogre_media
) )

198
cartographer_ros/src/cartographer_node_main.cc
View File

@ -15,6 +15,7 @@
*/ */
#include <cstring> #include <cstring>
#include <map>
#include <queue> #include <queue>
#include <string> #include <string>
#include <vector> #include <vector>
@ -26,6 +27,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/rate_timer.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/kalman_filter/pose_tracker.h"
@ -81,9 +83,11 @@ DEFINE_string(configuration_basename, "",
namespace cartographer_ros { namespace cartographer_ros {
namespace { namespace {
using ::cartographer::transform::Rigid3d; namespace carto = ::cartographer;
using ::cartographer::kalman_filter::PoseCovariance; namespace proto = carto::sensor::proto;
namespace proto = ::cartographer::sensor::proto;
using carto::transform::Rigid3d;
using carto::kalman_filter::PoseCovariance;
// TODO(hrapp): Support multi trajectory mapping. // TODO(hrapp): Support multi trajectory mapping.
constexpr int64 kTrajectoryId = 0; constexpr int64 kTrajectoryId = 0;
@ -91,6 +95,7 @@ constexpr int kSubscriberQueueSize = 150;
constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds
constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds
constexpr double kMaxTransformDelaySeconds = 0.01; constexpr double kMaxTransformDelaySeconds = 0.01;
constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
// Unique default topic names. Expected to be remapped as needed. // Unique default topic names. Expected to be remapped as needed.
constexpr char kLaserScanTopic[] = "/scan"; constexpr char kLaserScanTopic[] = "/scan";
@ -99,6 +104,13 @@ constexpr char kPointCloud2Topic[] = "/points2";
constexpr char kImuTopic[] = "/imu"; constexpr char kImuTopic[] = "/imu";
constexpr char kOdometryTopic[] = "/odom"; constexpr char kOdometryTopic[] = "/odom";
const string& CheckNoLeadingSlash(const string& frame_id) {
if (frame_id.size() > 0) {
CHECK_NE(frame_id[0], '/');
}
return frame_id;
}
Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
transform.transform.translation.y, transform.transform.translation.y,
@ -116,7 +128,7 @@ Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) {
pose.orientation.y, pose.orientation.z)); pose.orientation.y, pose.orientation.z));
} }
PoseCovariance ToPoseCovariance(const boost::array<double, 36ul>& covariance) { PoseCovariance ToPoseCovariance(const boost::array<double, 36>& covariance) {
return Eigen::Map<const Eigen::Matrix<double, 6, 6>>(covariance.data()); return Eigen::Map<const Eigen::Matrix<double, 6, 6>>(covariance.data());
} }
@ -149,21 +161,23 @@ geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) {
// is only used for time ordering sensor data before passing it on. // is only used for time ordering sensor data before passing it on.
enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry };
struct SensorData { struct SensorData {
SensorData(const string& init_frame_id, proto::Imu init_imu) SensorData(const string& frame_id, proto::Imu imu)
: type(SensorType::kImu), frame_id(init_frame_id), imu(init_imu) {} : type(SensorType::kImu),
SensorData(const string& init_frame_id, proto::LaserScan init_laser_scan) frame_id(CheckNoLeadingSlash(frame_id)),
imu(imu) {}
SensorData(const string& frame_id, proto::LaserScan laser_scan)
: type(SensorType::kLaserScan), : type(SensorType::kLaserScan),
frame_id(init_frame_id), frame_id(CheckNoLeadingSlash(frame_id)),
laser_scan(init_laser_scan) {} laser_scan(laser_scan) {}
SensorData(const string& init_frame_id, proto::LaserFan3D init_laser_fan_3d) SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d)
: type(SensorType::kLaserFan3D), : type(SensorType::kLaserFan3D),
frame_id(init_frame_id), frame_id(CheckNoLeadingSlash(frame_id)),
laser_fan_3d(init_laser_fan_3d) {} laser_fan_3d(laser_fan_3d) {}
SensorData(const string& init_frame_id, const Rigid3d& init_pose, SensorData(const string& frame_id, const Rigid3d& pose,
const PoseCovariance& init_covariance) const PoseCovariance& covariance)
: type(SensorType::kOdometry), : type(SensorType::kOdometry),
frame_id(init_frame_id), frame_id(frame_id),
odometry{init_pose, init_covariance} {} odometry{pose, covariance} {}
SensorType type; SensorType type;
string frame_id; string frame_id;
@ -208,7 +222,7 @@ class Node {
// Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at // Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time' or throws tf2::TransformException if it does not exist. // 'time' or throws tf2::TransformException if it does not exist.
Rigid3d LookupToTrackingTransformOrThrow(::cartographer::common::Time time, Rigid3d LookupToTrackingTransformOrThrow(carto::common::Time time,
const string& frame_id); const string& frame_id);
bool HandleSubmapQuery( bool HandleSubmapQuery(
@ -220,7 +234,7 @@ class Node {
// TODO(hrapp): Pull out the common functionality between this and MapWriter // TODO(hrapp): Pull out the common functionality between this and MapWriter
// into an open sourcable MapWriter. // into an open sourcable MapWriter.
::cartographer::mapping::SensorCollator<SensorData> sensor_collator_; carto::mapping::SensorCollator<SensorData> sensor_collator_;
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
ros::Subscriber imu_subscriber_; ros::Subscriber imu_subscriber_;
ros::Subscriber laser_subscriber_2d_; ros::Subscriber laser_subscriber_2d_;
@ -238,18 +252,22 @@ class Node {
tf2_ros::Buffer tf_buffer_; tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_; tf2_ros::TransformListener tf_;
tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::TransformBroadcaster tf_broadcaster_;
::cartographer::common::ThreadPool thread_pool_; carto::common::ThreadPool thread_pool_;
int64 last_pose_publish_timestamp_; int64 last_pose_publish_timestamp_;
::cartographer::common::Mutex mutex_; carto::common::Mutex mutex_;
std::unique_ptr<::cartographer::mapping::GlobalTrajectoryBuilderInterface> std::unique_ptr<carto::mapping::GlobalTrajectoryBuilderInterface>
trajectory_builder_ GUARDED_BY(mutex_); trajectory_builder_ GUARDED_BY(mutex_);
std::deque<::cartographer::mapping::TrajectoryNode::ConstantData> std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_node_data_
constant_node_data_ GUARDED_BY(mutex_); GUARDED_BY(mutex_);
std::unique_ptr<::cartographer::mapping::SparsePoseGraph> sparse_pose_graph_; std::unique_ptr<carto::mapping::SparsePoseGraph> sparse_pose_graph_;
::ros::Publisher submap_list_publisher_; ::ros::Publisher submap_list_publisher_;
int64 last_submap_list_publish_timestamp_; int64 last_submap_list_publish_timestamp_;
::ros::ServiceServer submap_query_server_; ::ros::ServiceServer submap_query_server_;
// Time at which we last logged the rates of incoming sensor data.
std::chrono::steady_clock::time_point last_sensor_data_rates_logging_time_;
std::map<string, carto::common::RateTimer<>> rate_timers_;
}; };
Node::Node() Node::Node()
@ -260,8 +278,8 @@ Node::Node()
last_submap_list_publish_timestamp_(0), last_submap_list_publish_timestamp_(0),
last_pose_publish_timestamp_(0) {} last_pose_publish_timestamp_(0) {}
Rigid3d Node::LookupToTrackingTransformOrThrow( Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
const ::cartographer::common::Time time, const string& frame_id) { const string& frame_id) {
return ToRigid3d( return ToRigid3d(
tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time), tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
ros::Duration(kMaxTransformDelaySeconds))); ros::Duration(kMaxTransformDelaySeconds)));
@ -285,18 +303,16 @@ void Node::AddOdometry(int64 timestamp, const string& frame_id,
} }
void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) { void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( auto sensor_data = carto::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg)); msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData( sensor_collator_.AddSensorData(
kTrajectoryId, kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)),
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
kImuTopic, std::move(sensor_data)); kImuTopic, std::move(sensor_data));
} }
void Node::AddImu(const int64 timestamp, const string& frame_id, void Node::AddImu(const int64 timestamp, const string& frame_id,
const proto::Imu& imu) { const proto::Imu& imu) {
const ::cartographer::common::Time time = const carto::common::Time time = carto::common::FromUniversal(timestamp);
::cartographer::common::FromUniversal(timestamp);
try { try {
const Rigid3d sensor_to_tracking = const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id); LookupToTrackingTransformOrThrow(time, frame_id);
@ -306,9 +322,9 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
"otherwise be imprecise."; "otherwise be imprecise.";
trajectory_builder_->AddImuData( trajectory_builder_->AddImuData(
time, sensor_to_tracking.rotation() * time, sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.linear_acceleration()), carto::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() * sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.angular_velocity())); carto::transform::ToEigen(imu.angular_velocity()));
} catch (const tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what(); << ": " << ex.what();
@ -317,28 +333,25 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
void Node::LaserScanMessageCallback( void Node::LaserScanMessageCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) { const sensor_msgs::LaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( auto sensor_data = carto::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg)); msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData( sensor_collator_.AddSensorData(
kTrajectoryId, kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)),
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
kLaserScanTopic, std::move(sensor_data)); kLaserScanTopic, std::move(sensor_data));
} }
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan) { const proto::LaserScan& laser_scan) {
const ::cartographer::common::Time time = const carto::common::Time time = carto::common::FromUniversal(timestamp);
::cartographer::common::FromUniversal(timestamp);
try { try {
const Rigid3d sensor_to_tracking = const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id); LookupToTrackingTransformOrThrow(time, frame_id);
const ::cartographer::sensor::LaserFan laser_fan = const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan(
::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_, laser_scan, laser_min_range_, laser_max_range_,
laser_max_range_,
laser_missing_echo_ray_length_); laser_missing_echo_ray_length_);
const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D( const auto laser_fan_3d = carto::sensor::TransformLaserFan3D(
::cartographer::sensor::ToLaserFan3D(laser_fan), carto::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>()); sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d); trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
} catch (const tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
@ -349,11 +362,10 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
void Node::MultiEchoLaserScanMessageCallback( void Node::MultiEchoLaserScanMessageCallback(
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
auto sensor_data = ::cartographer::common::make_unique<SensorData>( auto sensor_data = carto::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg)); msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData( sensor_collator_.AddSensorData(
kTrajectoryId, kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)),
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
kMultiEchoLaserScanTopic, std::move(sensor_data)); kMultiEchoLaserScanTopic, std::move(sensor_data));
} }
@ -362,24 +374,22 @@ void Node::PointCloud2MessageCallback(
pcl::PointCloud<pcl::PointXYZ> pcl_points; pcl::PointCloud<pcl::PointXYZ> pcl_points;
pcl::fromROSMsg(*msg, pcl_points); pcl::fromROSMsg(*msg, pcl_points);
auto sensor_data = ::cartographer::common::make_unique<SensorData>( auto sensor_data = carto::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(pcl_points)); msg->header.frame_id, ToCartographer(pcl_points));
sensor_collator_.AddSensorData( sensor_collator_.AddSensorData(
kTrajectoryId, kTrajectoryId, carto::common::ToUniversal(FromRos(msg->header.stamp)),
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, topic, std::move(sensor_data));
std::move(sensor_data));
} }
void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d) { const proto::LaserFan3D& laser_fan_3d) {
const ::cartographer::common::Time time = const carto::common::Time time = carto::common::FromUniversal(timestamp);
::cartographer::common::FromUniversal(timestamp);
try { try {
const Rigid3d sensor_to_tracking = const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id); LookupToTrackingTransformOrThrow(time, frame_id);
trajectory_builder_->AddLaserFan3D( trajectory_builder_->AddLaserFan3D(
time, ::cartographer::sensor::TransformLaserFan3D( time, carto::sensor::TransformLaserFan3D(
::cartographer::sensor::FromProto(laser_fan_3d), carto::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>())); sensor_to_tracking.cast<float>()));
} catch (const tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
@ -388,19 +398,20 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
} }
void Node::Initialize() { void Node::Initialize() {
auto file_resolver = ::cartographer::common::make_unique< auto file_resolver =
::cartographer::common::ConfigurationFileResolver>( carto::common::make_unique<carto::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory}); std::vector<string>{FLAGS_configuration_directory});
const string code = const string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( carto::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver), nullptr); code, std::move(file_resolver), nullptr);
tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame"); tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame");
odom_frame_ = lua_parameter_dictionary.GetString("odom_frame"); odom_frame_ = lua_parameter_dictionary.GetString("odom_frame");
map_frame_ = lua_parameter_dictionary.GetString("map_frame"); map_frame_ = lua_parameter_dictionary.GetString("map_frame");
provide_odom_frame_ = lua_parameter_dictionary.GetBool("provide_odom_frame"); provide_odom_frame_ = lua_parameter_dictionary.GetBool("provide_odom_frame");
expect_odometry_data_ = lua_parameter_dictionary.GetBool("expect_odometry_data"); expect_odometry_data_ =
lua_parameter_dictionary.GetBool("expect_odometry_data");
laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range"); laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range");
laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range"); laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range");
laser_missing_echo_ray_length_ = laser_missing_echo_ray_length_ =
@ -443,17 +454,17 @@ void Node::Initialize() {
bool expect_imu_data = true; bool expect_imu_data = true;
if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) { if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) {
auto sparse_pose_graph_2d = ::cartographer::common::make_unique< auto sparse_pose_graph_2d =
::cartographer::mapping_2d::SparsePoseGraph>( carto::common::make_unique<carto::mapping_2d::SparsePoseGraph>(
::cartographer::mapping::CreateSparsePoseGraphOptions( carto::mapping::CreateSparsePoseGraphOptions(
lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()), lua_parameter_dictionary.GetDictionary("sparse_pose_graph")
.get()),
&thread_pool_, &constant_node_data_); &thread_pool_, &constant_node_data_);
auto options = auto options = carto::mapping_2d::CreateLocalTrajectoryBuilderOptions(
::cartographer::mapping_2d::CreateLocalTrajectoryBuilderOptions(
lua_parameter_dictionary.GetDictionary("trajectory_builder").get()); lua_parameter_dictionary.GetDictionary("trajectory_builder").get());
expect_imu_data = options.expect_imu_data(); expect_imu_data = options.expect_imu_data();
trajectory_builder_ = ::cartographer::common::make_unique< trajectory_builder_ =
::cartographer::mapping_2d::GlobalTrajectoryBuilder>( carto::common::make_unique<carto::mapping_2d::GlobalTrajectoryBuilder>(
options, sparse_pose_graph_2d.get()); options, sparse_pose_graph_2d.get());
sparse_pose_graph_ = std::move(sparse_pose_graph_2d); sparse_pose_graph_ = std::move(sparse_pose_graph_2d);
} }
@ -494,8 +505,9 @@ void Node::Initialize() {
} }
if (expect_odometry_data_) { if (expect_odometry_data_) {
odometry_subscriber_ = node_handle_.subscribe( odometry_subscriber_ =
kOdometryTopic, kSubscriberQueueSize, &Node::OdometryMessageCallback, this); node_handle_.subscribe(kOdometryTopic, kSubscriberQueueSize,
&Node::OdometryMessageCallback, this);
expected_sensor_identifiers.insert(kOdometryTopic); expected_sensor_identifiers.insert(kOdometryTopic);
} }
@ -519,18 +531,18 @@ bool Node::HandleSubmapQuery(
return false; return false;
} }
::cartographer::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
// TODO(hrapp): return error messages and extract common code from MapBuilder. // TODO(hrapp): return error messages and extract common code from MapBuilder.
::cartographer::mapping::Submaps* submaps = trajectory_builder_->submaps(); carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
if (request.submap_id < 0 || request.submap_id >= submaps->size()) { if (request.submap_id < 0 || request.submap_id >= submaps->size()) {
return false; return false;
} }
::cartographer::mapping::proto::SubmapQuery::Response response_proto; carto::mapping::proto::SubmapQuery::Response response_proto;
response_proto.set_submap_id(request.submap_id); response_proto.set_submap_id(request.submap_id);
response_proto.set_submap_version( response_proto.set_submap_version(
submaps->Get(request.submap_id)->end_laser_fan_index); submaps->Get(request.submap_id)->end_laser_fan_index);
const std::vector<::cartographer::transform::Rigid3d> submap_transforms = const std::vector<carto::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps); sparse_pose_graph_->GetSubmapTransforms(*submaps);
submaps->SubmapToProto(request.submap_id, submaps->SubmapToProto(request.submap_id,
@ -544,7 +556,7 @@ bool Node::HandleSubmapQuery(
response.height = response_proto.height(); response.height = response_proto.height();
response.resolution = response_proto.resolution(); response.resolution = response_proto.resolution();
auto pose = ::cartographer::transform::ToRigid3(response_proto.slice_pose()); auto pose = carto::transform::ToRigid3(response_proto.slice_pose());
response.slice_pose.position.x = response.slice_pose.position.x =
response_proto.slice_pose().translation().x(); response_proto.slice_pose().translation().x();
response.slice_pose.position.y = response.slice_pose.position.y =
@ -563,10 +575,9 @@ bool Node::HandleSubmapQuery(
} }
void Node::PublishSubmapList(const int64 timestamp) { void Node::PublishSubmapList(const int64 timestamp) {
::cartographer::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
const ::cartographer::mapping::Submaps* submaps = const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
trajectory_builder_->submaps(); const std::vector<carto::transform::Rigid3d> submap_transforms =
const std::vector<::cartographer::transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps); sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size()); CHECK_EQ(submap_transforms.size(), submaps->size());
@ -579,8 +590,7 @@ void Node::PublishSubmapList(const int64 timestamp) {
} }
::cartographer_ros_msgs::SubmapList ros_submap_list; ::cartographer_ros_msgs::SubmapList ros_submap_list;
ros_submap_list.header.stamp = ros_submap_list.header.stamp = ToRos(carto::common::FromUniversal(timestamp));
ToRos(::cartographer::common::FromUniversal(timestamp));
ros_submap_list.header.frame_id = map_frame_; ros_submap_list.header.frame_id = map_frame_;
ros_submap_list.trajectory.push_back(ros_trajectory); ros_submap_list.trajectory.push_back(ros_trajectory);
submap_list_publisher_.publish(ros_submap_list); submap_list_publisher_.publish(ros_submap_list);
@ -588,11 +598,9 @@ void Node::PublishSubmapList(const int64 timestamp) {
} }
void Node::PublishPose(const int64 timestamp) { void Node::PublishPose(const int64 timestamp) {
const ::cartographer::common::Time time = const carto::common::Time time = carto::common::FromUniversal(timestamp);
::cartographer::common::FromUniversal(timestamp);
const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose; const Rigid3d tracking_to_local = trajectory_builder_->pose_estimate().pose;
const ::cartographer::mapping::Submaps* submaps = const carto::mapping::Submaps* submaps = trajectory_builder_->submaps();
trajectory_builder_->submaps();
const Rigid3d local_to_map = const Rigid3d local_to_map =
sparse_pose_graph_->GetLocalToGlobalTransform(*submaps); sparse_pose_graph_->GetLocalToGlobalTransform(*submaps);
const Rigid3d tracking_to_map = local_to_map * tracking_to_local; const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
@ -603,7 +611,7 @@ void Node::PublishPose(const int64 timestamp) {
stamped_transform.child_frame_id = odom_frame_; stamped_transform.child_frame_id = odom_frame_;
if (provide_odom_frame_) { if (provide_odom_frame_) {
::cartographer::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
stamped_transform.transform = ToGeometryMsgTransform(local_to_map); stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
tf_broadcaster_.sendTransform(stamped_transform); tf_broadcaster_.sendTransform(stamped_transform);
@ -637,6 +645,25 @@ void Node::HandleSensorData(const int64 timestamp,
PublishPose(timestamp); PublishPose(timestamp);
} }
auto it = rate_timers_.find(sensor_data->frame_id);
if (it == rate_timers_.end()) {
it = rate_timers_
.emplace(std::piecewise_construct,
std::forward_as_tuple(sensor_data->frame_id),
std::forward_as_tuple(carto::common::FromSeconds(
kSensorDataRatesLoggingPeriodSeconds)))
.first;
}
it->second.Pulse(carto::common::FromUniversal(timestamp));
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
for (const auto& pair : rate_timers_) {
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
}
switch (sensor_data->type) { switch (sensor_data->type) {
case SensorType::kImu: case SensorType::kImu:
AddImu(timestamp, sensor_data->frame_id, sensor_data->imu); AddImu(timestamp, sensor_data->frame_id, sensor_data->imu);
@ -708,8 +735,7 @@ class ScopedRosLogSink : public google::LogSink {
void WaitTillSent() override { void WaitTillSent() override {
if (will_die_) { if (will_die_) {
// Give ROS some time to actually publish our message. // Give ROS some time to actually publish our message.
std::this_thread::sleep_for( std::this_thread::sleep_for(carto::common::FromMilliseconds(1000));
::cartographer::common::FromMilliseconds(1000));
} }
} }