load_bag.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Michael Ferguson
3  * Copyright (C) 2015 Fetch Robotics Inc.
4  * Copyright (C) 2013-2014 Unbounded Robotics Inc.
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
19 // Author: Michael Ferguson
20 
21 #ifndef ROBOT_CALIBRATION_LOAD_BAG_H
22 #define ROBOT_CALIBRATION_LOAD_BAG_H
23 
24 #include <ros/ros.h>
25 #include <rosbag/bag.h>
26 #include <rosbag/view.h>
27 #include <rosbag/query.h>
28 #include <boost/foreach.hpp> // for rosbag iterator
29 
30 #include <std_msgs/String.h>
31 #include <robot_calibration_msgs/CalibrationData.h>
32 
33 namespace robot_calibration
34 {
35 
42 bool load_bag(const std::string& file_name,
43  std_msgs::String& description_msg,
44  std::vector<robot_calibration_msgs::CalibrationData>& data)
45 {
46  // Open the bag file
47  rosbag::Bag bag_;
48  try
49  {
50  bag_.open(file_name, rosbag::bagmode::Read);
51  }
52  catch (rosbag::BagException&)
53  {
54  ROS_FATAL_STREAM("Cannot open " << file_name);
55  return false;
56  }
57 
58  // Get robot_description from bag file
59  rosbag::View model_view_(bag_, rosbag::TopicQuery("robot_description"));
60  if (model_view_.size() < 1)
61  {
62  ROS_FATAL_STREAM("robot_description topic not found in bag file.");
63  return false;
64  }
65  std_msgs::String::ConstPtr description_ = model_view_.begin()->instantiate<std_msgs::String>();
66  description_msg = *description_;
67 
68  // Parse calibration_data topic
69  rosbag::View data_view_(bag_, rosbag::TopicQuery("calibration_data"));
70  BOOST_FOREACH (rosbag::MessageInstance const m, data_view_)
71  {
72  robot_calibration_msgs::CalibrationData::ConstPtr msg = m.instantiate<robot_calibration_msgs::CalibrationData>();
73  data.push_back(*msg);
74  }
75 
76  return true;
77 }
78 
79 } // namespace robot_calibration
80 
81 #endif // ROBOT_CALIBRATION_LOAD_BAG_H
void open(std::string const &filename, uint32_t mode=bagmode::Read)
uint32_t size()
#define ROS_FATAL_STREAM(args)
boost::shared_ptr< T > instantiate() const
Calibration code lives under this namespace.
bool load_bag(const std::string &file_name, std_msgs::String &description_msg, std::vector< robot_calibration_msgs::CalibrationData > &data)
Load a bagfile of calibration data.
Definition: load_bag.h:42
iterator begin()


robot_calibration
Author(s): Michael Ferguson
autogenerated on Tue Nov 3 2020 17:30:30