diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 87ee608..d55f387 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -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* 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* 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 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) { diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index f53d742..85b4d89 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -81,6 +81,7 @@ class MapBuilderBridge { std::unordered_map 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 map_builder_; tf2_ros::Buffer* const tf_buffer_; + std::unordered_map landmark_to_index_; + // These are keyed with 'trajectory_id'. std::unordered_map trajectory_options_; std::unordered_map> sensor_bridges_; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 47ce8df..f134f54 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -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_); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index ebf4a7a..fa7cd0a 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -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_; diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index de6bf6c..5c0164e 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -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;