node.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "cartographer_ros/node.h"
18 
19 #include <chrono>
20 #include <string>
21 #include <vector>
22 
23 #include "Eigen/Core"
29 #include "cartographer/mapping/proto/submap_visualization.pb.h"
30 #include "cartographer/mapping/sparse_pose_graph.h"
38 #include "glog/logging.h"
39 #include "nav_msgs/Odometry.h"
40 #include "ros/serialization.h"
41 #include "sensor_msgs/PointCloud2.h"
42 #include "tf2_eigen/tf2_eigen.h"
43 #include "visualization_msgs/MarkerArray.h"
44 
45 namespace cartographer_ros {
46 
47 namespace carto = ::cartographer;
48 
49 using carto::transform::Rigid3d;
50 
51 namespace {
52 
53 constexpr int kInfiniteSubscriberQueueSize = 0;
54 constexpr int kLatestOnlyPublisherQueueSize = 1;
55 
56 // Try to convert 'msg' into 'options'. Returns false on failure.
57 bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
58  TrajectoryOptions* options) {
59  options->tracking_frame = msg.tracking_frame;
60  options->published_frame = msg.published_frame;
61  options->odom_frame = msg.odom_frame;
62  options->provide_odom_frame = msg.provide_odom_frame;
63  options->use_odometry = msg.use_odometry;
64  options->use_laser_scan = msg.use_laser_scan;
65  options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
66  options->num_point_clouds = msg.num_point_clouds;
67  if (!options->trajectory_builder_options.ParseFromString(
68  msg.trajectory_builder_options_proto)) {
69  LOG(ERROR) << "Failed to parse protobuf";
70  return false;
71  }
72  return true;
73 }
74 
75 void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
76  int trajectory_id) {
77  if (subscribers.count(trajectory_id) == 0) {
78  return;
79  }
80  subscribers[trajectory_id].shutdown();
81  LOG(INFO) << "Shutdown the subscriber of ["
82  << subscribers[trajectory_id].getTopic() << "]";
83  CHECK_EQ(subscribers.erase(trajectory_id), 1);
84 }
85 
86 bool IsTopicNameUnique(
87  const string& topic,
88  const std::unordered_map<int, ::ros::Subscriber>& subscribers) {
89  for (auto& entry : subscribers) {
90  if (entry.second.getTopic() == topic) {
91  LOG(ERROR) << "Topic name [" << topic << "] is already used.";
92  return false;
93  }
94  }
95  return true;
96 }
97 
98 } // namespace
99 
100 Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
101  : node_options_(node_options),
102  map_builder_bridge_(node_options_, tf_buffer) {
103  carto::common::MutexLocker lock(&mutex_);
105  node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
106  kSubmapListTopic, kLatestOnlyPublisherQueueSize);
108  node_handle_.advertise<::visualization_msgs::MarkerArray>(
109  kTrajectoryNodesListTopic, kLatestOnlyPublisherQueueSize);
118 
119  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
121  node_handle_.advertise<::nav_msgs::OccupancyGrid>(
122  kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
123  true /* latched */);
125  std::thread(&Node::SpinOccupancyGridThreadForever, this);
126  }
127 
129  node_handle_.advertise<sensor_msgs::PointCloud2>(
130  kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
131 
134  &Node::PublishSubmapList, this));
141 }
142 
144  {
145  carto::common::MutexLocker lock(&mutex_);
146  terminating_ = true;
147  }
148  if (occupancy_grid_thread_.joinable()) {
149  occupancy_grid_thread_.join();
150  }
151 }
152 
154 
155 MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
156 
158  ::cartographer_ros_msgs::SubmapQuery::Request& request,
159  ::cartographer_ros_msgs::SubmapQuery::Response& response) {
160  carto::common::MutexLocker lock(&mutex_);
161  return map_builder_bridge_.HandleSubmapQuery(request, response);
162 }
163 
164 void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
165  carto::common::MutexLocker lock(&mutex_);
166  submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
167 }
168 
169 void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
170  carto::common::MutexLocker lock(&mutex_);
171  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
172  const auto& trajectory_state = entry.second;
173 
174  geometry_msgs::TransformStamped stamped_transform;
175  stamped_transform.header.stamp = ToRos(trajectory_state.pose_estimate.time);
176 
177  const auto& tracking_to_local = trajectory_state.pose_estimate.pose;
178  const Rigid3d tracking_to_map =
179  trajectory_state.local_to_map * tracking_to_local;
180 
181  // We only publish a point cloud if it has changed. It is not needed at high
182  // frequency, and republishing it would be computationally wasteful.
183  if (trajectory_state.pose_estimate.time !=
186  carto::common::ToUniversal(trajectory_state.pose_estimate.time),
187  trajectory_state.trajectory_options.tracking_frame,
188  carto::sensor::TransformPointCloud(
189  trajectory_state.pose_estimate.point_cloud,
190  tracking_to_local.inverse().cast<float>())));
191  last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time;
192  } else {
193  // If we do not publish a new point cloud, we still allow time of the
194  // published poses to advance.
195  stamped_transform.header.stamp = ros::Time::now();
196  }
197 
198  if (trajectory_state.published_to_tracking != nullptr) {
199  if (trajectory_state.trajectory_options.provide_odom_frame) {
200  std::vector<geometry_msgs::TransformStamped> stamped_transforms;
201 
202  stamped_transform.header.frame_id = node_options_.map_frame;
203  stamped_transform.child_frame_id =
204  trajectory_state.trajectory_options.odom_frame;
205  stamped_transform.transform =
206  ToGeometryMsgTransform(trajectory_state.local_to_map);
207  stamped_transforms.push_back(stamped_transform);
208 
209  stamped_transform.header.frame_id =
210  trajectory_state.trajectory_options.odom_frame;
211  stamped_transform.child_frame_id =
212  trajectory_state.trajectory_options.published_frame;
213  stamped_transform.transform = ToGeometryMsgTransform(
214  tracking_to_local * (*trajectory_state.published_to_tracking));
215  stamped_transforms.push_back(stamped_transform);
216 
217  tf_broadcaster_.sendTransform(stamped_transforms);
218  } else {
219  stamped_transform.header.frame_id = node_options_.map_frame;
220  stamped_transform.child_frame_id =
221  trajectory_state.trajectory_options.published_frame;
222  stamped_transform.transform = ToGeometryMsgTransform(
223  tracking_to_map * (*trajectory_state.published_to_tracking));
224  tf_broadcaster_.sendTransform(stamped_transform);
225  }
226  }
227  }
228 }
229 
231  const ::ros::WallTimerEvent& unused_timer_event) {
232  carto::common::MutexLocker lock(&mutex_);
235  map_builder_bridge_.GetTrajectoryNodesList());
236  }
237 }
238 
240  for (;;) {
241  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
242  {
243  carto::common::MutexLocker lock(&mutex_);
244  if (terminating_) {
245  return;
246  }
247  }
249  continue;
250  }
251  const auto occupancy_grid = map_builder_bridge_.BuildOccupancyGrid();
252  if (occupancy_grid != nullptr) {
253  occupancy_grid_publisher_.publish(*occupancy_grid);
254  }
255  }
256 }
257 
259  const cartographer_ros_msgs::SensorTopics& topics) {
260  std::unordered_set<string> expected_sensor_ids;
261 
262  if (options.use_laser_scan) {
263  expected_sensor_ids.insert(topics.laser_scan_topic);
264  }
265  if (options.use_multi_echo_laser_scan) {
266  expected_sensor_ids.insert(topics.multi_echo_laser_scan_topic);
267  }
268  if (options.num_point_clouds > 0) {
269  for (int i = 0; i < options.num_point_clouds; ++i) {
270  string topic = topics.point_cloud2_topic;
271  if (options.num_point_clouds > 1) {
272  topic += "_" + std::to_string(i + 1);
273  }
274  expected_sensor_ids.insert(topic);
275  }
276  }
277  if (options.trajectory_builder_options.trajectory_builder_2d_options()
278  .use_imu_data()) {
279  expected_sensor_ids.insert(topics.imu_topic);
280  }
281  if (options.use_odometry) {
282  expected_sensor_ids.insert(topics.odometry_topic);
283  }
284  return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
285 }
286 
288  const cartographer_ros_msgs::SensorTopics& topics,
289  const int trajectory_id) {
290  if (options.use_laser_scan) {
291  const string topic = topics.laser_scan_topic;
292  laser_scan_subscribers_[trajectory_id] =
293  node_handle_.subscribe<sensor_msgs::LaserScan>(
294  topic, kInfiniteSubscriberQueueSize,
295  boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
296  [this, trajectory_id,
297  topic](const sensor_msgs::LaserScan::ConstPtr& msg) {
298  map_builder_bridge_.sensor_bridge(trajectory_id)
299  ->HandleLaserScanMessage(topic, msg);
300  }));
301  }
302 
303  if (options.use_multi_echo_laser_scan) {
304  const string topic = topics.multi_echo_laser_scan_topic;
305  multi_echo_laser_scan_subscribers_[trajectory_id] =
306  node_handle_.subscribe<sensor_msgs::MultiEchoLaserScan>(
307  topic, kInfiniteSubscriberQueueSize,
308  boost::function<void(
309  const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
310  [this, trajectory_id,
311  topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
312  map_builder_bridge_.sensor_bridge(trajectory_id)
313  ->HandleMultiEchoLaserScanMessage(topic, msg);
314  }));
315  }
316 
317  std::vector<::ros::Subscriber> grouped_point_cloud_subscribers;
318  if (options.num_point_clouds > 0) {
319  for (int i = 0; i < options.num_point_clouds; ++i) {
320  string topic = topics.point_cloud2_topic;
321  if (options.num_point_clouds > 1) {
322  topic += "_" + std::to_string(i + 1);
323  }
324  grouped_point_cloud_subscribers.push_back(node_handle_.subscribe(
325  topic, kInfiniteSubscriberQueueSize,
326  boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
327  [this, trajectory_id,
328  topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
329  map_builder_bridge_.sensor_bridge(trajectory_id)
330  ->HandlePointCloud2Message(topic, msg);
331  })));
332  }
333  point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers;
334  }
335 
336  if (options.trajectory_builder_options.trajectory_builder_2d_options()
337  .use_imu_data()) {
338  string topic = topics.imu_topic;
339  imu_subscribers_[trajectory_id] = node_handle_.subscribe<sensor_msgs::Imu>(
340  topic, kInfiniteSubscriberQueueSize,
341  boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
342  [this, trajectory_id,
343  topic](const sensor_msgs::Imu::ConstPtr& msg) {
344  map_builder_bridge_.sensor_bridge(trajectory_id)
345  ->HandleImuMessage(topic, msg);
346  }));
347  }
348 
349  if (options.use_odometry) {
350  string topic = topics.odometry_topic;
351  odom_subscribers_[trajectory_id] =
352  node_handle_.subscribe<nav_msgs::Odometry>(
353  topic, kInfiniteSubscriberQueueSize,
354  boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
355  [this, trajectory_id,
356  topic](const nav_msgs::Odometry::ConstPtr& msg) {
357  map_builder_bridge_.sensor_bridge(trajectory_id)
358  ->HandleOdometryMessage(topic, msg);
359  }));
360  }
361 
362  is_active_trajectory_[trajectory_id] = true;
363 }
364 
366  if (node_options_.map_builder_options.use_trajectory_builder_2d() &&
367  options.trajectory_builder_options.has_trajectory_builder_2d_options()) {
368  // Only one point cloud source is supported in 2D.
369  if (options.num_point_clouds <= 1) {
370  return true;
371  }
372  }
373  if (node_options_.map_builder_options.use_trajectory_builder_3d() &&
374  options.trajectory_builder_options.has_trajectory_builder_3d_options()) {
375  if (options.num_point_clouds != 0) {
376  return true;
377  }
378  }
379  return false;
380 }
381 
383  const ::cartographer_ros_msgs::SensorTopics& topics,
384  const TrajectoryOptions& options) {
385  if (!IsTopicNameUnique(topics.laser_scan_topic, laser_scan_subscribers_)) {
386  return false;
387  }
388  if (!IsTopicNameUnique(topics.multi_echo_laser_scan_topic,
390  return false;
391  }
392  if (!IsTopicNameUnique(topics.imu_topic, imu_subscribers_)) {
393  return false;
394  }
395  if (!IsTopicNameUnique(topics.odometry_topic, odom_subscribers_)) {
396  return false;
397  }
398  for (auto& subscribers : point_cloud_subscribers_) {
399  string topic = topics.point_cloud2_topic;
400  int count = 0;
401  for (auto& subscriber : subscribers.second) {
402  if (options.num_point_clouds > 1) {
403  topic += "_" + std::to_string(count + 1);
404  ++count;
405  }
406  if (subscriber.getTopic() == topic) {
407  LOG(ERROR) << "Topic name [" << topic << "] is already used";
408  return false;
409  }
410  }
411  }
412  return true;
413 }
414 
416  ::cartographer_ros_msgs::StartTrajectory::Request& request,
417  ::cartographer_ros_msgs::StartTrajectory::Response& response) {
418  carto::common::MutexLocker lock(&mutex_);
419  TrajectoryOptions options;
420  if (!FromRosMessage(request.options, &options) ||
422  LOG(ERROR) << "Invalid trajectory options.";
423  return false;
424  }
425  if (!Node::ValidateTopicName(request.topics, options)) {
426  LOG(ERROR) << "Invalid topics.";
427  return false;
428  }
429 
430  const int trajectory_id = AddTrajectory(options, request.topics);
431  LaunchSubscribers(options, request.topics, trajectory_id);
432  response.trajectory_id = trajectory_id;
433 
434  is_active_trajectory_[trajectory_id] = true;
435  return true;
436 }
437 
439  carto::common::MutexLocker lock(&mutex_);
440  cartographer_ros_msgs::SensorTopics topics;
441  topics.laser_scan_topic = kLaserScanTopic;
442  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
443  topics.point_cloud2_topic = kPointCloud2Topic;
444  topics.imu_topic = kImuTopic;
445  topics.odometry_topic = kOdometryTopic;
446 
447  const int trajectory_id = AddTrajectory(options, topics);
448  LaunchSubscribers(options, topics, trajectory_id);
449  is_active_trajectory_[trajectory_id] = true;
450 }
451 
453  ::cartographer_ros_msgs::FinishTrajectory::Request& request,
454  ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
455  carto::common::MutexLocker lock(&mutex_);
456  const int trajectory_id = request.trajectory_id;
457  if (is_active_trajectory_.count(trajectory_id) == 0) {
458  LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet.";
459  return false;
460  }
461  if (!is_active_trajectory_[trajectory_id]) {
462  LOG(INFO) << "Trajectory_id " << trajectory_id
463  << " has already been finished.";
464  return false;
465  }
466 
467  ShutdownSubscriber(laser_scan_subscribers_, trajectory_id);
468  ShutdownSubscriber(multi_echo_laser_scan_subscribers_, trajectory_id);
469  ShutdownSubscriber(odom_subscribers_, trajectory_id);
470  ShutdownSubscriber(imu_subscribers_, trajectory_id);
471 
472  if (point_cloud_subscribers_.count(trajectory_id) != 0) {
473  for (auto& entry : point_cloud_subscribers_[trajectory_id]) {
474  LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]";
475  entry.shutdown();
476  }
477  CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1);
478  }
479  map_builder_bridge_.FinishTrajectory(trajectory_id);
480  is_active_trajectory_[trajectory_id] = false;
481  return true;
482 }
483 
485  ::cartographer_ros_msgs::WriteAssets::Request& request,
486  ::cartographer_ros_msgs::WriteAssets::Response& response) {
487  carto::common::MutexLocker lock(&mutex_);
488  map_builder_bridge_.SerializeState(request.stem);
489  map_builder_bridge_.WriteAssets(request.stem);
490  return true;
491 }
492 
494  carto::common::MutexLocker lock(&mutex_);
495  for (const auto& entry : is_active_trajectory_) {
496  const int trajectory_id = entry.first;
497  if (entry.second) {
498  map_builder_bridge_.FinishTrajectory(trajectory_id);
499  }
500  }
501 }
502 } // namespace cartographer_ros
::ros::Publisher trajectory_nodes_list_publisher_
Definition: node.h:109
::cartographer::mapping::proto::MapBuilderOptions map_builder_options
Definition: node_options.h:30
void publish(const boost::shared_ptr< M > &message) const
std::vector<::ros::ServiceServer > service_servers_
Definition: node.h:111
void FinishAllTrajectories()
Definition: node.cc:493
void PublishTrajectoryNodesList(const ::ros::WallTimerEvent &timer_event)
Definition: node.cc:230
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
Rigid3< double > Rigid3d
bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics &topics, const TrajectoryOptions &options)
Definition: node.cc:382
bool ValidateTrajectoryOptions(const TrajectoryOptions &options)
Definition: node.cc:365
MapBuilderBridge * map_builder_bridge()
Definition: node.cc:155
void PublishSubmapList(const ::ros::WallTimerEvent &timer_event)
Definition: node.cc:164
constexpr char kFinishTrajectoryServiceName[]
Definition: node.h:47
geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d &rigid3d)
std::thread occupancy_grid_thread_
Definition: node.h:125
constexpr char kImuTopic[]
Definition: node.h:45
cartographer::common::Mutex mutex_
Definition: node.h:104
const NodeOptions node_options_
Definition: node.h:100
constexpr char kTrajectoryNodesListTopic[]
Definition: node.h:54
constexpr char kLaserScanTopic[]
Definition: node.h:42
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
tf2_ros::TransformBroadcaster tf_broadcaster_
Definition: node.h:102
bool HandleStartTrajectory(cartographer_ros_msgs::StartTrajectory::Request &request, cartographer_ros_msgs::StartTrajectory::Response &response)
Definition: node.cc:415
::ros::Publisher scan_matched_point_cloud_publisher_
Definition: node.h:112
void SpinOccupancyGridThreadForever()
Definition: node.cc:239
::ros::Publisher submap_list_publisher_
Definition: node.h:108
std::unordered_map< int,::ros::Subscriber > odom_subscribers_
Definition: node.h:119
::ros::NodeHandle node_handle_
Definition: node.h:107
int count
std::vector<::ros::WallTimer > wall_timers_
Definition: node.h:130
int AddTrajectory(const TrajectoryOptions &options, const cartographer_ros_msgs::SensorTopics &topics)
Definition: node.cc:258
constexpr char kScanMatchedPointCloudTopic[]
Definition: node.h:49
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::unordered_map< int,::ros::Subscriber > imu_subscribers_
Definition: node.h:120
constexpr char kStartTrajectoryServiceName[]
Definition: node.h:52
bool HandleWriteAssets(cartographer_ros_msgs::WriteAssets::Request &request, cartographer_ros_msgs::WriteAssets::Response &response)
Definition: node.cc:484
std::unordered_map< int,::ros::Subscriber > laser_scan_subscribers_
Definition: node.h:117
void sendTransform(const geometry_msgs::TransformStamped &transform)
constexpr char kOdometryTopic[]
Definition: node.h:46
::cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options
cartographer::common::Time last_scan_matched_point_cloud_time_
Definition: node.h:113
sensor_msgs::PointCloud2 ToPointCloud2Message(const int64 timestamp, const string &frame_id, const ::cartographer::sensor::PointCloud &point_cloud)
::ros::NodeHandle * node_handle()
Definition: node.cc:153
constexpr char kOccupancyGridTopic[]
Definition: node.h:48
bool HandleFinishTrajectory(cartographer_ros_msgs::FinishTrajectory::Request &request, cartographer_ros_msgs::FinishTrajectory::Response &response)
Definition: node.cc:452
uint32_t getNumSubscribers() const
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions &options)
Definition: node.cc:438
constexpr char kWriteAssetsServiceName[]
Definition: node.h:53
constexpr char kMultiEchoLaserScanTopic[]
Definition: node.h:43
std::unordered_map< int, std::vector<::ros::Subscriber > > point_cloud_subscribers_
Definition: node.h:122
::ros::Publisher occupancy_grid_publisher_
Definition: node.h:124
constexpr char kSubmapQueryServiceName[]
Definition: node.h:51
Node(const NodeOptions &node_options, tf2_ros::Buffer *tf_buffer)
Definition: node.cc:100
constexpr char kPointCloud2Topic[]
Definition: node.h:44
static Time now()
constexpr char kSubmapListTopic[]
Definition: node.h:50
std::unordered_map< int,::ros::Subscriber > multi_echo_laser_scan_subscribers_
Definition: node.h:118
bool HandleSubmapQuery(cartographer_ros_msgs::SubmapQuery::Request &request, cartographer_ros_msgs::SubmapQuery::Response &response)
Definition: node.cc:157
void LaunchSubscribers(const TrajectoryOptions &options, const cartographer_ros_msgs::SensorTopics &topics, int trajectory_id)
Definition: node.cc:287
::ros::Time ToRos(::cartographer::common::Time time)
void PublishTrajectoryStates(const ::ros::WallTimerEvent &timer_event)
Definition: node.cc:169


cartographer_ros
Author(s):
autogenerated on Mon Jun 10 2019 12:59:40