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

24
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -221,25 +221,25 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
return submap_list;
}
std::unordered_map<int, MapBuilderBridge::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
std::unordered_map<int, TrajectoryState> trajectory_states;
std::unordered_map<int, MapBuilderBridge::LocalTrajectoryData>
MapBuilderBridge::GetLocalTrajectoryData() {
std::unordered_map<int, LocalTrajectoryData> local_trajectory_data;
for (const auto& entry : sensor_bridges_) {
const int trajectory_id = entry.first;
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_);
if (trajectory_state_data_.count(trajectory_id) == 0) {
if (local_slam_data_.count(trajectory_id) == 0) {
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'.
CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
trajectory_states[trajectory_id] = {
local_trajectory_data[trajectory_id] = {
local_slam_data,
map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking(
@ -247,7 +247,7 @@ MapBuilderBridge::GetTrajectoryStates() {
trajectory_options_[trajectory_id].published_frame),
trajectory_options_[trajectory_id]};
}
return trajectory_states;
return local_trajectory_data;
}
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
@ -497,12 +497,12 @@ void MapBuilderBridge::OnLocalSlamResult(
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>) {
std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
std::make_shared<TrajectoryState::LocalSlamData>(
TrajectoryState::LocalSlamData{time, local_pose,
std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
std::make_shared<LocalTrajectoryData::LocalSlamData>(
LocalTrajectoryData::LocalSlamData{time, local_pose,
std::move(range_data_in_local)});
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

11
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -40,8 +40,8 @@ namespace cartographer_ros {
class MapBuilderBridge {
public:
struct TrajectoryState {
// Contains the trajectory state data received from local SLAM, after
struct LocalTrajectoryData {
// Contains the trajectory data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'.
struct LocalSlamData {
@ -79,7 +79,7 @@ class MapBuilderBridge {
std::set<int> GetFrozenTrajectoryIds();
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
EXCLUDES(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList();
@ -98,8 +98,9 @@ class MapBuilderBridge {
cartographer::common::Mutex mutex_;
const NodeOptions node_options_;
std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
trajectory_state_data_ GUARDED_BY(mutex_);
std::unordered_map<int,
std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
local_slam_data_ GUARDED_BY(mutex_);
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
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(
::ros::WallDuration(node_options_.submap_publish_period_sec),
&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),
&Node::PublishTrajectoryStates, this);
&Node::PublishLocalTrajectoryData, this);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));
@ -176,36 +176,36 @@ void Node::AddSensorSamplers(const int trajectory_id,
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_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const auto& trajectory_state = entry.second;
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
const auto& trajectory_data = entry.second;
auto& extrapolator = extrapolators_.at(entry.first);
// We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful.
if (trajectory_state.local_slam_data->time !=
if (trajectory_data.local_slam_data->time !=
extrapolator.GetLastPoseTime()) {
if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time
// information.
carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_state.local_slam_data
->range_data_in_local.returns.size());
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size());
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;
point_time << point, 0.f;
point_cloud.push_back(point_time);
}
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,
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,
trajectory_state.local_slam_data->local_pose);
extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_data.local_slam_data->local_pose);
}
geometry_msgs::TransformStamped stamped_transform;
@ -218,7 +218,7 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) {
stamped_transform.header.stamp = ToRos(now);
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(
carto::transform::Project2D(extrapolator.ExtrapolatePose(now)));
}
@ -226,34 +226,34 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) {
}();
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_state.trajectory_options.provide_odom_frame) {
if (trajectory_data.published_to_tracking != nullptr) {
if (trajectory_data.trajectory_options.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.odom_frame;
trajectory_data.trajectory_options.odom_frame;
stamped_transform.transform =
ToGeometryMsgTransform(trajectory_state.local_to_map);
ToGeometryMsgTransform(trajectory_data.local_to_map);
stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id =
trajectory_state.trajectory_options.odom_frame;
trajectory_data.trajectory_options.odom_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame;
trajectory_data.trajectory_options.published_frame;
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);
tf_broadcaster_.sendTransform(stamped_transforms);
} else {
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_state.trajectory_options.published_frame;
trajectory_data.trajectory_options.published_frame;
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);
}
}

12
cartographer_ros/cartographer_ros/node.h
View File

@ -152,7 +152,7 @@ class Node {
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void AddExtrapolator(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 PublishLandmarkPosesList(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
// they do not fire.
std::vector<::ros::WallTimer> wall_timers_;
// The timer for publishing trajectory states (i.e. pose transforms) is a
// regular timer which is not triggered when simulation time is standing
// still. This prevents overflowing the transform listener buffer by
// publishing the same transforms over and over again.
::ros::Timer publish_trajectory_states_timer_;
// The timer for publishing local trajectory data (i.e. pose transforms and
// range data point clouds) is a regular timer which is not triggered when
// simulation time is standing still. This prevents overflowing the transform
// listener buffer by publishing the same transforms over and over again.
::ros::Timer publish_local_trajectory_data_timer_;
};
} // namespace cartographer_ros