激光雷达数据采集Linux&&ARM; Linux

/*! \file \brief URG センサ制御 \author Satofumi KAMIMURA $Id: UrgDevice.cpp 1992 2012-09-11 05:57:06Z satofumi $ \todo remain_times の 100 をマクロで置き換える */ #include "UrgDevice.h" #include "SerialDevice.h" #include "ScipHandler.h" #include "RangeSensorParameter.h" #include "ticks.h" #include "Thread.h" #include "LockGuard.h" #include "Lock.h" #include <deque> #include <limits> #include <cmath> #include <cstring> #include <cstdio> #ifdef MSC #define snprintf _snprintf #endif using namespace qrk; using namespace std; namespace { enum { MdScansMax = 100, // [times] }; } struct UrgDevice::pImpl { struct ScanData { vector<long> length_data; long timestamp; CaptureSettings settings; ScanData(void) : timestamp(-1) { } }; class Capture { public: virtual ~Capture(void) { } virtual string createCaptureCommand(void) = 0; virtual int capture(vector<long>& data, long* timestamp) = 0; virtual void setCapturesSize(size_t size) = 0; virtual size_t capturesSize(void) = 0; virtual size_t remainCaptureTimes(void) = 0; }; class RawManualCapture : public Capture { pImpl* pimpl_; public: RawManualCapture(pImpl* pimpl) : pimpl_(pimpl) { } ~RawManualCapture(void) { } string createCaptureCommand(void) { // !!! parameter 受信直後に、送受信パラメータへの代入を行う // !!! ここでの送受信には、送受信パラメータの内容を用いる char buffer[] = "GDbbbbeeeegg\n"; snprintf(buffer, strlen(buffer) + 1, "GD%04d%04d%02u\n", pimpl_->capture_begin_, pimpl_->capture_end_, pimpl_->capture_skip_lines_); return buffer; } int capture(vector<long>& data, long* timestamp) { pimpl_->retry_times_ = 0; // レーザを点灯させておく pimpl_->scip_.setLaserOutput(ScipHandler::On); // データ取得コマンドの送信 string command = createCaptureCommand(); int n = pimpl_->scip_.send(command.c_str(), static_cast<int>(command.size())); if (n != static_cast<int>(command.size())) { pimpl_->error_message_ = "Send command:" + command + " fail."; return -1; } CaptureSettings settings; pimpl_->scip_.receiveCaptureData(data, settings, timestamp, NULL); return static_cast<int>(data.size()); } void setCapturesSize(size_t size) { static_cast<void>(size); // 何もしない } size_t capturesSize(void) { return 1; } size_t remainCaptureTimes(void) { return 0; } }; class RawAutoCapture : public Capture { pImpl* pimpl_; size_t captures_size_; public: RawAutoCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1) { } ~RawAutoCapture(void) { } string createCaptureCommand(void) { char buffer[] = "MDbbbbeeeeggstt\n"; snprintf(buffer, strlen(buffer) + 1, "MD%04d%04d%02u%01u%02u\n", pimpl_->capture_begin_, pimpl_->capture_end_, pimpl_->capture_skip_lines_, pimpl_->capture_frame_interval_, (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_); pimpl_->remain_times_ = pimpl_->capture_times_; return buffer; } int capture(vector<long>& data, long* timestamp) { if (pimpl_->retry_times_ > pimpl_->max_retry_times_) { return -1; } // スレッドを起動 LockGuard guard(pimpl_->mutex_); if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) { pimpl_->thread_.run(1); } // 取得済みデータがなければ、戻る if (pimpl_->data_buffer_.empty()) { if (pimpl_->invalid_packet_) { return -1; } return 0; } swap(data, pimpl_->data_buffer_.front().length_data); if (timestamp) { *timestamp = pimpl_->data_buffer_.front().timestamp; //fprintf(stderr, "MD: %ld, %ld\n", ticks(), *timestamp); } pimpl_->data_buffer_.pop_front(); return static_cast<int>(data.size()); } void setCapturesSize(size_t size) { captures_size_ = size; } size_t capturesSize(void) { return captures_size_; } size_t remainCaptureTimes(void) { if (pimpl_->capture_times_ == 0) { return numeric_limits<size_t>::max(); } LockGuard guard(pimpl_->mutex_); return pimpl_->remain_times_; } }; class RawIntensityCapture : public Capture { pImpl* pimpl_; size_t captures_size_; public: RawIntensityCapture(pImpl* pimpl) : pimpl_(pimpl), captures_size_(1) { } ~RawIntensityCapture(void) { } string createCaptureCommand(void) { char buffer[] = "MEbbbbeeeeggstt\n"; if ((! pimpl_->urg_type_.compare("URG-04LX")) || (! pimpl_->urg_type_.compare("URG-04LX-UG01")) || (! pimpl_->urg_type_.compare("UBG-04LX-F01"))) { // URG-04LX ならば、FF 形式でデータ送信を行う snprintf(buffer, strlen(buffer) + 1, "MD%04d%04dFF%01u%02u\n", pimpl_->capture_begin_, pimpl_->capture_end_, pimpl_->capture_frame_interval_, (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_); } else { // UTM-30LX ならば、ME コマンドを用いる snprintf(buffer, strlen(buffer) + 1, "ME%04d%04d%02u%01u%02u\n", pimpl_->capture_begin_, pimpl_->capture_end_, pimpl_->capture_skip_lines_, pimpl_->capture_frame_interval_, (pimpl_->capture_times_ > 99) ? 0 : pimpl_->capture_times_); } pimpl_->remain_times_ = pimpl_->capture_times_; return buffer; } int capture(vector<long>& data, long* timestamp) { if (pimpl_->retry_times_ > pimpl_->max_retry_times_) { return -1; } LockGuard guard(pimpl_->mutex_); // 取得済みデータがなければ、スレッドを起動 if ((! pimpl_->thread_.isRunning()) && pimpl_->data_buffer_.empty()) { pimpl_->thread_.run(1); } // 取得済みデータがなければ、戻る if (pimpl_->data_buffer_.empty()) { if (pimpl_->invalid_packet_) { return -1; } return 0; } swap(data, pimpl_->data_buffer_.front().length_data); if (timestamp) { *timestamp = pimpl_->data_buffer_.front().timestamp; } CaptureSettings settings = pimpl_->data_buffer_.front().settings; pimpl_->data_buffer_.pop_front(); pimpl_->intensity_data_.timestamp = *timestamp; pimpl_->intensity_data_.length_data.clear(); if (pimpl_->urg_type_.compare("URG-04LX") && pimpl_->urg_type_.compare("URG-04LX-UG01") && pimpl_->urg_type_.compare("UBG-04LX-F01")) { // UTM-30LX のとき int skip_lines = settings.skip_lines; int min_length = pimpl_->parameters_.distance_min; int n = data.size() / skip_lines / 2; pimpl_->intensity_data_.length_data.reserve(n * skip_lines); int filled = 0; for (int i = 0; i < n; ++i) { size_t index = 2 * skip_lines * i; long length = data[index]; long intensity_value = (length < min_length) ? 0 : data[index + skip_lines]; for (int j = 0; j < skip_lines; ++j) { pimpl_->intensity_data_.length_data.push_back(intensity_value); data[filled++] = length; } } vector<long>::iterator first_it = data.begin() + (data.size() / 2);