Pulls out TrajectoryState into MapBuilderBridge. (#197)

master
Damon Kohler 2016-11-29 11:10:52 +01:00 committed by GitHub
parent 33271f0290
commit 102fb4ef4e
4 changed files with 82 additions and 66 deletions

View File

@ -127,12 +127,33 @@ MapBuilderBridge::BuildOccupancyGrid() {
return occupancy_grid; return occupancy_grid;
} }
std::unordered_map<int, MapBuilderBridge::TrajectoryState>
MapBuilderBridge::GetTrajectoryStates() {
std::unordered_map<int, TrajectoryState> trajectory_states;
for (const auto& entry : sensor_bridges_) {
const int trajectory_id = entry.first;
const SensorBridge& sensor_bridge = *entry.second;
const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(trajectory_id);
const cartographer::mapping::TrajectoryBuilder::PoseEstimate
pose_estimate = trajectory_builder->pose_estimate();
if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
continue;
}
trajectory_states[trajectory_id] = {
pose_estimate,
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
*trajectory_builder->submaps()),
sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
options_.published_frame)};
}
return trajectory_states;
}
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get(); return sensor_bridges_.at(trajectory_id).get();
} }
cartographer::mapping::MapBuilder* MapBuilderBridge::map_builder() {
return &map_builder_;
}
} // namespace cartographer_ros } // namespace cartographer_ros

View File

@ -35,6 +35,12 @@ namespace cartographer_ros {
class MapBuilderBridge { class MapBuilderBridge {
public: public:
struct TrajectoryState {
cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
cartographer::transform::Rigid3d local_to_map;
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
};
MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer); MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge(const MapBuilderBridge&) = delete;
@ -51,9 +57,9 @@ class MapBuilderBridge {
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid(); std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
SensorBridge* sensor_bridge(int trajectory_id); SensorBridge* sensor_bridge(int trajectory_id);
cartographer::mapping::MapBuilder* map_builder();
private: private:
const NodeOptions options_; const NodeOptions options_;

View File

@ -179,7 +179,7 @@ void Node::Initialize() {
&Node::PublishSubmapList, this)); &Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(options_.pose_publish_period_sec), ::ros::WallDuration(options_.pose_publish_period_sec),
&Node::PublishPoseAndScanMatchedPointCloud, this)); &Node::PublishTrajectoryStates, this));
} }
bool Node::HandleSubmapQuery( bool Node::HandleSubmapQuery(
@ -206,69 +206,59 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
void Node::PublishPoseAndScanMatchedPointCloud( void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
const ::ros::WallTimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
const carto::mapping::TrajectoryBuilder* trajectory_builder = for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
map_builder_bridge_.map_builder()->GetTrajectoryBuilder(trajectory_id_); const auto& trajectory_state = entry.second;
const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate =
trajectory_builder->pose_estimate();
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
return;
}
const Rigid3d tracking_to_local = last_pose_estimate.pose; geometry_msgs::TransformStamped stamped_transform;
const Rigid3d local_to_map = stamped_transform.header.stamp = ToRos(trajectory_state.pose_estimate.time);
map_builder_bridge_.map_builder()
->sparse_pose_graph()
->GetLocalToGlobalTransform(*trajectory_builder->submaps());
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
geometry_msgs::TransformStamped stamped_transform; const auto& tracking_to_local = trajectory_state.pose_estimate.pose;
stamped_transform.header.stamp = ToRos(last_pose_estimate.time); const Rigid3d tracking_to_map =
trajectory_state.local_to_map * tracking_to_local;
// 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 (last_pose_estimate.time != last_scan_matched_point_cloud_time_) { if (trajectory_state.pose_estimate.time != last_scan_matched_point_cloud_time_) {
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(last_pose_estimate.time), carto::common::ToUniversal(trajectory_state.pose_estimate.time),
options_.tracking_frame, options_.tracking_frame,
carto::sensor::TransformPointCloud( carto::sensor::TransformPointCloud(
last_pose_estimate.point_cloud, trajectory_state.pose_estimate.point_cloud,
tracking_to_local.inverse().cast<float>()))); tracking_to_local.inverse().cast<float>())));
last_scan_matched_point_cloud_time_ = last_pose_estimate.time; last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time;
} else {
// If we do not publish a new point cloud, we still allow time of the
// published poses to advance.
stamped_transform.header.stamp = ros::Time::now();
}
const auto published_to_tracking =
map_builder_bridge_.sensor_bridge(trajectory_id_)
->tf_bridge()
.LookupToTracking(last_pose_estimate.time, options_.published_frame);
if (published_to_tracking != nullptr) {
if (options_.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.odom_frame;
stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id = options_.odom_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform =
ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking));
stamped_transforms.push_back(stamped_transform);
tf_broadcaster_.sendTransform(stamped_transforms);
} else { } else {
stamped_transform.header.frame_id = options_.map_frame; // If we do not publish a new point cloud, we still allow time of the
stamped_transform.child_frame_id = options_.published_frame; // published poses to advance.
stamped_transform.transform = stamped_transform.header.stamp = ros::Time::now();
ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking)); }
tf_broadcaster_.sendTransform(stamped_transform);
if (trajectory_state.published_to_tracking != nullptr) {
if (options_.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = options_.map_frame;
// TODO(damonkohler): 'odom_frame' and 'published_frame' must be
// per-trajectory to fully support the multi-robot use case.
stamped_transform.child_frame_id = options_.odom_frame;
stamped_transform.transform = ToGeometryMsgTransform(trajectory_state.local_to_map);
stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id = options_.odom_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_state.published_to_tracking));
stamped_transforms.push_back(stamped_transform);
tf_broadcaster_.sendTransform(stamped_transforms);
} else {
stamped_transform.header.frame_id = options_.map_frame;
stamped_transform.child_frame_id = options_.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_state.published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform);
}
} }
} }
} }

View File

@ -55,8 +55,7 @@ class Node {
cartographer_ros_msgs::FinishTrajectory::Response& response); cartographer_ros_msgs::FinishTrajectory::Response& response);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void PublishPoseAndScanMatchedPointCloud( void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever(); void SpinOccupancyGridThreadForever();
const NodeOptions options_; const NodeOptions options_;