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

Use LocalSlamResultCallback for trajectory states (#594) 

Replaces #556.
Use `LocalSlamResultCallback` googlecartographer/cartographer#574 instead of `PoseEstimate`.
master
Juraj Oršulić 2017-11-16 18:05:05 +01:00 committed by Wally B. Feed
parent a10dea9c11
commit fd52ddf45b
3 changed files with 51 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -65,7 +65,26 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker,
MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(node_options.map_builder_options, {}),
map_builder_(
node_options.map_builder_options,
cartographer::mapping::MapBuilder::LocalSlamResultCallback(
[this](
const int trajectory_id,
const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::NodeId>)
EXCLUDES(mutex_) {
std::shared_ptr<const TrajectoryState::LocalSlamData>
local_slam_data =
std::make_shared<TrajectoryState::LocalSlamData>(
TrajectoryState::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);
})),
tf_buffer_(tf_buffer) {}
void MapBuilderBridge::LoadMap(const std::string& map_filename) {
@ -169,24 +188,22 @@ MapBuilderBridge::GetTrajectoryStates() {
const int trajectory_id = entry.first;
const SensorBridge& sensor_bridge = *entry.second;
const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(trajectory_id);
if (trajectory_builder == nullptr) {
continue;
}
const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate =
trajectory_builder->pose_estimate();
if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
continue;
std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data;
{
cartographer::common::MutexLocker lock(&mutex_);
if (trajectory_state_data_.count(trajectory_id) == 0) {
continue;
}
local_slam_data = trajectory_state_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] = {
pose_estimate,
local_slam_data,
map_builder_.pose_graph()->GetLocalToGlobalTransform(trajectory_id),
sensor_bridge.tf_bridge().LookupToTracking(
pose_estimate.time,
local_slam_data->time,
trajectory_options_[trajectory_id].published_frame),
trajectory_options_[trajectory_id]};
}

16
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -39,7 +39,15 @@ namespace cartographer_ros {
class MapBuilderBridge {
public:
struct TrajectoryState {
cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
// Contains the trajectory state data received from local SLAM, after
// it had processed accumulated 'range_data_in_local' and estimated
// current 'local_pose' at 'time'.
struct LocalSlamData {
::cartographer::common::Time time;
::cartographer::transform::Rigid3d local_pose;
::cartographer::sensor::RangeData range_data_in_local;
};
std::shared_ptr<const LocalSlamData> local_slam_data;
cartographer::transform::Rigid3d local_to_map;
std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
TrajectoryOptions trajectory_options;
@ -62,14 +70,18 @@ class MapBuilderBridge {
cartographer_ros_msgs::SubmapQuery::Response& response);
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
EXCLUDES(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetConstraintList();
SensorBridge* sensor_bridge(int trajectory_id);
private:
cartographer::common::Mutex mutex_;
const NodeOptions node_options_;
std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
trajectory_state_data_ GUARDED_BY(mutex_);
cartographer::mapping::MapBuilder map_builder_;
tf2_ros::Buffer* const tf_buffer_;

14
cartographer_ros/cartographer_ros/node.cc
View File

@ -169,23 +169,25 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
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.pose_estimate.time != extrapolator.GetLastPoseTime()) {
if (trajectory_state.local_slam_data->time !=
extrapolator.GetLastPoseTime()) {
// TODO(gaschler): Consider using other message without time information.
carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_state.pose_estimate.point_cloud.size());
point_cloud.reserve(
trajectory_state.local_slam_data->range_data_in_local.returns.size());
for (const Eigen::Vector3f point :
trajectory_state.pose_estimate.point_cloud) {
trajectory_state.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.pose_estimate.time),
carto::common::ToUniversal(trajectory_state.local_slam_data->time),
node_options_.map_frame,
carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_state.local_to_map.cast<float>())));
extrapolator.AddPose(trajectory_state.pose_estimate.time,
trajectory_state.pose_estimate.pose);
extrapolator.AddPose(trajectory_state.local_slam_data->time,
trajectory_state.local_slam_data->local_pose);
}
geometry_msgs::TransformStamped stamped_transform;