urg_stamped.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 The urg_stamped Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include <ros/ros.h>
00018 #include <sensor_msgs/LaserScan.h>
00019 
00020 #include <boost/bind/bind.hpp>
00021 #include <boost/format.hpp>
00022 #include <boost/thread.hpp>
00023 
00024 #include <algorithm>
00025 #include <map>
00026 #include <random>
00027 #include <string>
00028 #include <vector>
00029 #include <list>
00030 
00031 #include <scip2/scip2.h>
00032 #include <scip2/walltime.h>
00033 
00034 #include <first_order_filter.h>
00035 #include <timestamp_moving_average.h>
00036 #include <timestamp_outlier_remover.h>
00037 
00038 class UrgStampedNode
00039 {
00040 protected:
00041   ros::NodeHandle nh_;
00042   ros::NodeHandle pnh_;
00043   ros::Publisher pub_scan_;
00044   ros::Timer timer_sync_;
00045   ros::Timer timer_delay_estim_;
00046   ros::Timer timer_try_tm_;
00047 
00048   sensor_msgs::LaserScan msg_base_;
00049   uint32_t step_min_;
00050   uint32_t step_max_;
00051 
00052   scip2::Connection::Ptr device_;
00053   scip2::Protocol::Ptr scip_;
00054 
00055   bool publish_intensity_;
00056 
00057   boost::posix_time::ptime time_tm_request;
00058   std::list<ros::Duration> communication_delays_;
00059   std::list<ros::Time> device_time_origins_;
00060   ros::Duration estimated_communication_delay_;
00061   size_t tm_iter_num_;
00062   size_t tm_median_window_;
00063   bool estimated_communication_delay_init_;
00064   double communication_delay_filter_alpha_;
00065 
00066   boost::posix_time::ptime time_ii_request;
00067   std::vector<ros::Duration> on_scan_communication_delays_;
00068 
00069   scip2::Walltime<24> walltime_;
00070 
00071   std::default_random_engine random_engine_;
00072   std::uniform_real_distribution<double> sync_interval_;
00073   ros::Time last_sync_time_;
00074 
00075   class DriftedTime
00076   {
00077   public:
00078     ros::Time origin_;
00079     double gain_;
00080 
00081     DriftedTime()
00082       : gain_(1.0)
00083     {
00084     }
00085     DriftedTime(const ros::Time origin, const float gain)
00086       : origin_(origin)
00087       , gain_(gain)
00088     {
00089     }
00090   };
00091   DriftedTime device_time_origin_;
00092 
00093   ros::Time t0_;
00094   FirstOrderLPF<double> timestamp_lpf_;
00095   FirstOrderHPF<double> timestamp_hpf_;
00096   TimestampOutlierRemover timestamp_outlier_removal_;
00097   TimestampMovingAverage timestamp_moving_average_;
00098 
00099   ros::Time calculateDeviceTimeOriginByAverage(
00100       const boost::posix_time::ptime &time_request,
00101       const boost::posix_time::ptime &time_response,
00102       const uint64_t &device_timestamp)
00103   {
00104     const auto delay =
00105         ros::Time::fromBoost(time_response) -
00106         ros::Time::fromBoost(time_request);
00107     const ros::Time time_at_device_timestamp = ros::Time::fromBoost(time_request) + delay * 0.5;
00108 
00109     return time_at_device_timestamp - ros::Duration().fromNSec(device_timestamp * 1e6);
00110   }
00111   ros::Time calculateDeviceTimeOrigin(
00112       const boost::posix_time::ptime &time_response,
00113       const uint64_t &device_timestamp,
00114       ros::Time &time_at_device_timestamp)
00115   {
00116     time_at_device_timestamp = ros::Time::fromBoost(time_response) - estimated_communication_delay_ * 0.5;
00117 
00118     return time_at_device_timestamp - ros::Duration().fromNSec(device_timestamp * 1e6);
00119   }
00120 
00121   void cbM(
00122       const boost::posix_time::ptime &time_read,
00123       const std::string &echo_back,
00124       const std::string &status,
00125       const scip2::ScanData &scan,
00126       const bool has_intensity)
00127   {
00128     const uint64_t walltime_device = walltime_.update(scan.timestamp_);
00129 
00130     const auto estimated_timestamp_lf =
00131         device_time_origin_.origin_ +
00132         ros::Duration().fromNSec(walltime_device * 1e6 * device_time_origin_.gain_) +
00133         ros::Duration(msg_base_.time_increment * step_min_);
00134 
00135     if (t0_ == ros::Time(0))
00136       t0_ = estimated_timestamp_lf;
00137 
00138     const auto receive_time =
00139         timestamp_outlier_removal_.update(
00140             ros::Time::fromBoost(time_read) -
00141             estimated_communication_delay_ * 0.5 -
00142             ros::Duration(msg_base_.scan_time));
00143 
00144     sensor_msgs::LaserScan msg(msg_base_);
00145     msg.header.stamp =
00146         timestamp_moving_average_.update(
00147             t0_ +
00148             ros::Duration(
00149                 timestamp_lpf_.update((estimated_timestamp_lf - t0_).toSec()) +
00150                 timestamp_hpf_.update((receive_time - t0_).toSec())));
00151 
00152     if (scan.ranges_.size() != step_max_ - step_min_ + 1)
00153     {
00154       ROS_DEBUG("Size of the received scan data is wrong (expected: %d, received: %lu); refreshing",
00155                 step_max_ - step_min_ + 1, scan.ranges_.size());
00156       scip_->sendCommand(
00157           (has_intensity ? "ME" : "MD") +
00158           (boost::format("%04d%04d") % step_min_ % step_max_).str() +
00159           "00000");
00160       return;
00161     }
00162 
00163     msg.ranges.reserve(scan.ranges_.size());
00164     for (auto &r : scan.ranges_)
00165       msg.ranges.push_back(r * 1e-3);
00166     if (has_intensity)
00167     {
00168       msg.intensities.reserve(scan.intensities_.size());
00169       for (auto &r : scan.intensities_)
00170         msg.intensities.push_back(r);
00171     }
00172 
00173     pub_scan_.publish(msg);
00174   }
00175   void cbTMSend(const boost::posix_time::ptime &time_send)
00176   {
00177     time_tm_request = time_send;
00178   }
00179   void cbTM(
00180       const boost::posix_time::ptime &time_read,
00181       const std::string &echo_back,
00182       const std::string &status,
00183       const scip2::Timestamp &time_device)
00184   {
00185     if (status != "00")
00186       return;
00187 
00188     timer_try_tm_.stop();
00189     switch (echo_back[2])
00190     {
00191       case '0':
00192       {
00193         scip_->sendCommand(
00194             "TM1",
00195             boost::bind(&UrgStampedNode::cbTMSend, this, boost::arg<1>()));
00196         break;
00197       }
00198       case '1':
00199       {
00200         const uint64_t walltime_device = walltime_.update(time_device.timestamp_);
00201 
00202         const auto delay =
00203             ros::Time::fromBoost(time_read) -
00204             ros::Time::fromBoost(time_tm_request);
00205         communication_delays_.push_back(delay);
00206         if (communication_delays_.size() > tm_median_window_)
00207           communication_delays_.pop_front();
00208 
00209         const auto origin = calculateDeviceTimeOriginByAverage(
00210             time_tm_request, time_read, walltime_device);
00211         device_time_origins_.push_back(origin);
00212         if (device_time_origins_.size() > tm_median_window_)
00213           device_time_origins_.pop_front();
00214 
00215         if (communication_delays_.size() >= tm_iter_num_)
00216         {
00217           std::vector<ros::Duration> delays(communication_delays_.begin(), communication_delays_.end());
00218           std::vector<ros::Time> origins(device_time_origins_.begin(), device_time_origins_.end());
00219           sort(delays.begin(), delays.end());
00220           sort(origins.begin(), origins.end());
00221 
00222           if (!estimated_communication_delay_init_)
00223           {
00224             estimated_communication_delay_ = delays[tm_iter_num_ / 2];
00225             device_time_origin_ = DriftedTime(origins[tm_iter_num_ / 2], 1.0);
00226           }
00227           else
00228           {
00229             estimated_communication_delay_ =
00230                 estimated_communication_delay_ * (1.0 - communication_delay_filter_alpha_) +
00231                 delays[tm_iter_num_ / 2] * communication_delay_filter_alpha_;
00232           }
00233           estimated_communication_delay_init_ = true;
00234           ROS_DEBUG("delay: %0.6f, device timestamp: %ld, device time origin: %0.6f",
00235                     estimated_communication_delay_.toSec(),
00236                     walltime_device,
00237                     origins[tm_iter_num_ / 2].toSec());
00238           scip_->sendCommand("TM2");
00239         }
00240         else
00241         {
00242           ros::Duration(0.005).sleep();
00243           scip_->sendCommand(
00244               "TM1",
00245               boost::bind(&UrgStampedNode::cbTMSend, this, boost::arg<1>()));
00246         }
00247         break;
00248       }
00249       case '2':
00250       {
00251         scip_->sendCommand(
00252             (publish_intensity_ ? "ME" : "MD") +
00253             (boost::format("%04d%04d") % step_min_ % step_max_).str() +
00254             "00000");
00255         timeSync();
00256         timestamp_outlier_removal_.reset();
00257         timestamp_moving_average_.reset();
00258         t0_ = ros::Time();
00259         break;
00260       }
00261     }
00262   }
00263   void cbPP(
00264       const boost::posix_time::ptime &time_read,
00265       const std::string &echo_back,
00266       const std::string &status,
00267       const std::map<std::string, std::string> &params)
00268   {
00269     const auto amin = params.find("AMIN");
00270     const auto amax = params.find("AMAX");
00271     const auto dmin = params.find("DMIN");
00272     const auto dmax = params.find("DMAX");
00273     const auto ares = params.find("ARES");
00274     const auto afrt = params.find("AFRT");
00275     const auto scan = params.find("SCAN");
00276     if (amin == params.end() || amax == params.end() ||
00277         dmin == params.end() || dmax == params.end() ||
00278         ares == params.end() || afrt == params.end() ||
00279         scan == params.end())
00280     {
00281       ROS_ERROR("PP doesn't have required parameters");
00282       return;
00283     }
00284     step_min_ = std::stoi(amin->second);
00285     step_max_ = std::stoi(amax->second);
00286     msg_base_.scan_time = 60.0 / std::stoi(scan->second);
00287     msg_base_.angle_increment = 2.0 * M_PI / std::stoi(ares->second);
00288     msg_base_.time_increment = msg_base_.scan_time / std::stoi(ares->second);
00289     msg_base_.range_min = std::stoi(dmin->second) * 1e-3;
00290     msg_base_.range_max = std::stoi(dmax->second) * 1e-3;
00291     msg_base_.angle_min =
00292         (std::stoi(amin->second) - std::stoi(afrt->second)) * msg_base_.angle_increment;
00293     msg_base_.angle_max =
00294         (std::stoi(amax->second) - std::stoi(afrt->second)) * msg_base_.angle_increment;
00295 
00296     timestamp_outlier_removal_.setInterval(ros::Duration(msg_base_.scan_time));
00297     timestamp_moving_average_.setInterval(ros::Duration(msg_base_.scan_time));
00298     delayEstimation();
00299   }
00300   void cbVV(
00301       const boost::posix_time::ptime &time_read,
00302       const std::string &echo_back,
00303       const std::string &status,
00304       const std::map<std::string, std::string> &params)
00305   {
00306   }
00307   void cbIISend(const boost::posix_time::ptime &time_send)
00308   {
00309     time_ii_request = time_send;
00310   }
00311   void cbII(
00312       const boost::posix_time::ptime &time_read,
00313       const std::string &echo_back,
00314       const std::string &status,
00315       const std::map<std::string, std::string> &params)
00316   {
00317     const auto delay =
00318         ros::Time::fromBoost(time_read) -
00319         ros::Time::fromBoost(time_ii_request);
00320 
00321     if (delay.toSec() < 0.002)
00322     {
00323       const auto time = params.find("TIME");
00324       if (time == params.end())
00325       {
00326         ROS_DEBUG("II doesn't have timestamp");
00327         return;
00328       }
00329       if (time->second.size() != 6 && time->second.size() != 4)
00330       {
00331         ROS_DEBUG("Timestamp in II is ill-formatted (%s)", time->second.c_str());
00332         return;
00333       }
00334       const uint32_t time_device =
00335           time->second.size() == 6 ?
00336               std::stoi(time->second, nullptr, 16) :
00337               *(scip2::Decoder<4>(time->second).begin());
00338 
00339       const uint64_t walltime_device = walltime_.update(time_device);
00340 
00341       ros::Time time_at_device_timestamp;
00342       const auto origin = calculateDeviceTimeOrigin(
00343           time_read, walltime_device, time_at_device_timestamp);
00344 
00345       const auto now = ros::Time::fromBoost(time_read);
00346       if (last_sync_time_ == ros::Time(0))
00347         last_sync_time_ = now;
00348       const double dt = std::min((now - last_sync_time_).toSec(), 10.0);
00349       last_sync_time_ = now;
00350 
00351       const double gain =
00352           (time_at_device_timestamp - device_time_origin_.origin_).toSec() /
00353           (time_at_device_timestamp - origin).toSec();
00354       const double exp_lpf_alpha =
00355           dt * (1.0 / 30.0);  // 30 seconds exponential LPF
00356       const double updated_gain =
00357           (1.0 - exp_lpf_alpha) * device_time_origin_.gain_ + exp_lpf_alpha * gain;
00358       device_time_origin_.gain_ = updated_gain;
00359       device_time_origin_.origin_ +=
00360           ros::Duration(exp_lpf_alpha * (origin - device_time_origin_.origin_).toSec());
00361 
00362       ROS_DEBUG("on scan delay: %0.6f, device timestamp: %ld, device time origin: %0.6f, gain: %0.6f",
00363                 delay.toSec(),
00364                 walltime_device,
00365                 origin.toSec(),
00366                 updated_gain);
00367     }
00368     else
00369     {
00370       ROS_DEBUG("on scan delay (%0.6f) is larger than expected; skipping",
00371                 delay.toSec());
00372     }
00373   }
00374   void cbQT(
00375       const boost::posix_time::ptime &time_read,
00376       const std::string &echo_back,
00377       const std::string &status)
00378   {
00379     ROS_DEBUG("Scan data stopped");
00380   }
00381   void cbConnect()
00382   {
00383     scip_->sendCommand("PP");
00384     device_->startWatchdog(boost::posix_time::seconds(1));
00385   }
00386 
00387   void timeSync(const ros::TimerEvent &event = ros::TimerEvent())
00388   {
00389     scip_->sendCommand(
00390         "II",
00391         boost::bind(&UrgStampedNode::cbIISend, this, boost::arg<1>()));
00392     timer_sync_ = nh_.createTimer(
00393         ros::Duration(sync_interval_(random_engine_)),
00394         &UrgStampedNode::timeSync, this, true);
00395   }
00396   void delayEstimation(const ros::TimerEvent &event = ros::TimerEvent())
00397   {
00398     timer_sync_.stop();
00399     ROS_INFO("Starting communication delay estimation");
00400     scip_->sendCommand("QT");
00401     timer_try_tm_ = nh_.createTimer(
00402         ros::Duration(0.05),
00403         &UrgStampedNode::tryTM, this);
00404   }
00405   void tryTM(const ros::TimerEvent &event = ros::TimerEvent())
00406   {
00407     scip_->sendCommand("QT");
00408     scip_->sendCommand("TM0");
00409   }
00410 
00411 public:
00412   UrgStampedNode()
00413     : nh_("")
00414     , pnh_("~")
00415     , tm_iter_num_(5)
00416     , tm_median_window_(35)
00417     , estimated_communication_delay_init_(false)
00418     , communication_delay_filter_alpha_(0.3)
00419     , last_sync_time_(0)
00420     , timestamp_lpf_(20)
00421     , timestamp_hpf_(20)
00422     , timestamp_outlier_removal_(ros::Duration(0.001), ros::Duration())
00423     , timestamp_moving_average_(5, ros::Duration())
00424   {
00425     std::string ip;
00426     int port;
00427     double sync_interval_min;
00428     double sync_interval_max;
00429     double delay_estim_interval;
00430 
00431     pnh_.param("ip_address", ip, std::string("192.168.0.10"));
00432     pnh_.param("ip_port", port, 10940);
00433     pnh_.param("frame_id", msg_base_.header.frame_id, std::string("laser"));
00434     pnh_.param("publish_intensity", publish_intensity_, true);
00435     pnh_.param("sync_interval_min", sync_interval_min, 1.0);
00436     pnh_.param("sync_interval_max", sync_interval_max, 1.5);
00437     sync_interval_ = std::uniform_real_distribution<double>(sync_interval_min, sync_interval_max);
00438     pnh_.param("delay_estim_interval", delay_estim_interval, 20.0);
00439 
00440     pub_scan_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10);
00441 
00442     device_.reset(new scip2::ConnectionTcp(ip, port));
00443     device_->registerCloseCallback(ros::shutdown);
00444     device_->registerConnectCallback(
00445         boost::bind(&UrgStampedNode::cbConnect, this));
00446 
00447     scip_.reset(new scip2::Protocol(device_));
00448     scip_->registerCallback<scip2::ResponsePP>(
00449         boost::bind(&UrgStampedNode::cbPP, this,
00450                     boost::arg<1>(),
00451                     boost::arg<2>(),
00452                     boost::arg<3>(),
00453                     boost::arg<4>()));
00454     scip_->registerCallback<scip2::ResponseVV>(
00455         boost::bind(&UrgStampedNode::cbVV, this,
00456                     boost::arg<1>(),
00457                     boost::arg<2>(),
00458                     boost::arg<3>(),
00459                     boost::arg<4>()));
00460     scip_->registerCallback<scip2::ResponseII>(
00461         boost::bind(&UrgStampedNode::cbII, this,
00462                     boost::arg<1>(),
00463                     boost::arg<2>(),
00464                     boost::arg<3>(),
00465                     boost::arg<4>()));
00466     scip_->registerCallback<scip2::ResponseMD>(
00467         boost::bind(&UrgStampedNode::cbM, this,
00468                     boost::arg<1>(),
00469                     boost::arg<2>(),
00470                     boost::arg<3>(),
00471                     boost::arg<4>(),
00472                     false));
00473     scip_->registerCallback<scip2::ResponseME>(
00474         boost::bind(&UrgStampedNode::cbM, this,
00475                     boost::arg<1>(),
00476                     boost::arg<2>(),
00477                     boost::arg<3>(),
00478                     boost::arg<4>(),
00479                     true));
00480     scip_->registerCallback<scip2::ResponseTM>(
00481         boost::bind(&UrgStampedNode::cbTM, this,
00482                     boost::arg<1>(),
00483                     boost::arg<2>(),
00484                     boost::arg<3>(),
00485                     boost::arg<4>()));
00486     scip_->registerCallback<scip2::ResponseQT>(
00487         boost::bind(&UrgStampedNode::cbQT, this,
00488                     boost::arg<1>(),
00489                     boost::arg<2>(),
00490                     boost::arg<3>()));
00491 
00492     if (delay_estim_interval > 0.0)
00493     {
00494       timer_delay_estim_ = nh_.createTimer(
00495           ros::Duration(delay_estim_interval), &UrgStampedNode::delayEstimation, this);
00496     }
00497   }
00498   void spin()
00499   {
00500     boost::thread thread(
00501         boost::bind(&scip2::Connection::spin, device_.get()));
00502     ros::spin();
00503     timer_sync_.stop();
00504     scip_->sendCommand("QT");
00505     device_->stop();
00506   }
00507 };
00508 
00509 int main(int argc, char **argv)
00510 {
00511   ros::init(argc, argv, "urg_stamped");
00512   UrgStampedNode node;
00513   node.spin();
00514 
00515   return 1;
00516 }


urg_stamped
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 6 2019 18:59:51