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

Improve internal naming of local SLAM data. (#908) 

Prevents confusion with TrajectoryState and GetTrajectoryStates()
of the pose graph interface. The affected data is only local.
master
Michael Grupp 2018-06-25 16:07:32 +02:00 committed by gaschler
parent 424e702289
commit c560d05d99
4 changed files with 49 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -221,25 +221,25 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
return submap_list; return submap_list;
} }
std::unordered_map<int, MapBuilderBridge::TrajectoryState> std::unordered_map<int, MapBuilderBridge::LocalTrajectoryData>
MapBuilderBridge::GetTrajectoryStates() { MapBuilderBridge::GetLocalTrajectoryData() {
std::unordered_map<int, TrajectoryState> trajectory_states; std::unordered_map<int, LocalTrajectoryData> local_trajectory_data;
for (const auto& entry : sensor_bridges_) { for (const auto& entry : sensor_bridges_) {
const int trajectory_id = entry.first; const int trajectory_id = entry.first;
const SensorBridge& sensor_bridge = *entry.second; const SensorBridge& sensor_bridge = *entry.second;
std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data; std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data;
{ {
cartographer::common::MutexLocker lock(&mutex_); cartographer::common::MutexLocker lock(&mutex_);
if (trajectory_state_data_.count(trajectory_id) == 0) { if (local_slam_data_.count(trajectory_id) == 0) {
continue; continue;
} }
local_slam_data = trajectory_state_data_.at(trajectory_id); local_slam_data = local_slam_data_.at(trajectory_id);
} }
// Make sure there is a trajectory with 'trajectory_id'. // Make sure there is a trajectory with 'trajectory_id'.
CHECK_EQ(trajectory_options_.count(trajectory_id), 1); CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
trajectory_states[trajectory_id] = { local_trajectory_data[trajectory_id] = {
local_slam_data, local_slam_data,
map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id), map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking( sensor_bridge.tf_bridge().LookupToTracking(
@ -247,7 +247,7 @@ MapBuilderBridge::GetTrajectoryStates() {
trajectory_options_[trajectory_id].published_frame), trajectory_options_[trajectory_id].published_frame),
trajectory_options_[trajectory_id]}; trajectory_options_[trajectory_id]};
} }
return trajectory_states; return local_trajectory_data;
} }
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
@ -497,12 +497,12 @@ void MapBuilderBridge::OnLocalSlamResult(
::cartographer::sensor::RangeData range_data_in_local, ::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping:: const std::unique_ptr<const ::cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>) { TrajectoryBuilderInterface::InsertionResult>) {
std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data = std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
std::make_shared<TrajectoryState::LocalSlamData>( std::make_shared<LocalTrajectoryData::LocalSlamData>(
TrajectoryState::LocalSlamData{time, local_pose, LocalTrajectoryData::LocalSlamData{time, local_pose,
std::move(range_data_in_local)}); std::move(range_data_in_local)});
cartographer::common::MutexLocker lock(&mutex_); cartographer::common::MutexLocker lock(&mutex_);
trajectory_state_data_[trajectory_id] = std::move(local_slam_data); local_slam_data_[trajectory_id] = std::move(local_slam_data);
} }
} // namespace cartographer_ros } // namespace cartographer_ros

11
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -40,8 +40,8 @@ namespace cartographer_ros {
class MapBuilderBridge { class MapBuilderBridge {
public: public:
struct TrajectoryState { struct LocalTrajectoryData {
// Contains the trajectory state data received from local SLAM, after // Contains the trajectory data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated // it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'. // current 'local_pose' at 'time'.
struct LocalSlamData { struct LocalSlamData {
@ -79,7 +79,7 @@ class MapBuilderBridge {
std::set<int> GetFrozenTrajectoryIds(); std::set<int> GetFrozenTrajectoryIds();
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates() std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
EXCLUDES(mutex_); EXCLUDES(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList(); visualization_msgs::MarkerArray GetLandmarkPosesList();
@ -98,8 +98,9 @@ class MapBuilderBridge {
cartographer::common::Mutex mutex_; cartographer::common::Mutex mutex_;
const NodeOptions node_options_; const NodeOptions node_options_;
std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>> std::unordered_map<int,
trajectory_state_data_ GUARDED_BY(mutex_); std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
local_slam_data_ GUARDED_BY(mutex_);
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_; std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
tf2_ros::Buffer* const tf_buffer_; tf2_ros::Buffer* const tf_buffer_;

48
cartographer_ros/cartographer_ros/node.cc
View File

@ -117,9 +117,9 @@ Node::Node(
wall_timers_.push_back(node_handle_.createWallTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec), ::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this)); &Node::PublishSubmapList, this));
publish_trajectory_states_timer_ = node_handle_.createTimer( publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec), ::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishTrajectoryStates, this); &Node::PublishLocalTrajectoryData, this);
wall_timers_.push_back(node_handle_.createWallTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec), ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this)); &Node::PublishTrajectoryNodeList, this));
@ -176,36 +176,36 @@ void Node::AddSensorSamplers(const int trajectory_id,
options.landmarks_sampling_ratio)); options.landmarks_sampling_ratio));
} }
void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
const auto& trajectory_state = entry.second; const auto& trajectory_data = entry.second;
auto& extrapolator = extrapolators_.at(entry.first); auto& extrapolator = extrapolators_.at(entry.first);
// We only publish a point cloud if it has changed. It is not needed at high // We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful. // frequency, and republishing it would be computationally wasteful.
if (trajectory_state.local_slam_data->time != if (trajectory_data.local_slam_data->time !=
extrapolator.GetLastPoseTime()) { extrapolator.GetLastPoseTime()) {
if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) { if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time // TODO(gaschler): Consider using other message without time
// information. // information.
carto::sensor::TimedPointCloud point_cloud; carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_state.local_slam_data point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
->range_data_in_local.returns.size()); .returns.size());
for (const Eigen::Vector3f point : for (const Eigen::Vector3f point :
trajectory_state.local_slam_data->range_data_in_local.returns) { trajectory_data.local_slam_data->range_data_in_local.returns) {
Eigen::Vector4f point_time; Eigen::Vector4f point_time;
point_time << point, 0.f; point_time << point, 0.f;
point_cloud.push_back(point_time); point_cloud.push_back(point_time);
} }
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_state.local_slam_data->time), carto::common::ToUniversal(trajectory_data.local_slam_data->time),
node_options_.map_frame, node_options_.map_frame,
carto::sensor::TransformTimedPointCloud( carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_state.local_to_map.cast<float>()))); point_cloud, trajectory_data.local_to_map.cast<float>())));
} }
extrapolator.AddPose(trajectory_state.local_slam_data->time, extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_state.local_slam_data->local_pose); trajectory_data.local_slam_data->local_pose);
} }
geometry_msgs::TransformStamped stamped_transform; geometry_msgs::TransformStamped stamped_transform;
@ -218,7 +218,7 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) {
stamped_transform.header.stamp = ToRos(now); stamped_transform.header.stamp = ToRos(now);
const Rigid3d tracking_to_local = [&] { const Rigid3d tracking_to_local = [&] {
if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) { if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
return carto::transform::Embed3D( return carto::transform::Embed3D(
carto::transform::Project2D(extrapolator.ExtrapolatePose(now))); carto::transform::Project2D(extrapolator.ExtrapolatePose(now)));
} }
@ -226,34 +226,34 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) {
}(); }();
const Rigid3d tracking_to_map = const Rigid3d tracking_to_map =
trajectory_state.local_to_map * tracking_to_local; trajectory_data.local_to_map * tracking_to_local;
if (trajectory_state.published_to_tracking != nullptr) { if (trajectory_data.published_to_tracking != nullptr) {
if (trajectory_state.trajectory_options.provide_odom_frame) { if (trajectory_data.trajectory_options.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms; std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id = stamped_transform.child_frame_id =
trajectory_state.trajectory_options.odom_frame; trajectory_data.trajectory_options.odom_frame;
stamped_transform.transform = stamped_transform.transform =
ToGeometryMsgTransform(trajectory_state.local_to_map); ToGeometryMsgTransform(trajectory_data.local_to_map);
stamped_transforms.push_back(stamped_transform); stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id = stamped_transform.header.frame_id =
trajectory_state.trajectory_options.odom_frame; trajectory_data.trajectory_options.odom_frame;
stamped_transform.child_frame_id = stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame; trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform( stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_state.published_to_tracking)); tracking_to_local * (*trajectory_data.published_to_tracking));
stamped_transforms.push_back(stamped_transform); stamped_transforms.push_back(stamped_transform);
tf_broadcaster_.sendTransform(stamped_transforms); tf_broadcaster_.sendTransform(stamped_transforms);
} else { } else {
stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id = stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame; trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform( stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_state.published_to_tracking)); tracking_to_map * (*trajectory_data.published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform); tf_broadcaster_.sendTransform(stamped_transform);
} }
} }

12
cartographer_ros/cartographer_ros/node.h
View File

@ -152,7 +152,7 @@ class Node {
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
void PublishTrajectoryStates(const ::ros::TimerEvent& timer_event); void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
@ -208,11 +208,11 @@ class Node {
// We have to keep the timer handles of ::ros::WallTimers around, otherwise // We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire. // they do not fire.
std::vector<::ros::WallTimer> wall_timers_; std::vector<::ros::WallTimer> wall_timers_;
// The timer for publishing trajectory states (i.e. pose transforms) is a // The timer for publishing local trajectory data (i.e. pose transforms and
// regular timer which is not triggered when simulation time is standing // range data point clouds) is a regular timer which is not triggered when
// still. This prevents overflowing the transform listener buffer by // simulation time is standing still. This prevents overflowing the transform
// publishing the same transforms over and over again. // listener buffer by publishing the same transforms over and over again.
::ros::Timer publish_trajectory_states_timer_; ::ros::Timer publish_local_trajectory_data_timer_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros