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

Publish Landmark markers for RViz. (#713) 

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-02-08 14:34:58 +01:00 committed by GitHub
parent f5a7e2bb77
commit c2b0143a5e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 63 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -25,9 +25,10 @@
#include "cartographer_ros_msgs/StatusResponse.h"
namespace cartographer_ros {
namespace {
using ::cartographer::transform::Rigid3d;
constexpr double kTrajectoryLineStripMarkerScale = 0.07;
constexpr double kConstraintMarkerScale = 0.025;
@ -55,6 +56,32 @@ visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
return marker;
}
int GetLandmarkIndex(
const std::string& landmark_id,
std::unordered_map<std::string, int>* landmark_id_to_index) {
auto it = landmark_id_to_index->find(landmark_id);
if (it == landmark_id_to_index->end()) {
const int new_index = landmark_id_to_index->size();
landmark_id_to_index->at(landmark_id) = new_index;
return new_index;
}
return it->second;
}
visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
const Rigid3d& landmark_pose,
const std::string& frame_id) {
visualization_msgs::Marker marker;
marker.ns = "Landmarks";
marker.id = landmark_index;
marker.type = visualization_msgs::Marker::SPHERE;
marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = frame_id;
marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
marker.pose = ToGeometryMsgPose(landmark_pose);
return marker;
}
void PushAndResetLineMarker(visualization_msgs::Marker* marker,
std::vector<visualization_msgs::Marker>* markers) {
if (marker->points.size() > 1) {
@ -233,6 +260,18 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
return trajectory_node_list;
}
visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {
visualization_msgs::MarkerArray landmark_poses_list;
const std::map<std::string, Rigid3d> landmark_poses =
map_builder_->pose_graph()->GetLandmarkPoses();
for (const auto& id_to_pose : landmark_poses) {
landmark_poses_list.markers.push_back(CreateLandmarkMarker(
GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
id_to_pose.second, node_options_.map_frame));
}
return landmark_poses_list;
}
visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
visualization_msgs::MarkerArray constraint_list;
int marker_id = 0;
@ -336,8 +375,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
continue;
}
const auto& trajectory_node_pose = node_it->data.global_pose;
const cartographer::transform::Rigid3d constraint_pose =
submap_pose * constraint.pose.zbar_ij;
const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;
constraint_marker->points.push_back(
ToGeometryMsgPoint(submap_pose.translation()));
@ -365,7 +403,7 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
void MapBuilderBridge::OnLocalSlamResult(
const int trajectory_id, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>) {

3
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -81,6 +81,7 @@ class MapBuilderBridge {
std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
EXCLUDES(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList();
visualization_msgs::MarkerArray GetConstraintList();
SensorBridge* sensor_bridge(int trajectory_id);
@ -101,6 +102,8 @@ class MapBuilderBridge {
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
tf2_ros::Buffer* const tf_buffer_;
std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

15
cartographer_ros/cartographer_ros/node.cc
View File

@ -94,6 +94,9 @@ Node::Node(
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
@ -119,6 +122,9 @@ Node::Node(
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this));
@ -250,6 +256,15 @@ void Node::PublishTrajectoryNodeList(
}
}
void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
landmark_poses_list_publisher_.publish(
map_builder_bridge_.GetLandmarkPosesList());
}
}
void Node::PublishConstraintList(
const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);

2
cartographer_ros/cartographer_ros/node.h
View File

@ -151,6 +151,7 @@ class Node {
void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever();
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
@ -169,6 +170,7 @@ class Node {
::ros::NodeHandle node_handle_;
::ros::Publisher submap_list_publisher_;
::ros::Publisher trajectory_node_list_publisher_;
::ros::Publisher landmark_poses_list_publisher_;
::ros::Publisher constraint_list_publisher_;
// These ros::ServiceServers need to live for the lifetime of the node.
std::vector<::ros::ServiceServer> service_servers_;

1
cartographer_ros/cartographer_ros/node_constants.h
View File

@ -37,6 +37,7 @@ constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
constexpr char kConstraintListTopic[] = "constraint_list";
constexpr double kConstraintPublishPeriodSec = 0.5;