raw_snapshot.cpp
Go to the documentation of this file.
00001 
00033 #include <ros/ros.h>
00034 #include <ros/topic.h>
00035 #include <rosbag/bag.h>
00036 #include <multisense_ros/DeviceInfo.h>
00037 #include <multisense_ros/RawCamConfig.h>
00038 #include <multisense_ros/RawCamCal.h>
00039 #include <multisense_ros/RawCamData.h>
00040 #include <multisense_ros/RawLidarData.h>
00041 #include <multisense_ros/RawLidarCal.h>
00042 #include <multisense_lib/MultiSenseTypes.hh>
00043 #include <stdio.h>
00044 #include <ros/callback_queue.h>
00045 
00046 #include <dynamic_reconfigure/DoubleParameter.h>
00047 #include <dynamic_reconfigure/StrParameter.h>
00048 #include <dynamic_reconfigure/Reconfigure.h>
00049 #include <dynamic_reconfigure/Config.h>
00050 
00051 namespace { // anonymous
00052 
00053 #define TOPIC_DEVICE_INFO     "/multisense/calibration/device_info"
00054 #define TOPIC_RAW_CAM_CAL     "/multisense/calibration/raw_cam_cal"
00055 #define TOPIC_RAW_CAM_CONFIG  "/multisense/calibration/raw_cam_config"
00056 #define TOPIC_RAW_CAM_DATA    "/multisense/calibration/raw_cam_data"
00057 #define TOPIC_RAW_LIDAR       "/multisense/calibration/raw_lidar_data"
00058 #define TOPIC_RAW_LIDAR_CAL   "/multisense/calibration/raw_lidar_cal"
00059 
00060 //
00061 // Helper class for getting a full rotation of laser scan data
00062 
00063 class LaserHelper {
00064 public:
00065 
00066     LaserHelper() : motion_started_(false) {}
00067 
00068     void callback(const multisense_ros::RawLidarData::ConstPtr& msg)
00069     {
00070         //
00071         // Keep around one scan until the head starts spinning, then
00072         // start collecting
00073 
00074         if (motion_started_ || 0 == scans_.size())
00075             scans_.push_back(msg);
00076 
00077         if (!motion_started_) {
00078             std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it = scans_.begin();
00079 
00080             if (msg->angle_start != (*it)->angle_start) {
00081                 motion_started_ = true;
00082                 scans_.clear();
00083                 scans_.push_back(msg);
00084             }
00085         }
00086     }
00087 
00088     //
00089     // Check if we have received an entire rotation of laser data
00090     // return True if we have a full rotation of data data
00091 
00092     bool done() const
00093     {
00094         std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it;
00095 
00096         //
00097         // If we have no scans, then we're definitely not done
00098 
00099         it = scans_.begin();
00100         if (scans_.end() == it)
00101             return false;
00102 
00103         //
00104         // Calculate the total angle swept through all the scans, accounting for wrap-around
00105 
00106         double previous_start = (*it)->angle_start / 1000000.0;
00107         double total_swept    = 0.0;
00108 
00109         for (it = scans_.begin(); it != scans_.end(); it++) {
00110 
00111             double current_start = (*it)->angle_start / 1000000.0;
00112             double current_swept = current_start - previous_start;
00113 
00114             //
00115             // Unwrap the swept angle
00116 
00117             while(current_swept > M_PI)
00118                 current_swept -= 2*M_PI;
00119             while(current_swept < -M_PI)
00120                 current_swept += 2*M_PI;
00121 
00122             //
00123             // Accumulate
00124 
00125             total_swept += current_swept;
00126 
00127             //
00128             // Save the previous start angle
00129 
00130             previous_start = current_start;
00131         }
00132 
00133         return ( fabs(total_swept) > 2.0 * M_PI);
00134     }
00135 
00136     //
00137     // Gets an entire rotation of laser data.  This method constructs it own subscriber
00138     // and callback queue, and services them itself.
00139     // It will return list of the captured messages for this single rotation
00140 
00141     bool getRotation(std::list<multisense_ros::RawLidarData::ConstPtr>& scan_collection)
00142     {
00143         scans_.clear();
00144         ros::NodeHandle nh;
00145         nh.setCallbackQueue(&queue_);
00146 
00147         ros::Subscriber sub_ = nh.subscribe<multisense_ros::RawLidarData>(TOPIC_RAW_LIDAR, 5,
00148                                             boost::bind(&LaserHelper::callback, this, _1));
00149 
00150         ros::Time start = ros::Time::now();
00151 
00152         //
00153         // Service our own callback queue
00154 
00155         while(1) {
00156 
00157             queue_.callOne(ros::WallDuration(1.0));
00158 
00159             if (done()) {
00160                 scan_collection = scans_;
00161                 return true;
00162             }
00163 
00164             if ((ros::Time::now().toSec() - start.toSec()) >= SCAN_COLLECT_TIMEOUT)
00165                 return false;
00166         }
00167     }
00168 
00169 private:
00170 
00171     static const double SCAN_COLLECT_TIMEOUT = 20.0; // seconds
00172 
00173     bool motion_started_;
00174     ros::CallbackQueue queue_;
00175     std::list<multisense_ros::RawLidarData::ConstPtr> scans_;
00176 };
00177 
00178 void setConf(const dynamic_reconfigure::Config& conf)
00179 {
00180     dynamic_reconfigure::ReconfigureRequest  srv_req;
00181     dynamic_reconfigure::ReconfigureResponse srv_resp;
00182 
00183     srv_req.config = conf;
00184 
00185     ros::service::call("/multisense/set_parameters", srv_req, srv_resp);
00186 }
00187 
00188 void setMotorSpeed(double radPerSec)
00189 {
00190     dynamic_reconfigure::DoubleParameter double_param;
00191     dynamic_reconfigure::Config          conf;
00192 
00193     double_param.name  = "motor_speed";
00194     double_param.value = radPerSec;
00195     conf.doubles.push_back(double_param);
00196 
00197     setConf(conf);
00198 }
00199 
00200 void setResolution(const std::string& res)
00201 {
00202     dynamic_reconfigure::StrParameter str_param;
00203     dynamic_reconfigure::Config       conf;
00204 
00205     str_param.name  = "resolution";
00206     str_param.value = res;
00207     conf.strs.push_back(str_param);
00208 
00209     setConf(conf);
00210 }
00211 
00212 }; // anonymous
00213 
00214 int main(int    argc,
00215          char** argvPP)
00216 {
00217     ros::init(argc, argvPP, "raw_snapshot");
00218     ros::NodeHandle nh;
00219 
00220     if (argc != 2 ||
00221         std::string(argvPP[1]) == "--help" ||
00222         std::string(argvPP[1]) == "-h") {
00223 
00224         printf("Usage: %s <out_bag_filename>\n", argvPP[0]);
00225         return -1;
00226     }
00227 
00228     std::string outfile(argvPP[1]);
00229 
00230     printf("Capturing device information... "); fflush(stdout);
00231     multisense_ros::DeviceInfo::ConstPtr device_info = ros::topic::waitForMessage<multisense_ros::DeviceInfo>(TOPIC_DEVICE_INFO, nh);
00232     if (!device_info) {
00233         printf("  Error capturing DeviceInfo. Exiting\n");
00234         return -1;
00235     }
00236     printf("  Success\n");
00237 
00238     switch(device_info->imagerType) {
00239     case crl::multisense::system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
00240     case crl::multisense::system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
00241       printf("Imager type is CMV2000; setting resolution to 1024x544x128... ");
00242       setResolution("1024x544x128");
00243       break;
00244     case crl::multisense::system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
00245     case crl::multisense::system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
00246       printf("Imager type is CMV4000; setting resolution to 1024x1024x128... ");
00247       setResolution("1024x1024x128");
00248       break;
00249     default:
00250       printf("Imager type is not recognized; not setting resolution... ");
00251       printf("  Error setting imager resolution.  Exiting\n");
00252       return -1;
00253       break;
00254     }
00255     ros::Duration(1.0).sleep();
00256 
00257     printf("Capturing lidar calibration... ");
00258     multisense_ros::RawLidarCal::ConstPtr lidar_cal = ros::topic::waitForMessage<multisense_ros::RawLidarCal>(TOPIC_RAW_LIDAR_CAL, nh);
00259     if (!lidar_cal) {
00260         printf("  Error capturing RawLidarCal. Exiting\n");
00261         return -1;
00262     }
00263     printf("  Success\n");
00264 
00265     printf("Capturing camera calibration... "); fflush(stdout);
00266     multisense_ros::RawCamCal::ConstPtr cam_cal = ros::topic::waitForMessage<multisense_ros::RawCamCal>(TOPIC_RAW_CAM_CAL, nh);
00267     if (!cam_cal) {
00268         printf("  Error capturing RawCamCal. Exiting\n");
00269         return -1;
00270     }
00271     printf("  Success\n");
00272 
00273     printf("Capturing camera config... "); fflush(stdout);
00274     multisense_ros::RawCamConfig::ConstPtr cam_config = ros::topic::waitForMessage<multisense_ros::RawCamConfig>(TOPIC_RAW_CAM_CONFIG, nh);
00275     if (!cam_config) {
00276         printf("  Error capturing RawCamConfig. Exiting\n");
00277         return -1;
00278     }
00279     printf("  Success\n");
00280 
00281     printf("Capturing a single left-rectified/disparity image pair... "); fflush(stdout);
00282     multisense_ros::RawCamData::ConstPtr cam_data = ros::topic::waitForMessage<multisense_ros::RawCamData>(TOPIC_RAW_CAM_DATA, nh);
00283     if (!cam_data) {
00284         printf("  Error capturing RawCamData. Exiting\n");
00285         return -1;
00286     }
00287     printf("  Success\n");
00288 
00289     printf("Capturing a full rotation of lidar scans... "); fflush(stdout);
00290 
00291     setMotorSpeed(0.785);
00292 
00293     LaserHelper laser_helper;
00294     std::list<multisense_ros::RawLidarData::ConstPtr> raw_lidar_data;
00295 
00296     if (false == laser_helper.getRotation(raw_lidar_data)) {
00297         printf("  Error capturing RawLidarData...\n");
00298         return -1;
00299     }
00300     printf("  Captured %zu lidar scans\n", raw_lidar_data.size());
00301 
00302     setMotorSpeed(0.0);
00303 
00304     //
00305     // Save all of the data to a bag file
00306 
00307     printf("Saving data to file [%s]\n", outfile.c_str());
00308     rosbag::Bag bag;
00309     bag.open(outfile, rosbag::bagmode::Write);
00310 
00311     bag.write(TOPIC_DEVICE_INFO, ros::TIME_MIN, *device_info);
00312     bag.write(TOPIC_RAW_LIDAR_CAL, ros::TIME_MIN, *lidar_cal);
00313     bag.write(TOPIC_RAW_CAM_CAL, ros::TIME_MIN, *cam_cal);
00314     bag.write(TOPIC_RAW_CAM_CONFIG, ros::TIME_MIN, *cam_config);
00315     bag.write(TOPIC_RAW_CAM_DATA, ros::TIME_MIN, *cam_data);
00316 
00317     std::list<multisense_ros::RawLidarData::ConstPtr>::const_iterator it;
00318     for (it = raw_lidar_data.begin(); it != raw_lidar_data.end(); it++)
00319         bag.write(TOPIC_RAW_LIDAR, ros::TIME_MIN, **it);
00320 
00321     bag.close();
00322     printf("  Success\n");
00323 
00324     return 0;
00325 }
00326 


multisense_ros
Author(s):
autogenerated on Thu Aug 27 2015 14:01:22