raw_snapshot.cpp
Go to the documentation of this file.
1 
33 #include <ros/ros.h>
34 #include <ros/topic.h>
35 #include <rosbag/bag.h>
36 #include <multisense_ros/DeviceInfo.h>
37 #include <multisense_ros/RawCamConfig.h>
38 #include <multisense_ros/RawCamCal.h>
39 #include <multisense_ros/RawCamData.h>
40 #include <multisense_ros/RawLidarData.h>
41 #include <multisense_ros/RawLidarCal.h>
42 #include <multisense_lib/MultiSenseTypes.hh>
43 #include <stdio.h>
44 #include <ros/callback_queue.h>
45 
46 #include <dynamic_reconfigure/DoubleParameter.h>
47 #include <dynamic_reconfigure/StrParameter.h>
48 #include <dynamic_reconfigure/Reconfigure.h>
49 #include <dynamic_reconfigure/Config.h>
50 
51 namespace { // anonymous
52 
53 #define TOPIC_DEVICE_INFO "/multisense/calibration/device_info"
54 #define TOPIC_RAW_CAM_CAL "/multisense/calibration/raw_cam_cal"
55 #define TOPIC_RAW_CAM_CONFIG "/multisense/calibration/raw_cam_config"
56 #define TOPIC_RAW_CAM_DATA "/multisense/calibration/raw_cam_data"
57 #define TOPIC_RAW_LIDAR "/multisense/calibration/raw_lidar_data"
58 #define TOPIC_RAW_LIDAR_CAL "/multisense/calibration/raw_lidar_cal"
59 
60 //
61 // Helper class for getting a full rotation of laser scan data
62 
63 class LaserHelper {
64 public:
65 
66  LaserHelper() : motion_started_(false) {}
67 
68  void callback(const multisense_ros::RawLidarData::ConstPtr& msg)
69  {
70  //
71  // Keep around one scan until the head starts spinning, then
72  // start collecting
73 
74  if (motion_started_ || 0 == scans_.size())
75  scans_.push_back(msg);
76 
77  if (!motion_started_) {
78  std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it = scans_.begin();
79 
80  if (msg->angle_start != (*it)->angle_start) {
81  motion_started_ = true;
82  scans_.clear();
83  scans_.push_back(msg);
84  }
85  }
86  }
87 
88  //
89  // Check if we have received an entire rotation of laser data
90  // return True if we have a full rotation of data data
91 
92  bool done() const
93  {
94  std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it;
95 
96  //
97  // If we have no scans, then we're definitely not done
98 
99  it = scans_.begin();
100  if (scans_.end() == it)
101  return false;
102 
103  //
104  // Calculate the total angle swept through all the scans, accounting for wrap-around
105 
106  double previous_start = (*it)->angle_start / 1000000.0;
107  double total_swept = 0.0;
108 
109  for (it = scans_.begin(); it != scans_.end(); it++) {
110 
111  double current_start = (*it)->angle_start / 1000000.0;
112  double current_swept = current_start - previous_start;
113 
114  //
115  // Unwrap the swept angle
116 
117  while(current_swept > M_PI)
118  current_swept -= 2*M_PI;
119  while(current_swept < -M_PI)
120  current_swept += 2*M_PI;
121 
122  //
123  // Accumulate
124 
125  total_swept += current_swept;
126 
127  //
128  // Save the previous start angle
129 
130  previous_start = current_start;
131  }
132 
133  return ( fabs(total_swept) > 2.0 * M_PI);
134  }
135 
136  //
137  // Gets an entire rotation of laser data. This method constructs it own subscriber
138  // and callback queue, and services them itself.
139  // It will return list of the captured messages for this single rotation
140 
141  bool getRotation(std::list<multisense_ros::RawLidarData::ConstPtr>& scan_collection)
142  {
143  scans_.clear();
144  ros::NodeHandle nh;
145  nh.setCallbackQueue(&queue_);
146 
147  ros::Subscriber sub_ = nh.subscribe<multisense_ros::RawLidarData>(TOPIC_RAW_LIDAR, 5,
148  boost::bind(&LaserHelper::callback, this, _1));
149 
150  ros::Time start = ros::Time::now();
151 
152  //
153  // Service our own callback queue
154 
155  while(1) {
156 
157  queue_.callOne(ros::WallDuration(1.0));
158 
159  if (done()) {
160  scan_collection = scans_;
161  return true;
162  }
163 
164  if ((ros::Time::now().toSec() - start.toSec()) >= SCAN_COLLECT_TIMEOUT)
165  return false;
166  }
167  }
168 
169 private:
170 
171 #if __cplusplus >= 201103
172  static constexpr double SCAN_COLLECT_TIMEOUT = 20.0; // seconds
173 #else
174  static const double SCAN_COLLECT_TIMEOUT = 20.0; // seconds
175 #endif
176 
177  bool motion_started_;
178  ros::CallbackQueue queue_;
179  std::list<multisense_ros::RawLidarData::ConstPtr> scans_;
180 };
181 
182 void setConf(const dynamic_reconfigure::Config& conf)
183 {
184  dynamic_reconfigure::ReconfigureRequest srv_req;
185  dynamic_reconfigure::ReconfigureResponse srv_resp;
186 
187  srv_req.config = conf;
188 
189  ros::service::call("/multisense/set_parameters", srv_req, srv_resp);
190 }
191 
192 void setMotorSpeed(double radPerSec)
193 {
194  dynamic_reconfigure::DoubleParameter double_param;
195  dynamic_reconfigure::Config conf;
196 
197  double_param.name = "motor_speed";
198  double_param.value = radPerSec;
199  conf.doubles.push_back(double_param);
200 
201  setConf(conf);
202 }
203 
204 void setResolution(const std::string& res)
205 {
206  dynamic_reconfigure::StrParameter str_param;
207  dynamic_reconfigure::Config conf;
208 
209  str_param.name = "resolution";
210  str_param.value = res;
211  conf.strs.push_back(str_param);
212 
213  setConf(conf);
214 }
215 
216 }; // anonymous
217 
218 int main(int argc,
219  char** argvPP)
220 {
221  ros::init(argc, argvPP, "raw_snapshot");
222  ros::NodeHandle nh;
223 
224  if (argc != 2 ||
225  std::string(argvPP[1]) == "--help" ||
226  std::string(argvPP[1]) == "-h") {
227 
228  printf("Usage: %s <out_bag_filename>\n", argvPP[0]);
229  return -1;
230  }
231 
232  std::string outfile(argvPP[1]);
233 
234  printf("Capturing device information... "); fflush(stdout);
235  multisense_ros::DeviceInfo::ConstPtr device_info = ros::topic::waitForMessage<multisense_ros::DeviceInfo>(TOPIC_DEVICE_INFO, nh);
236  if (!device_info) {
237  printf(" Error capturing DeviceInfo. Exiting\n");
238  return -1;
239  }
240  printf(" Success\n");
241 
242  switch(device_info->imagerType) {
245  printf("Imager type is CMV2000; setting resolution to 1024x544x128... ");
246  setResolution("1024x544x128");
247  break;
250  printf("Imager type is CMV4000; setting resolution to 1024x1024x128... ");
251  setResolution("1024x1024x128");
252  break;
253  default:
254  printf("Imager type is not recognized; not setting resolution... ");
255  printf(" Error setting imager resolution. Exiting\n");
256  return -1;
257  break;
258  }
259  ros::Duration(1.0).sleep();
260 
261  printf("Capturing lidar calibration... ");
262  multisense_ros::RawLidarCal::ConstPtr lidar_cal = ros::topic::waitForMessage<multisense_ros::RawLidarCal>(TOPIC_RAW_LIDAR_CAL, nh);
263  if (!lidar_cal) {
264  printf(" Error capturing RawLidarCal. Exiting\n");
265  return -1;
266  }
267  printf(" Success\n");
268 
269  printf("Capturing camera calibration... "); fflush(stdout);
270  multisense_ros::RawCamCal::ConstPtr cam_cal = ros::topic::waitForMessage<multisense_ros::RawCamCal>(TOPIC_RAW_CAM_CAL, nh);
271  if (!cam_cal) {
272  printf(" Error capturing RawCamCal. Exiting\n");
273  return -1;
274  }
275  printf(" Success\n");
276 
277  printf("Capturing camera config... "); fflush(stdout);
278  multisense_ros::RawCamConfig::ConstPtr cam_config = ros::topic::waitForMessage<multisense_ros::RawCamConfig>(TOPIC_RAW_CAM_CONFIG, nh);
279  if (!cam_config) {
280  printf(" Error capturing RawCamConfig. Exiting\n");
281  return -1;
282  }
283  printf(" Success\n");
284 
285  printf("Capturing a single left-rectified/disparity image pair... "); fflush(stdout);
286  multisense_ros::RawCamData::ConstPtr cam_data = ros::topic::waitForMessage<multisense_ros::RawCamData>(TOPIC_RAW_CAM_DATA, nh);
287  if (!cam_data) {
288  printf(" Error capturing RawCamData. Exiting\n");
289  return -1;
290  }
291  printf(" Success\n");
292 
293  printf("Capturing a full rotation of lidar scans... "); fflush(stdout);
294 
295  setMotorSpeed(0.785);
296 
297  LaserHelper laser_helper;
298  std::list<multisense_ros::RawLidarData::ConstPtr> raw_lidar_data;
299 
300  if (false == laser_helper.getRotation(raw_lidar_data)) {
301  printf(" Error capturing RawLidarData...\n");
302  return -1;
303  }
304  printf(" Captured %zu lidar scans\n", raw_lidar_data.size());
305 
306  setMotorSpeed(0.0);
307 
308  //
309  // Save all of the data to a bag file
310 
311  printf("Saving data to file [%s]\n", outfile.c_str());
312  rosbag::Bag bag;
313  bag.open(outfile, rosbag::bagmode::Write);
314 
315  bag.write(TOPIC_DEVICE_INFO, ros::TIME_MIN, *device_info);
316  bag.write(TOPIC_RAW_LIDAR_CAL, ros::TIME_MIN, *lidar_cal);
317  bag.write(TOPIC_RAW_CAM_CAL, ros::TIME_MIN, *cam_cal);
318  bag.write(TOPIC_RAW_CAM_CONFIG, ros::TIME_MIN, *cam_config);
319  bag.write(TOPIC_RAW_CAM_DATA, ros::TIME_MIN, *cam_data);
320 
321  std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it;
322  for (it = raw_lidar_data.begin(); it != raw_lidar_data.end(); it++)
323  bag.write(TOPIC_RAW_LIDAR, ros::TIME_MIN, **it);
324 
325  bag.close();
326  printf(" Success\n");
327 
328  return 0;
329 }
330 
#define TOPIC_RAW_LIDAR
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_COLOR
void open(std::string const &filename, uint32_t mode=bagmode::Read)
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_GREY
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define TOPIC_DEVICE_INFO
bool call(const std::string &service_name, MReq &req, MRes &res)
int main(int argc, char **argvPP)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define TOPIC_RAW_CAM_DATA
#define TOPIC_RAW_LIDAR_CAL
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_GREY
void setCallbackQueue(CallbackQueueInterface *queue)
#define TOPIC_RAW_CAM_CAL
#define TOPIC_RAW_CAM_CONFIG
ROSTIME_DECL const Time TIME_MIN
static Time now()
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV2000_COLOR


multisense_ros
Author(s):
autogenerated on Wed Jan 8 2020 03:37:59