From: <gb...@us...> - 2010-08-11 00:50:01
|
Revision: 511 http://gearbox.svn.sourceforge.net/gearbox/?rev=511&view=rev Author: gbiggs Date: 2010-08-11 00:49:53 +0000 (Wed, 11 Aug 2010) Log Message: ----------- Better clock calibration, including skew and drift adjustment. Modified Paths: -------------- gearbox/trunk/src/hokuyo_aist/scan_data.cpp gearbox/trunk/src/hokuyo_aist/scan_data.h gearbox/trunk/src/hokuyo_aist/sensor.cpp gearbox/trunk/src/hokuyo_aist/sensor.h gearbox/trunk/src/hokuyo_aist/sensor_info.cpp gearbox/trunk/src/hokuyo_aist/sensor_info.h gearbox/trunk/src/hokuyo_aist/test/example.cpp gearbox/trunk/src/hokuyo_aist/utils.h Modified: gearbox/trunk/src/hokuyo_aist/scan_data.cpp =================================================================== --- gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/scan_data.cpp 2010-08-11 00:49:53 UTC (rev 511) @@ -31,7 +31,7 @@ /////////////////////////////////////////////////////////////////////////////// ScanData::ScanData() - : ranges_(NULL), intensities_(NULL), ranges_length_(0), + : ranges_(0), intensities_(0), ranges_length_(0), intensities_length_(0), error_(false), laser_time_(0), system_time_(0), model_(MODEL_UNKNOWN), buffers_provided_(false) { @@ -54,7 +54,7 @@ ranges_length_ = rhs.ranges_length(); intensities_length_ = rhs.intensities_length(); if(ranges_length_ == 0) - ranges_ = NULL; + ranges_ = 0; else { try @@ -69,7 +69,7 @@ memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * ranges_length_); } if(intensities_length_ == 0) - intensities_ = NULL; + intensities_ = 0; else { try @@ -96,15 +96,15 @@ { if(!buffers_provided_) { - if (ranges_ != NULL) + if (ranges_ != 0) { delete[] ranges_; - ranges_ = NULL; + ranges_ = 0; } - if (intensities_ != NULL) + if (intensities_ != 0) { delete[] intensities_; - intensities_ = NULL; + intensities_ = 0; } } } @@ -192,14 +192,14 @@ unsigned int rhslength = rhs.ranges_length(); if(rhslength == 0) { - ranges_ = NULL; + ranges_ = 0; ranges_length_ = 0; } else { if(rhslength != ranges_length_) { - if(!buffers_provided_ && ranges_ != NULL) + if(!buffers_provided_ && ranges_ != 0) { // Just copy memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * rhslength); @@ -211,7 +211,7 @@ // self-assignment making a mess). uint32_t* new_data = new uint32_t[rhslength]; memcpy(new_data, rhs.ranges(), sizeof(uint32_t) * rhslength); - if(ranges_ != NULL) + if(ranges_ != 0) delete[] ranges_; ranges_ = new_data; ranges_length_ = rhs.ranges_length(); @@ -227,14 +227,14 @@ rhslength = rhs.intensities_length(); if(rhslength == 0) { - intensities_ = NULL; + intensities_ = 0; intensities_length_ = 0; } else { if(rhslength != intensities_length_) { - if(!buffers_provided_ && intensities_ != NULL) + if(!buffers_provided_ && intensities_ != 0) { // Just copy memcpy(intensities_, rhs.intensities(), @@ -248,7 +248,7 @@ uint32_t* new_data = new uint32_t[rhslength]; memcpy(new_data, rhs.intensities(), sizeof(uint32_t) * rhslength); - if(intensities_ != NULL) + if(intensities_ != 0) delete[] intensities_; intensities_ = new_data; intensities_length_ = rhs.intensities_length(); @@ -284,7 +284,7 @@ { std::stringstream ss; - if(ranges_ != NULL) + if(ranges_ != 0) { ss << ranges_length_ << " ranges from model "; ss << model_to_string(model_) << ":\n"; @@ -292,7 +292,7 @@ ss << ranges_[ii] << '\t'; ss << '\n'; } - if(intensities_ != NULL) + if(intensities_ != 0) { ss << intensities_length_ << " intensities from model "; ss << model_to_string(model_) << ":\n"; @@ -323,12 +323,12 @@ { if(!buffers_provided_) { - if(ranges_ != NULL) + if(ranges_ != 0) delete[] ranges_; - ranges_ = NULL; - if(intensities_ != NULL) + ranges_ = 0; + if(intensities_ != 0) delete[] intensities_; - intensities_ = NULL; + intensities_ = 0; } ranges_length_ = 0; intensities_length_ = 0; @@ -345,7 +345,7 @@ return; // If no data yet, allocate new - if(ranges_ == NULL) + if(ranges_ == 0) { try { @@ -378,7 +378,7 @@ if(include_intensities) { // If no data yet, allocate new - if(intensities_ == NULL) + if(intensities_ == 0) { try { @@ -408,12 +408,12 @@ } // Else data is already allocated to the right length, so do nothing } - else if(intensities_ != NULL) + else if(intensities_ != 0) { // If not told to allocate space for intensity data and it exists, // remove it delete[] intensities_; - intensities_ = NULL; + intensities_ = 0; intensities_length_ = 0; } } @@ -421,7 +421,7 @@ void ScanData::write_range_(unsigned int index, uint32_t value) { - if(ranges_ != NULL) + if(ranges_ != 0) { if(index >= ranges_length_) throw IndexError(); @@ -434,7 +434,7 @@ void ScanData::write_intensity_(unsigned int index, uint32_t value) { - if(intensities_ != NULL) + if(intensities_ != 0) { if(index >= intensities_length_) throw IndexError(); Modified: gearbox/trunk/src/hokuyo_aist/scan_data.h =================================================================== --- gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/scan_data.h 2010-08-11 00:49:53 UTC (rev 511) @@ -60,7 +60,7 @@ /// This constructor uses a provided data buffer rather than allocating /// automatically. /// - /// If the intensity pointer is NULL, no data will be provided of that + /// If the intensity pointer is 0, no data will be provided of that /// type. /// /// @param ranges_buffer A pointer to a data area to store range data @@ -75,7 +75,7 @@ /// only for copy constructor and similar. ScanData(uint32_t* const ranges_buffer, unsigned int ranges_length, - uint32_t* const intensities_buffer=NULL, + uint32_t* const intensities_buffer=0, unsigned int intensities_length=0); /// This copy constructor performs a deep copy of present data. ScanData(ScanData const& rhs); @@ -122,7 +122,7 @@ /// buffers. Instead, it will copy the data into its own buffers. /// If the lhs has provided buffers, it is the caller's responsibility /// to ensure they will be big enough to receive the data from the rhs, - /// except in the case of NULL buffers (no data will be copied for NULL + /// except in the case of 0 buffers (no data will be copied for 0 /// buffers). ScanData& operator=(ScanData const& rhs); /** @brief Subscript operator. Modified: gearbox/trunk/src/hokuyo_aist/sensor.cpp =================================================================== --- gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/sensor.cpp 2010-08-11 00:49:53 UTC (rev 511) @@ -48,9 +48,9 @@ namespace hokuyo_aist { -// SCIP1: 66 bytes (64 bytes of data + line feed + NULL) +// SCIP1: 66 bytes (64 bytes of data + line feed + 0) unsigned int const SCIP1_LINE_LENGTH = 66; -// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + NULL) +// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + 0) unsigned int const SCIP2_LINE_LENGTH = 67; /////////////////////////////////////////////////////////////////////////////// @@ -274,28 +274,30 @@ /////////////////////////////////////////////////////////////////////////////// Sensor::Sensor() - : port_(NULL), err_output_(std::cerr), scip_version_(2), verbose_(false), + : port_(0), err_output_(std::cerr), scip_version_(2), verbose_(false), enable_checksum_workaround_(false), ignore_unknowns_(false), multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0), resolution_(0.0), first_step_(0), last_step_(0), front_step_(0), - max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0) + max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0), + wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0) { } Sensor::Sensor(std::ostream& err_output) - : port_(NULL), err_output_(err_output), scip_version_(2), verbose_(false), + : port_(0), err_output_(err_output), scip_version_(2), verbose_(false), enable_checksum_workaround_(false), ignore_unknowns_(false), multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0), resolution_(0.0), first_step_(0), last_step_(0), front_step_(0), - max_range_(0), time_offset_(0), last_timestamp_(0), wrap_count_(0) + max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0), + wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0) { } Sensor::~Sensor() { - if(port_ != NULL) + if(port_ != 0) delete port_; } @@ -425,13 +427,13 @@ if(verbose_) err_output_ << "Sensor::" << __func__ << "() Closing connection.\n"; delete port_; - port_ = NULL; + port_ = 0; } bool Sensor::is_open() const { - if(port_ != NULL) + if(port_ != 0) return port_->IsOpen(); return false; } @@ -446,14 +448,14 @@ if(verbose_) err_output_ << "Sensor::" << __func__ << "() Turning laser on.\n"; - send_command_("L", "1", 1, NULL); + send_command_("L", "1", 1, 0); } else { if(verbose_) err_output_ << "Sensor::" << __func__ << "() Turning laser off.\n"; - send_command_("L", "0", 1, NULL); + send_command_("L", "0", 1, 0); } skip_lines_(1); } @@ -464,14 +466,14 @@ if(verbose_) err_output_ << "Sensor::" << __func__ << "() Turning laser on.\n"; - send_command_("BM", NULL, 0, "02"); + send_command_("BM", 0, 0, "02"); } else { if(verbose_) err_output_ << "Sensor::" << __func__ << "() Turning laser off.\n"; - send_command_("QT", NULL, 0, "02"); + send_command_("QT", 0, 0, "02"); } skip_lines_(1); } @@ -500,7 +502,7 @@ if(scip_version_ == 1) { // Send the command to change baud rate - send_command_("S", newBaud, 13, NULL); + send_command_("S", newBaud, 13, 0); skip_lines_(1); // Change the port's baud rate reinterpret_cast<flexiport::SerialPort*>(port_)->SetBaudRate(baud); @@ -540,7 +542,7 @@ err_output_ << "Sensor::" << __func__ << "() Setting IP information to $I" << params.str() << '\n'; } - int status = send_command_(&command[0], params.str().c_str(), 48, NULL); + int status = send_command_(&command[0], params.str().c_str(), 48, 0); // Skip the extra line feed skip_lines_(1); if(status != 0) @@ -559,7 +561,7 @@ err_output_ << "Sensor::" << __func__ << "() Resetting laser.\n"; } - send_command_("RS", NULL, 0, NULL); + send_command_("RS", 0, 0, 0); skip_lines_(1); } else @@ -576,7 +578,7 @@ if(verbose_) err_output_ << "Sensor::" << __func__ << "() Resetting laser.\n"; - send_command_("RS", NULL, 0, NULL); + send_command_("RS", 0, 0, 0); skip_lines_(1); } else @@ -664,7 +666,7 @@ void Sensor::get_sensor_info(SensorInfo* info) { - if(info == NULL) + if(info == 0) throw NoDestinationError(); if(scip_version_ == 1) @@ -680,7 +682,7 @@ char buffer[SCIP1_LINE_LENGTH]; memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH); - send_command_("V", NULL, 0, NULL); + send_command_("V", 0, 0, 0); // Get the vendor info line read_line_(buffer); info->vendor = &buffer[5]; // Chop off the "VEND:" tag @@ -722,7 +724,7 @@ // POSIX we get to do it the hard way. // Start by finding the first ( char const* valueStart; - if((valueStart = strchr(info->firmware.c_str(), '(')) == NULL) + if((valueStart = strchr(info->firmware.c_str(), '(')) == 0) { // No bracket? Crud. Fail and use the hard-coded values from // the manual. @@ -796,17 +798,17 @@ // We need to send three commands to get all the info we want: VV, PP // and II - send_command_("VV", NULL, 0, NULL); + send_command_("VV", 0, 0, 0); while(read_line_with_check_(buffer, -1, true) != 0) process_vv_line_(buffer, info); // Next up, PP - send_command_("PP", NULL, 0, NULL); + send_command_("PP", 0, 0, 0); while(read_line_with_check_(buffer, -1, true) != 0) process_pp_line_(buffer, info); // Command II: Revenge of the Commands. - send_command_("II", NULL, 0, NULL); + send_command_("II", 0, 0, 0); while(read_line_with_check_(buffer, -1, true) != 0) process_ii_line_(buffer, info); @@ -839,11 +841,11 @@ if(verbose_) err_output_ << "Sensor::" << __func__ << "() Retrieving time from laser.\n"; - send_command_("TM", "0", 1, NULL); - send_command_("TM", "1", 1, NULL); + send_command_("TM", "0", 1, 0); + send_command_("TM", "1", 1, 0); char buffer[7]; read_line_with_check_(buffer, 6); - send_command_("TM", "2", 1, NULL); + send_command_("TM", "2", 1, 0); skip_lines_(1); // We need to decode the time value that's in the buffer return decode_4_byte_value(buffer); @@ -855,138 +857,92 @@ } -unsigned int Sensor::calibrate_time(unsigned int samples) +long long Sensor::calibrate_time(unsigned int skew_sleep_time, + unsigned int samples) { + if(verbose_) + err_output_ << "Entering timing mode.\n"; enter_timing_mode_(); - std::ofstream df("times.txt", std::ofstream::app); - - // Measure the latency with 1000 plain TM commands - std::vector<unsigned long long> latencies; + // From A. Carballo, Y. Hara, H. Kawata, T. + // Yoshida, A. Ohya, S. Yuta, “Time synchronisation + // between SOKUIKI sensor and host computer using + // timestamps”, Proceedings of the JSME Conference on + // Robotics and Mechatronics ROBOMEC 2007, Akita, + // Japan, 2007, Paper 1P1-K05. + // + // Calibration is performed by calculating the offset between the laser's + // clock and the computer's clock. The algorithm is: + // Offset = Comp time before - + // (Laser time - (Comp time before - Comp time after) / 2) + // This is calculated samples times, and the median taken. + if(verbose_) + err_output_ << "Gathering " << samples << " offset values.\n"; + std::vector<long long> offsets; for(unsigned int ii = 0; ii < samples; ii++) { - unsigned long long start_time(get_computer_time_()); - get_timing_mode_time_(""); - unsigned long long end_time(get_computer_time_()); - latencies.push_back(end_time - start_time); + unsigned long long end_time(0); + unsigned long long start_time = get_computer_time_(); + unsigned long long laser_time = + wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6; + offsets.push_back(start_time - + (laser_time - (end_time - start_time) / 2)); } - // Calculate the median two-way latency - unsigned long long two_way_latency = median(latencies); - df << two_way_latency << '\t'; + // Calculate the median offset + time_offset_ = median(offsets); + if(verbose_) + err_output_ << "Calculated offset is " << time_offset_ << '\n'; - // Measure the latency with 1000 TM commands + 32 bytes - std::string bytes = "12345678901234567890123456789012"; - latencies.clear(); - for(unsigned int ii = 0; ii < samples; ii++) + if(skew_sleep_time > 0) { - unsigned long long start_time(get_computer_time_()); - get_timing_mode_time_(bytes); - unsigned long long end_time(get_computer_time_()); - latencies.push_back(end_time - start_time); - } - // Calculate the median two-way latency - unsigned long long latency_32bytes = median(latencies); - df << latency_32bytes << '\t'; - // Subtract the no-data latency to remove the command transmission time - latency_32bytes -= two_way_latency; - df << latency_32bytes << '\t'; - // Divide by 32 to get the per-byte transmission time - unsigned long long one_byte_latency32 = latency_32bytes / 32; - df << one_byte_latency32 << '\t'; + // Sleep, then do it again to approximate a skew line + struct timespec sleep_time = {0, 0}; + sleep_time.tv_sec = skew_sleep_time; + if(verbose_) + err_output_ << "Sleeping for " << skew_sleep_time << "s.\n"; + nanosleep(&sleep_time, NULL); - // Measure the latency with 1000 TM commands + 64 bytes - bytes = "1234567890123456789012345678901234567890123456789012345678901234"; - latencies.clear(); - for(unsigned int ii = 0; ii < samples; ii++) - { - unsigned long long start_time(get_computer_time_()); - get_timing_mode_time_(bytes); - unsigned long long end_time(get_computer_time_()); - latencies.push_back(end_time - start_time); + if(verbose_) + err_output_ << "Gathering " << samples << " offset values.\n"; + offsets.clear(); + for(unsigned int ii = 0; ii < samples; ii++) + { + unsigned long long end_time(0); + unsigned long long start_time = get_computer_time_(); + unsigned long long laser_time = + wrap_timestamp_(get_timing_mode_time_(&end_time)) * 1e6; + offsets.push_back(start_time - + (laser_time - (end_time - start_time) / 2)); + } + // Calculate the median offset + long long offset2 = median(offsets); + if(verbose_) + { + err_output_ << "Calculated second offset is " << offset2 << + '\n'; + } + + // Approximate the line slope as (offset2 - offset1) / sleep_time + time_skew_alpha_ = (offset2 - time_offset_) / + static_cast<double>(skew_sleep_time * 1e9); + if(verbose_) + { + err_output_ << "Calculated alpha is " << time_skew_alpha_ << '\n'; + } } - // Calculate the median two-way latency - unsigned long long latency_64bytes = median(latencies); - df << latency_64bytes << '\t'; - // Subtract the no-data latency to remove the command transmission time - latency_64bytes -= two_way_latency; - df << latency_64bytes << '\t'; - // Divide by 32 to get the per-byte transmission time - unsigned long long one_byte_latency64 = latency_64bytes / 64; - df << one_byte_latency64 << '\t'; - // Get the time from the laser - // To get the most accurate result, we cannot do any processing while - // receiving. We want to know the time that reception finished as exactly - // as possible. To achieve this, we do not use send_command_, but instead - // send the command manually and receive the entire expected reply at once, - // then check/decode it later. - char response[17]; - if(port_->Write("TM1\n", 4) < 4) - throw WriteError(19); - unsigned int line_length = port_->ReadLine(response, 16); - unsigned long long end_time(get_computer_time_()); - df << end_time << '\t'; - // Process the response to confirm it is correct - if(line_length < 0) - throw ReadError(0); - else if(line_length == 0) - throw ReadError(1); - else if(line_length < 15) - throw LineLengthError(line_length, 15); - response[line_length - 1] = '\0'; - if(response[0] != 'T' || response[1] != 'M' || response[2] != '1') - throw CommandEchoError("TM", response); - response[7] = '\0'; - if(response[4] != '0' || response[5] != '0' || response[6] != 'P') - throw ResponseError(response, "TM"); - // Check the checksum on the time stamp is accurate - confirm_checksum_(&response[8], 4, response[12]); - // Decode the time stamp - unsigned int timestamp = - wrap_timestamp_(decode_4_byte_value(&response[8])); - df << timestamp << '\t'; - // The laser had to send 15 bytes to send back the time stamp. This means - // that there was approximately a 15 byte delay between the time stamp's - // occurance in real time and when we finished receiving the data. - // The offset from computer time to laser time is: - // (computer time - comms time) - laser time - // Not forgetting that the computer times need to be converted to - // milliseconds. - time_offset_ = ((end_time - (one_byte_latency64 * 15)) / 1e6) - timestamp; - df << time_offset_ << '\n'; - // All done. + if(verbose_) + err_output_ << "Leaving timing mode.\n"; leave_timing_mode_(); - df.close(); return time_offset_; } -unsigned int Sensor::wrap_timestamp_(unsigned int timestamp) -{ - unsigned int result; - if(timestamp < last_timestamp_) - { - wrap_count_++; - result = timestamp + wrap_count_ * 0x1000; // 24-bit value + 1 - } - else - result = timestamp; - last_timestamp_ = timestamp; - return result; -} - - -unsigned int Sensor::offset_timestamp_(unsigned int timestamp) -{ - return timestamp + time_offset_; -} - - unsigned int Sensor::get_ranges(ScanData* data, int start_step, int end_step, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); char buffer[11]; @@ -1013,7 +969,7 @@ number_to_string(start_step, buffer, 3); number_to_string(end_step, &buffer[3], 3); number_to_string(cluster_count, &buffer[6], 2); - send_command_("G", buffer, 8, NULL); + send_command_("G", buffer, 8, 0); // In SCIP1 mode we're going to get back 2-byte data read_2_byte_range_data_(data, num_steps); } @@ -1025,15 +981,16 @@ number_to_string(end_step, &buffer[4], 4); number_to_string(cluster_count, &buffer[8], 2); if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) - send_command_("HD", buffer, 10, NULL); + send_command_("HD", buffer, 10, 0); else - send_command_("GD", buffer, 10, NULL); + send_command_("GD", buffer, 10, 0); // There will be a timestamp before the data (if there is data) // Normally we would send 6 for the expected length, but we may get no // timestamp back if there was no data. if(read_line_with_check_(buffer) == 0) throw NoDataError(); - data->laser_time_ = decode_4_byte_value(buffer); + data->laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset_(start_step); data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_)); // In SCIP2 mode we're going to get back 3-byte data because we're // sending the GD command @@ -1049,7 +1006,7 @@ unsigned int Sensor::get_ranges_by_angle(ScanData* data, double start_angle, double end_angle, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDataError(); // Calculate the given angles in steps, rounding towards front_step_ @@ -1078,7 +1035,7 @@ unsigned int Sensor::get_ranges_intensities(ScanData* data, int start_step, int end_step, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); char buffer[11]; @@ -1109,15 +1066,16 @@ number_to_string(end_step, &buffer[4], 4); number_to_string(cluster_count, &buffer[8], 2); if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) - send_command_("HE", buffer, 10, NULL); + send_command_("HE", buffer, 10, 0); else - send_command_("GE", buffer, 10, NULL); + send_command_("GE", buffer, 10, 0); // There will be a timestamp before the data (if there is data) // Normally we would send 6 for the expected length, but we may get no // timestamp back if there was no data. if(read_line_with_check_(buffer) == 0) throw NoDataError(); - data->laser_time_ = decode_4_byte_value(buffer); + data->laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset_(start_step); data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_)); // In SCIP2 mode we're going to get back 3-byte data because we're // sending the GE command @@ -1130,7 +1088,7 @@ unsigned int Sensor::get_ranges_intensities_by_angle(ScanData* data, double start_angle, double end_angle, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDataError(); // Calculate the given angles in steps, rounding towards front_step_ @@ -1159,7 +1117,7 @@ unsigned int Sensor::get_new_ranges(ScanData* data, int start_step, int end_step, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); if(scip_version_ == 1) @@ -1197,7 +1155,7 @@ command[0] = 'M'; command[1] = 'D'; command[2] = '\0'; - send_command_(command, buffer, 13, NULL); + send_command_(command, buffer, 13, 0); // Mx commands will perform a scan, then send the data prefixed with // another command echo. // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes) @@ -1234,7 +1192,8 @@ // timestamp back if there was no data. if(read_line_with_check_(buffer) == 0) throw NoDataError(); - data->laser_time_ = decode_4_byte_value(buffer); + data->laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset_(start_step); data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_)); // In SCIP2 mode we're going to get back 3-byte data because we're // sending the MD command @@ -1247,7 +1206,7 @@ unsigned int Sensor::get_new_ranges_by_angle(ScanData* data, double start_angle, double end_angle, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); if(scip_version_ == 1) throw UnsupportedError(16); @@ -1278,7 +1237,7 @@ unsigned int Sensor::get_new_ranges_intensities(ScanData* data, int start_step, int end_step, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); if(scip_version_ == 1) @@ -1317,7 +1276,7 @@ command[0] = 'M'; command[1] = 'E'; command[2] = '\0'; - send_command_(command, buffer, 13, NULL); + send_command_(command, buffer, 13, 0); // Mx commands will perform a scan, then send the data prefixed with // another command echo. // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes) @@ -1354,7 +1313,8 @@ // timestamp back if there was no data. if(read_line_with_check_(buffer) == 0) throw NoDataError(); - data->laser_time_ = decode_4_byte_value(buffer); + data->laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset_(start_step); data->system_time_ = offset_timestamp_(wrap_timestamp_(data->laser_time_)); // In SCIP2 mode we're going to get back 3-byte data because we're // sending the ME command @@ -1367,7 +1327,7 @@ unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData* data, double start_angle, double end_angle, unsigned int cluster_count) { - if(data == NULL) + if(data == 0) throw NoDestinationError(); if(scip_version_ == 1) throw UnsupportedError(17); @@ -1438,12 +1398,12 @@ // If expected_length is not -1, it should include the terminating line feed but -// not the NULL (although the buffer still has to include this). +// not the 0 (although the buffer still has to include this). // If expected_length is -1, this function expects buffer to be a certain length // to allow up to the maximum line length to be read. See SCIP1_LINE_LENGTH and // SCIP2_LINE_LENGTH. -// The line feed that terminates a line will be replaced with a NULL. -// The return value is the number of bytes received, not including the NULL +// The line feed that terminates a line will be replaced with a 0. +// The return value is the number of bytes received, not including the 0 // byte or the line feed. int Sensor::read_line_(char* buffer, int expected_length) { @@ -1463,7 +1423,7 @@ throw ReadError(0); else if(linelength == 0) throw ReadError(1); - // Replace the line feed with a NULL + // Replace the line feed with a 0 buffer[linelength - 1] = '\0'; } else @@ -1473,14 +1433,14 @@ err_output_ << "Sensor::" << __func__ << "() Reading exactly " << expected_length << " bytes.\n"; } - // expected_length+1 for the NULL + // expected_length+1 for the 0 if((linelength = port_->ReadLine(buffer, expected_length + 1)) < 0) throw ReadError(0); else if(linelength == 0) throw ReadError(1); else if(linelength < expected_length) throw LineLengthError(linelength, expected_length); - // Replace the line feed with a NULL + // Replace the line feed with a 0 buffer[linelength - 1] = '\0'; } @@ -1552,7 +1512,7 @@ { // Here comes the UTM-30LX workaround char* hasComment = strstr(buffer, "<-"); - if(hasComment != NULL) + if(hasComment != 0) { int newBytesToConsider = hasComment - buffer; if(verbose_) @@ -1602,7 +1562,7 @@ // command and parameters sent are correct, and that the returned status code // is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two // bytes of extra_ok (for SCIP2). -// cmd must be a 1 byte string for SCIP1 and a 2-byte NULL-terminated string +// cmd must be a 1 byte string for SCIP1 and a 2-byte 0-terminated string // for SCIP2. // If param_length is 0, no parameters will be sent or expected in the reply. // extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2. @@ -1672,7 +1632,7 @@ } if(response[statusIndex] != '0') { - if(extra_ok != NULL) + if(extra_ok != 0) { if(response[statusIndex] != extra_ok[0]) { @@ -1737,7 +1697,7 @@ if(!(response[0] == '0' && response[1] == '0') && !(response[0] == '9' && response[1] == '9')) { - if(extra_ok != NULL) + if(extra_ok != 0) { if(response[0] != extra_ok[0] || response[1] != extra_ok[1]) { @@ -1768,25 +1728,54 @@ /// Puts the laser into the timing mode. void Sensor::enter_timing_mode_() { - send_command_("TM", "0", 1, NULL); + send_command_("TM", "0", 1, 0); + skip_lines_(1); } +/// Take the laser out of timing mode. +void Sensor::leave_timing_mode_() +{ + send_command_("TM", "2", 1, 0); + skip_lines_(1); +} + + /// Get the timestamp from the laser. -unsigned int Sensor::get_timing_mode_time_(std::string const& data) +/// If @ref reception_time is not 0, it will be filled with the time at +/// which data reception was completed. +unsigned int Sensor::get_timing_mode_time_(unsigned long long* reception_time) { - std::string command = "TM1"; - if(data != "") - { - send_command_((command + data).c_str(), NULL, - command.size() + data.size(), NULL); - } - else - send_command_("TM", "1", 1, NULL); - - char buffer[7]; - read_line_with_check_(buffer, 6); - return decode_4_byte_value(buffer); + // To get the most accurate result, we cannot do any processing while + // receiving. We want to know the time that reception finished as exactly + // as possible. To achieve this, we do not use send_command_, but instead + // send the command manually and receive the entire expected reply at once, + // then check/decode it later. + char response[17]; + if(port_->Write("TM1\n", 4) < 4) + throw WriteError(19); + unsigned int line_length = port_->Read(response, 16); + if(reception_time) + *reception_time = get_computer_time_(); + // Process the response to confirm it is correct + if(line_length < 0) + throw ReadError(0); + else if(line_length == 0) + throw ReadError(1); + else if(line_length < 15) + throw LineLengthError(line_length, 15); + response[line_length - 1] = '\0'; + if(response[0] != 'T' || response[1] != 'M' || response[2] != '1') + throw CommandEchoError("TM", response); + response[7] = '\0'; + if(response[4] != '0' || response[5] != '0' || response[6] != 'P') + throw ResponseError(response, "TM"); + // Check the checksum on the time stamp is accurate + confirm_checksum_(&response[8], 4, response[12]); + // Decode the time stamp + unsigned int timestamp = + decode_4_byte_value(&response[8]); + return timestamp; } @@ -1799,37 +1788,59 @@ return ts.tv_sec * 1e9 + ts.tv_nsec; #else struct timeval tv; - gettimeofday(&tv, NULL); + gettimeofday(&tv, 0); return tv.tv_sec * 1e9 + tv.tv_usec * 1e3; #endif } -/// Take the laser out of timing mode. -void Sensor::leave_timing_mode_() +/// timestamp must be in milliseconds. The result is in milliseconds. +unsigned int Sensor::wrap_timestamp_(unsigned int timestamp) { - send_command_("TM", "2", 1, NULL); - skip_lines_(1); + if(timestamp < last_timestamp_) + { + wrap_count_++; + } + last_timestamp_ = timestamp; + + return timestamp + wrap_count_ * 0x01000000; // 24-bit value + 1 } +/// timestamp must be in milliseconds. The result is in nanoseconds. +unsigned long long Sensor::offset_timestamp_(unsigned int timestamp) +{ + return ((1 - time_drift_rate_) * timestamp * 1e6 + time_offset_) / + (1 - time_skew_alpha_); +} + + +unsigned int Sensor::step_to_time_offset_(int start_step) +{ + if(start_step < 0) + return first_step_ * time_resolution_; + else + return start_step * time_resolution_; +} + + /// Search a string for the laser's model. void Sensor::find_model_(char const* buffer) { - if(strstr(buffer, "URG-04LX") != NULL) + if(strstr(buffer, "URG-04LX") != 0) model_ = MODEL_URG04LX; - else if(strstr(buffer, "UBG-04LX") != NULL) + else if(strstr(buffer, "UBG-04LX") != 0) model_ = MODEL_UBG04LXF01; - else if(strstr(buffer, "UHG-08LX") != NULL) + else if(strstr(buffer, "UHG-08LX") != 0) model_ = MODEL_UHG08LX; - else if(strstr(buffer, "UTM-30LX") != NULL) + else if(strstr(buffer, "UTM-30LX") != 0) { model_ = MODEL_UTM30LX; // Also enable the work around for a checksum problem in this // model. enable_checksum_workaround_ = true; } - else if(strstr(buffer, "UXM-30LX") != NULL) + else if(strstr(buffer, "UXM-30LX") != 0) model_ = MODEL_UXM30LXE; else model_ = MODEL_UNKNOWN; @@ -1847,7 +1858,7 @@ // Try SCIP version 2 first by sending an info command try { - send_command_("VV", NULL, 0, NULL); + send_command_("VV", 0, 0, 0); } catch(BaseError) { @@ -1867,7 +1878,7 @@ port_->Flush(); try { - send_command_("V", NULL, 0, NULL); + send_command_("V", 0, 0, 0); } catch(BaseError) { @@ -1887,7 +1898,7 @@ // different firmware version format, that doesn't matter because they // don't support SCIP v1 and so shouldn't get to this point anyway - if // they do, it's an uncaught error. - int majorVer = strtol(&buffer[5], NULL, 10); + int majorVer = strtol(&buffer[5], 0, 10); if(errno == ERANGE) throw FirmwareError(); if(verbose_) @@ -1921,7 +1932,7 @@ // first byte as the command and the other 6 as parameters. try { - send_command_("S", "CIP2.0", 6, NULL); + send_command_("S", "CIP2.0", 6, 0); } catch(BaseError) { @@ -2032,7 +2043,7 @@ * arrived yet, so don't know what to look for. else if(strncmp(buffer, "", 4) == 0) { - if(strstr(buffer, "CCW") != NULL) + if(strstr(buffer, "CCW") != 0) info->rot_dir = COUNTERCLOCKWISE; else info->rot_dir = CLOCKWISE; @@ -2064,6 +2075,7 @@ throw ParseError(buffer, "Motor speed"); } info->speed_level = 0; + time_resolution_ = info->speed; } else { @@ -2074,6 +2086,7 @@ { throw ParseError(buffer, "Motor speed"); } + time_resolution_ = info->speed; } } else if(strncmp(buffer, "MESM", 4) == 0) Modified: gearbox/trunk/src/hokuyo_aist/sensor.h =================================================================== --- gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/sensor.h 2010-08-11 00:49:53 UTC (rev 511) @@ -218,12 +218,62 @@ approximately every 9.5 hours. This class will detect wrapped time stamps and adjust them accordingly. + @param skew_sleep_time Whether to approximate the laser clock skew. + This requires sleeping for the specified time, then calibrating again. + The results of the two calibrations are used to approximate the clock + skew as a straight line between the two points. This is very + approximate; in order to overcome noise in the signal you will need to + calibrate over a period of up to several minutes. Set this to 0 to not + approximate the skew. @param samples The number of samples to use in calculating latencies. - @return The calculated offset in milliseconds. + @return The calculated offset in nanoseconds. */ - unsigned int calibrate_time(unsigned int samples=10); + long long calibrate_time(unsigned int skew_sleep_time=0, + unsigned int samples=10); + /// Retrieve the calculated time offset (0 if not calibrated). + long long time_offset() const { return time_offset_; } + + /// Set the time offset (if the calculated value is bad). + void set_time_offset(long long time_offset) + { time_offset_ = time_offset; } + /// Retrieve the current clock drift rate (0 if not set). + float drift_rate() const { return time_drift_rate_; } + /** Set the current clock drift rate. + + The drift rate is used when correcting laser time stamps to computer + time. It is factored into the equation as: + Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha) + where Tc is the computer time and Tl is the laser time. See @ref + set_skew_alpha for the values of alpha and beta. + + If the drift rate is zero, it means the laser's clock does not drift. + This is extremely unlikely, but the laser's drift may not matter for + your application. + + Drift should usually be provided by the manufacturer. If it is not, + you can calculate it by calibrating the laser many times over a long + period and looking at the change in the calculated offset. */ + void set_drift_rate(float drift_rate) + { time_drift_rate_ = drift_rate; } + + /// Get the calculated skew line slope (default: 0). + float skew_alpha() const { return time_skew_alpha_; } + /** Set a skew line slope value. + + The skew is used when correcting laser time stamps to computer time. + It is factored into the equation as: + Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha) + where Tc is the computer time and Tl is the laser time. See @ref + drift_rate for the value of the drift. Beta, the line's y crossing, is + assumed to be 0 since we always use a two-point line fit instead of + something like least squares. + + The effect of this is to cancel out the skew caused by the different + frequencies of the computer clock and the laser clock. */ + void set_skew_alpha(float alpha) { time_skew_alpha_ = alpha; } + /** @brief Get the latest scan data from the scanner. This function requires a pointer to a @ref ScanData object. It will @@ -444,8 +494,20 @@ double min_angle_, max_angle_, resolution_; int first_step_, last_step_, front_step_; unsigned int max_range_; - unsigned int time_offset_; - unsigned int last_timestamp_, wrap_count_; + /// The time between two points in a scan, in milliseconds. + unsigned int time_resolution_; + /// The offset from the laser's clock to the computer's clock in + /// nanoseconds. + long long time_offset_; + /// The previous received timestamp from the laser, in laser time and + /// in milliseconds. + unsigned int last_timestamp_; + /// The number of times the laser's clock has wrapped. + unsigned int wrap_count_; + /// The drift rate of the laser's clock + float time_drift_rate_; + /// The clock skew alpha value. + float time_skew_alpha_; void clear_read_buffer_(); int read_line_(char* buffer, int expected_length = -1); @@ -455,12 +517,22 @@ int send_command_(char const* cmd, char const* param, int param_length, char const* extra_ok); + public: void enter_timing_mode_(); - unsigned int get_timing_mode_time_(std::string const& data); + void leave_timing_mode_(); + /// Get the laser time in milliseconds. + unsigned int get_timing_mode_time_(unsigned long long* reception_time=0); + /// Get the computer time in nanoseconds. unsigned long long get_computer_time_(); - void leave_timing_mode_(); + /// Adjust a wrapped laser timestamp, in milliseconds. unsigned int wrap_timestamp_(unsigned int timestamp); - unsigned int offset_timestamp_(unsigned int timestamp); + /// Offset a laser timestamp, in milliseconds, into computer time, in + /// nanoseconds. + unsigned long long offset_timestamp_(unsigned int timestamp); + private: + /// Convert a step value into its time offset from the start of a scan, + /// in milliseconds. + unsigned int step_to_time_offset_(int start_step); void find_model_(char const* buffer); void get_and_set_scip_version_(); Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.cpp =================================================================== --- gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/sensor_info.cpp 2010-08-11 00:49:53 UTC (rev 511) @@ -32,7 +32,7 @@ : min_range(0), max_range(0), steps(0), first_step(0), last_step(0), front_step(0), standard_speed(0), rot_dir(COUNTERCLOCKWISE), power(false), speed(0), speed_level(0), baud(0), time(0), min_angle(0.0), max_angle(0.0), - resolution(0.0), scanable_steps(0), max_step(0), + resolution(0.0), time_resolution(0), scanable_steps(0), max_step(0), detected_model(MODEL_UNKNOWN) { } @@ -113,6 +113,7 @@ max_angle = (static_cast<int>(last_step) - static_cast<int>(front_step)) * resolution; scanable_steps = last_step - first_step + 1; + time_resolution = 60000 / speed; } Modified: gearbox/trunk/src/hokuyo_aist/sensor_info.h =================================================================== --- gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/sensor_info.h 2010-08-11 00:49:53 UTC (rev 511) @@ -196,6 +196,8 @@ double max_angle; /// Angle between two scan points (radians). double resolution; + /// Time between two scan points (milliseconds). + unsigned int time_resolution; /// Total number of steps in a full scan (lastStep - firstStep). unsigned int scanable_steps; /// Absolute maximum commandable step. Modified: gearbox/trunk/src/hokuyo_aist/test/example.cpp =================================================================== --- gearbox/trunk/src/hokuyo_aist/test/example.cpp 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/test/example.cpp 2010-08-11 00:49:53 UTC (rev 511) @@ -130,6 +130,10 @@ { std::cerr << "Failed to change baud rate: " << e.what() << '\n'; } + catch(...) + { + std::cerr << "Failed to change baud rate\n"; + } // Set the motor speed try { Modified: gearbox/trunk/src/hokuyo_aist/utils.h =================================================================== --- gearbox/trunk/src/hokuyo_aist/utils.h 2010-08-08 04:31:17 UTC (rev 510) +++ gearbox/trunk/src/hokuyo_aist/utils.h 2010-08-11 00:49:53 UTC (rev 511) @@ -24,7 +24,10 @@ #include <string> #include <vector> #include <algorithm> +#include <cassert> +#include <iostream> + #if defined(WIN32) typedef unsigned char uint8_t; typedef unsigned int uint32_t; @@ -65,6 +68,7 @@ } #endif + /// Find the median value of a std::vector. template<typename T> inline T median(std::vector<T>& v) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |