safety_limiter.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014-2017, the neonavigation authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <diagnostic_updater/diagnostic_updater.h>
00032 #include <sensor_msgs/PointCloud2.h>
00033 #include <sensor_msgs/PointCloud.h>
00034 #include <std_msgs/Bool.h>
00035 #include <std_msgs/Empty.h>
00036 #include <pcl_ros/point_cloud.h>
00037 #include <pcl_ros/transforms.h>
00038 #include <tf2_ros/transform_listener.h>
00039 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
00040 
00041 #include <Eigen/Core>
00042 #include <Eigen/Geometry>
00043 
00044 #include <pcl/point_types.h>
00045 #include <pcl/conversions.h>
00046 #include <pcl_conversions/pcl_conversions.h>
00047 #include <pcl/kdtree/kdtree_flann.h>
00048 #include <pcl/filters/voxel_grid.h>
00049 
00050 #include <algorithm>
00051 #include <cmath>
00052 #include <random>
00053 #include <string>
00054 #include <iostream>
00055 #include <sstream>
00056 #include <vector>
00057 
00058 #include <neonavigation_common/compatibility.h>
00059 
00060 pcl::PointXYZ operator-(const pcl::PointXYZ& a, const pcl::PointXYZ& b)
00061 {
00062   auto c = a;
00063   c.x -= b.x;
00064   c.y -= b.y;
00065   c.z -= b.z;
00066   return c;
00067 }
00068 pcl::PointXYZ operator+(const pcl::PointXYZ& a, const pcl::PointXYZ& b)
00069 {
00070   auto c = a;
00071   c.x += b.x;
00072   c.y += b.y;
00073   c.z += b.z;
00074   return c;
00075 }
00076 pcl::PointXYZ operator*(const pcl::PointXYZ& a, const float& b)
00077 {
00078   auto c = a;
00079   c.x *= b;
00080   c.y *= b;
00081   c.z *= b;
00082   return c;
00083 }
00084 bool XmlRpc_isNumber(XmlRpc::XmlRpcValue& value)
00085 {
00086   return value.getType() == XmlRpc::XmlRpcValue::TypeInt ||
00087          value.getType() == XmlRpc::XmlRpcValue::TypeDouble;
00088 }
00089 
00090 class SafetyLimiterNode
00091 {
00092 protected:
00093   ros::NodeHandle nh_;
00094   ros::NodeHandle pnh_;
00095   ros::Publisher pub_twist_;
00096   ros::Publisher pub_cloud_;
00097   ros::Publisher pub_debug_;
00098   ros::Subscriber sub_twist_;
00099   std::vector<ros::Subscriber> sub_clouds_;
00100   ros::Subscriber sub_disable_;
00101   ros::Subscriber sub_watchdog_;
00102   ros::Timer watchdog_timer_;
00103   tf2_ros::Buffer tfbuf_;
00104   tf2_ros::TransformListener tfl_;
00105 
00106   geometry_msgs::Twist twist_;
00107   ros::Time last_cloud_stamp_;
00108   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
00109   double hz_;
00110   double timeout_;
00111   double disable_timeout_;
00112   double vel_[2];
00113   double acc_[2];
00114   double tmax_;
00115   double dt_;
00116   double d_margin_;
00117   double d_escape_;
00118   double yaw_margin_;
00119   double yaw_escape_;
00120   double r_lim_;
00121   double z_range_[2];
00122   float footprint_radius_;
00123   double downsample_grid_;
00124   std::string frame_id_;
00125 
00126   ros::Time last_disable_cmd_;
00127   ros::Duration hold_;
00128   ros::Time hold_off_;
00129   ros::Duration watchdog_interval_;
00130   bool allow_empty_cloud_;
00131 
00132   bool watchdog_stop_;
00133   bool has_cloud_;
00134   bool has_twist_;
00135   bool has_collision_at_now_;
00136 
00137   constexpr static float EPSILON = 1e-6;
00138 
00139   diagnostic_updater::Updater diag_updater_;
00140 
00141 public:
00142   SafetyLimiterNode()
00143     : nh_()
00144     , pnh_("~")
00145     , tfl_(tfbuf_)
00146     , cloud_(new pcl::PointCloud<pcl::PointXYZ>)
00147     , last_disable_cmd_(0)
00148     , watchdog_stop_(false)
00149     , has_cloud_(false)
00150     , has_twist_(true)
00151     , has_collision_at_now_(false)
00152   {
00153     neonavigation_common::compat::checkCompatMode();
00154     pub_twist_ = neonavigation_common::compat::advertise<geometry_msgs::Twist>(
00155         nh_, "cmd_vel",
00156         pnh_, "cmd_vel_out", 1, true);
00157     pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud>("collision", 1, true);
00158     pub_debug_ = nh_.advertise<sensor_msgs::PointCloud>("debug", 1, true);
00159     sub_twist_ = neonavigation_common::compat::subscribe(
00160         nh_, "cmd_vel_in",
00161         pnh_, "cmd_vel_in", 1, &SafetyLimiterNode::cbTwist, this);
00162     sub_disable_ = neonavigation_common::compat::subscribe(
00163         nh_, "disable_safety",
00164         pnh_, "disable", 1, &SafetyLimiterNode::cbDisable, this);
00165     sub_watchdog_ = neonavigation_common::compat::subscribe(
00166         nh_, "watchdog_reset",
00167         pnh_, "watchdog_reset", 1, &SafetyLimiterNode::cbWatchdogReset, this);
00168 
00169     int num_input_clouds;
00170     pnh_.param("num_input_clouds", num_input_clouds, 1);
00171     if (num_input_clouds == 1)
00172     {
00173       sub_clouds_.push_back(neonavigation_common::compat::subscribe(
00174           nh_, "cloud",
00175           pnh_, "cloud", 1, &SafetyLimiterNode::cbCloud, this));
00176     }
00177     else
00178     {
00179       for (int i = 0; i < num_input_clouds; ++i)
00180       {
00181         sub_clouds_.push_back(nh_.subscribe(
00182             "cloud" + std::to_string(i), 1, &SafetyLimiterNode::cbCloud, this));
00183       }
00184     }
00185 
00186     pnh_.param("freq", hz_, 6.0);
00187     pnh_.param("cloud_timeout", timeout_, 0.8);
00188     pnh_.param("disable_timeout", disable_timeout_, 0.1);
00189     pnh_.param("lin_vel", vel_[0], 0.5);
00190     pnh_.param("lin_acc", acc_[0], 1.0);
00191     pnh_.param("ang_vel", vel_[1], 0.8);
00192     pnh_.param("ang_acc", acc_[1], 1.6);
00193     pnh_.param("z_range_min", z_range_[0], 0.0);
00194     pnh_.param("z_range_max", z_range_[1], 0.5);
00195     pnh_.param("dt", dt_, 0.1);
00196     if (pnh_.hasParam("t_margin"))
00197       ROS_WARN("safety_limiter: t_margin parameter is obsolated. Use d_margin and yaw_margin instead.");
00198     pnh_.param("d_margin", d_margin_, 0.2);
00199     pnh_.param("d_escape", d_escape_, 0.05);
00200     pnh_.param("yaw_margin", yaw_margin_, 0.2);
00201     pnh_.param("yaw_escape", yaw_escape_, 0.05);
00202     pnh_.param("downsample_grid", downsample_grid_, 0.05);
00203     pnh_.param("frame_id", frame_id_, std::string("base_link"));
00204     double hold_d;
00205     pnh_.param("hold", hold_d, 0.0);
00206     hold_ = ros::Duration(std::max(hold_d, 1.0 / hz_));
00207     double watchdog_interval_d;
00208     pnh_.param("watchdog_interval", watchdog_interval_d, 0.0);
00209     watchdog_interval_ = ros::Duration(watchdog_interval_d);
00210     pnh_.param("allow_empty_cloud", allow_empty_cloud_, false);
00211 
00212     tmax_ = 0.0;
00213     for (int i = 0; i < 2; i++)
00214     {
00215       auto t = vel_[i] / acc_[i];
00216       if (tmax_ < t)
00217         tmax_ = t;
00218     }
00219     tmax_ *= 1.5;
00220     tmax_ += std::max(d_margin_ / vel_[0], yaw_margin_ / vel_[1]);
00221     r_lim_ = 1.0;
00222 
00223     XmlRpc::XmlRpcValue footprint_xml;
00224     if (!pnh_.hasParam("footprint"))
00225     {
00226       ROS_FATAL("Footprint doesn't specified");
00227       throw std::runtime_error("Footprint doesn't specified");
00228     }
00229     pnh_.getParam("footprint", footprint_xml);
00230     if (footprint_xml.getType() != XmlRpc::XmlRpcValue::TypeArray || footprint_xml.size() < 3)
00231     {
00232       ROS_FATAL("Invalid footprint");
00233       throw std::runtime_error("Invalid footprint");
00234     }
00235     footprint_radius_ = 0;
00236     for (int i = 0; i < footprint_xml.size(); i++)
00237     {
00238       if (!XmlRpc_isNumber(footprint_xml[i][0]) ||
00239           !XmlRpc_isNumber(footprint_xml[i][1]))
00240       {
00241         ROS_FATAL("Invalid footprint value");
00242         throw std::runtime_error("Invalid footprint value");
00243       }
00244 
00245       vec v;
00246       v[0] = static_cast<double>(footprint_xml[i][0]);
00247       v[1] = static_cast<double>(footprint_xml[i][1]);
00248       footprint_p.v.push_back(v);
00249 
00250       auto dist = hypotf(v[0], v[2]);
00251       if (dist > footprint_radius_)
00252         footprint_radius_ = dist;
00253     }
00254     footprint_p.v.push_back(footprint_p.v.front());
00255     ROS_INFO("footprint radius: %0.3f", footprint_radius_);
00256 
00257     diag_updater_.setHardwareID("none");
00258     diag_updater_.add("Collision", this, &SafetyLimiterNode::diagnoseCollision);
00259   }
00260   void spin()
00261   {
00262     ros::Timer predict_timer =
00263         nh_.createTimer(ros::Duration(1.0 / hz_), &SafetyLimiterNode::cbPredictTimer, this);
00264 
00265     if (watchdog_interval_ != ros::Duration(0.0))
00266     {
00267       watchdog_timer_ =
00268           nh_.createTimer(watchdog_interval_, &SafetyLimiterNode::cbWatchdogTimer, this);
00269     }
00270 
00271     ros::spin();
00272   }
00273 
00274 protected:
00275   void cbWatchdogReset(const std_msgs::Empty::ConstPtr& msg)
00276   {
00277     watchdog_timer_.setPeriod(watchdog_interval_, true);
00278     watchdog_stop_ = false;
00279   }
00280   void cbWatchdogTimer(const ros::TimerEvent& event)
00281   {
00282     ROS_WARN_THROTTLE(1.0, "safety_limiter: Watchdog timed-out");
00283     watchdog_stop_ = true;
00284     geometry_msgs::Twist cmd_vel;
00285     pub_twist_.publish(cmd_vel);
00286   }
00287   void cbPredictTimer(const ros::TimerEvent& event)
00288   {
00289     if (!has_twist_)
00290       return;
00291     if (!has_cloud_)
00292       return;
00293 
00294     if (ros::Time::now() - last_cloud_stamp_ > ros::Duration(timeout_))
00295     {
00296       ROS_WARN_THROTTLE(1.0, "safety_limiter: PointCloud timed-out");
00297       geometry_msgs::Twist cmd_vel;
00298       pub_twist_.publish(cmd_vel);
00299 
00300       cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00301       return;
00302     }
00303 
00304     ros::Time now = ros::Time::now();
00305     const double r_lim_current = predict(twist_);
00306 
00307     if (r_lim_current < r_lim_)
00308       r_lim_ = r_lim_current;
00309 
00310     if (r_lim_current < 1.0)
00311       hold_off_ = now + hold_;
00312     cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00313 
00314     diag_updater_.force_update();
00315   }
00316   double predict(const geometry_msgs::Twist& in)
00317   {
00318     pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
00319     pcl::VoxelGrid<pcl::PointXYZ> ds;
00320     ds.setInputCloud(cloud_);
00321     ds.setLeafSize(downsample_grid_, downsample_grid_, downsample_grid_);
00322     ds.filter(*pc);
00323 
00324     auto filter_z = [this](pcl::PointXYZ& p)
00325     {
00326       if (p.z < this->z_range_[0] || this->z_range_[1] < p.z)
00327         return true;
00328       p.z = 0.0;
00329       return false;
00330     };
00331     pc->erase(std::remove_if(pc->points.begin(), pc->points.end(), filter_z),
00332               pc->points.end());
00333 
00334     if (pc->size() == 0)
00335     {
00336       if (allow_empty_cloud_)
00337       {
00338         return 60.0;
00339       }
00340       ROS_WARN_THROTTLE(1.0, "safety_limiter: Empty pointcloud passed.");
00341       return DBL_MAX;
00342     }
00343 
00344     pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00345     kdtree.setInputCloud(pc);
00346 
00347     Eigen::Affine3f move;
00348     Eigen::Affine3f move_inv;
00349     Eigen::Affine3f motion =
00350         Eigen::AngleAxisf(-twist_.angular.z * dt_, Eigen::Vector3f::UnitZ()) *
00351         Eigen::Translation3f(Eigen::Vector3f(-twist_.linear.x * dt_, 0.0, 0.0));
00352     Eigen::Affine3f motion_inv =
00353         Eigen::Translation3f(Eigen::Vector3f(twist_.linear.x * dt_, 0.0, 0.0)) *
00354         Eigen::AngleAxisf(twist_.angular.z * dt_, Eigen::Vector3f::UnitZ());
00355     move.setIdentity();
00356     move_inv.setIdentity();
00357     sensor_msgs::PointCloud col_points;
00358     sensor_msgs::PointCloud debug_points;
00359     col_points.header.frame_id = frame_id_;
00360     col_points.header.stamp = ros::Time::now();
00361     debug_points.header = col_points.header;
00362 
00363     float d_col = 0;
00364     float yaw_col = 0;
00365     bool has_collision = false;
00366     float d_escape_remain = 0;
00367     float yaw_escape_remain = 0;
00368     has_collision_at_now_ = false;
00369 
00370     for (float t = 0; t < tmax_; t += dt_)
00371     {
00372       if (t != 0)
00373       {
00374         d_col += twist_.linear.x * dt_;
00375         d_escape_remain -= std::abs(twist_.linear.x) * dt_;
00376         yaw_col += twist_.angular.z * dt_;
00377         yaw_escape_remain -= std::abs(twist_.angular.z) * dt_;
00378         move = move * motion;
00379         move_inv = move_inv * motion_inv;
00380       }
00381 
00382       pcl::PointXYZ center;
00383       center = pcl::transformPoint(center, move_inv);
00384       geometry_msgs::Point32 p;
00385       p.x = center.x;
00386       p.y = center.y;
00387       p.z = 0.0;
00388       debug_points.points.push_back(p);
00389 
00390       std::vector<int> indices;
00391       std::vector<float> dist;
00392       const int num = kdtree.radiusSearch(center, footprint_radius_, indices, dist);
00393       if (num == 0)
00394         continue;
00395 
00396       bool colliding = false;
00397       for (auto& i : indices)
00398       {
00399         auto& p = pc->points[i];
00400         auto point = pcl::transformPoint(p, move);
00401         vec v(point.x, point.y);
00402         if (footprint_p.inside(v))
00403         {
00404           geometry_msgs::Point32 pos;
00405           pos.x = p.x;
00406           pos.y = p.y;
00407           pos.z = p.z;
00408           col_points.points.push_back(pos);
00409           colliding = true;
00410           break;
00411         }
00412       }
00413       if (colliding)
00414       {
00415         if (t == 0)
00416         {
00417           // The robot is already in collision.
00418           // Allow movement under d_escape_ and yaw_escape_
00419           d_escape_remain = d_escape_;
00420           yaw_escape_remain = yaw_escape_;
00421           has_collision_at_now_ = true;
00422         }
00423         if (d_escape_remain <= 0 || yaw_escape_remain <= 0)
00424         {
00425           if (has_collision_at_now_)
00426           {
00427             // It's not possible to escape from collision; stop completely.
00428             d_col = yaw_col = 0;
00429           }
00430 
00431           has_collision = true;
00432           break;
00433         }
00434       }
00435     }
00436     pub_debug_.publish(debug_points);
00437     pub_cloud_.publish(col_points);
00438 
00439     if (!has_collision)
00440       return 1.0;
00441 
00442     d_col = std::max<float>(std::abs(d_col) - d_margin_, 0.0);
00443     yaw_col = std::max<float>(std::abs(yaw_col) - yaw_margin_, 0.0);
00444 
00445     float d_r =
00446         (sqrtf(std::abs(2 * acc_[0] * d_col)) + EPSILON) / std::abs(twist_.linear.x);
00447     float yaw_r =
00448         (sqrtf(std::abs(2 * acc_[1] * yaw_col)) + EPSILON) / std::abs(twist_.angular.z);
00449     if (!std::isfinite(d_r))
00450       d_r = 1.0;
00451     if (!std::isfinite(yaw_r))
00452       yaw_r = 1.0;
00453 
00454     const auto r = std::min(d_r, yaw_r);
00455 
00456     return r;
00457   }
00458 
00459   geometry_msgs::Twist limit(const geometry_msgs::Twist& in)
00460   {
00461     auto out = in;
00462     if (r_lim_ < 1.0 - EPSILON)
00463     {
00464       out.linear.x *= r_lim_;
00465       out.angular.z *= r_lim_;
00466       if (std::abs(in.linear.x - out.linear.x) > EPSILON ||
00467           std::abs(in.angular.z - out.angular.z) > EPSILON)
00468       {
00469         ROS_WARN_THROTTLE(
00470             1.0, "safety_limiter: (%0.2f, %0.2f)->(%0.2f, %0.2f)",
00471             in.linear.x, in.angular.z,
00472             out.linear.x, out.angular.z);
00473       }
00474     }
00475     return out;
00476   }
00477 
00478   class vec
00479   {
00480   public:
00481     float c[2];
00482     vec(const float x, const float y)
00483     {
00484       c[0] = x;
00485       c[1] = y;
00486     }
00487     vec()
00488     {
00489       c[0] = c[1] = 0.0;
00490     }
00491     float& operator[](const int& i)
00492     {
00493       return c[i];
00494     }
00495     const float& operator[](const int& i) const
00496     {
00497       return c[i];
00498     }
00499     vec operator-(const vec& a) const
00500     {
00501       vec out = *this;
00502       out[0] -= a[0];
00503       out[1] -= a[1];
00504       return out;
00505     }
00506     float cross(const vec& a) const
00507     {
00508       return (*this)[0] * a[1] - (*this)[1] * a[0];
00509     }
00510     float dot(const vec& a) const
00511     {
00512       return (*this)[0] * a[0] + (*this)[1] * a[1];
00513     }
00514     float dist(const vec& a) const
00515     {
00516       return hypotf((*this)[0] - a[0], (*this)[1] - a[1]);
00517     }
00518     float dist_line(const vec& a, const vec& b) const
00519     {
00520       return (b - a).cross((*this) - a) / b.dist(a);
00521     }
00522     float dist_linestrip(const vec& a, const vec& b) const
00523     {
00524       if ((b - a).dot((*this) - a) <= 0)
00525         return this->dist(a);
00526       if ((a - b).dot((*this) - b) <= 0)
00527         return this->dist(b);
00528       return std::abs(this->dist_line(a, b));
00529     }
00530   };
00531   class polygon
00532   {
00533   public:
00534     std::vector<vec> v;
00535     void move(const float& x, const float& y, const float& yaw)
00536     {
00537       const float cos_v = cosf(yaw);
00538       const float sin_v = sinf(yaw);
00539       for (auto& p : v)
00540       {
00541         const auto tmp = p;
00542         p[0] = cos_v * tmp[0] - sin_v * tmp[1] + x;
00543         p[1] = sin_v * tmp[0] + cos_v * tmp[1] + y;
00544       }
00545     }
00546     bool inside(const vec& a) const
00547     {
00548       int cn = 0;
00549       for (size_t i = 0; i < v.size() - 1; i++)
00550       {
00551         auto& v1 = v[i];
00552         auto& v2 = v[i + 1];
00553         if ((v1[1] <= a[1] && a[1] < v2[1]) ||
00554             (v2[1] <= a[1] && a[1] < v1[1]))
00555         {
00556           float lx;
00557           lx = v1[0] + (v2[0] - v1[0]) * (a[1] - v1[1]) / (v2[1] - v1[1]);
00558           if (a[0] < lx)
00559             cn++;
00560         }
00561       }
00562       return ((cn & 1) == 1);
00563     }
00564     float dist(const vec& a) const
00565     {
00566       float dist = FLT_MAX;
00567       for (size_t i = 0; i < v.size() - 1; i++)
00568       {
00569         auto& v1 = v[i];
00570         auto& v2 = v[i + 1];
00571         auto d = a.dist_linestrip(v1, v2);
00572         if (d < dist)
00573           dist = d;
00574       }
00575       return dist;
00576     }
00577   };
00578 
00579   polygon footprint_p;
00580 
00581   void cbTwist(const geometry_msgs::Twist::ConstPtr& msg)
00582   {
00583     ros::Time now = ros::Time::now();
00584 
00585     twist_ = *msg;
00586     has_twist_ = true;
00587 
00588     if (now - last_disable_cmd_ < ros::Duration(disable_timeout_))
00589     {
00590       pub_twist_.publish(twist_);
00591     }
00592     else if (ros::Time::now() - last_cloud_stamp_ > ros::Duration(timeout_) || watchdog_stop_)
00593     {
00594       geometry_msgs::Twist cmd_vel;
00595       pub_twist_.publish(cmd_vel);
00596     }
00597     else
00598     {
00599       geometry_msgs::Twist cmd_vel = limit(twist_);
00600       pub_twist_.publish(cmd_vel);
00601 
00602       if (now > hold_off_)
00603         r_lim_ = 1.0;
00604     }
00605   }
00606   void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
00607   {
00608     pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>());
00609 
00610     try
00611     {
00612       sensor_msgs::PointCloud2 pc2_tmp;
00613       geometry_msgs::TransformStamped trans = tfbuf_.lookupTransform(
00614           frame_id_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
00615       tf2::doTransform(*msg, pc2_tmp, trans);
00616       pcl::fromROSMsg(pc2_tmp, *pc);
00617     }
00618     catch (tf2::TransformException& e)
00619     {
00620       ROS_WARN_THROTTLE(1.0, "safety_limiter: Transform failed: %s", e.what());
00621       return;
00622     }
00623 
00624     *cloud_ += *pc;
00625     last_cloud_stamp_ = msg->header.stamp;
00626     has_cloud_ = true;
00627   }
00628   void cbDisable(const std_msgs::Bool::ConstPtr& msg)
00629   {
00630     if (msg->data)
00631     {
00632       last_disable_cmd_ = ros::Time::now();
00633     }
00634   }
00635 
00636   void diagnoseCollision(diagnostic_updater::DiagnosticStatusWrapper& stat)
00637   {
00638     if (r_lim_ == 1.0)
00639     {
00640       stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
00641     }
00642     else if (r_lim_ == 0.0)
00643     {
00644       stat.summary(diagnostic_msgs::DiagnosticStatus::WARN,
00645                    (has_collision_at_now_) ?
00646                        "Cannot escape from collision." :
00647                        "Trying to avoid collision, but cannot move anymore.");
00648     }
00649     else
00650     {
00651       stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00652                    (has_collision_at_now_) ?
00653                        "Escaping from collision." :
00654                        "Reducing velocity to avoid collision.");
00655     }
00656     stat.addf("Velocity Limit Ratio", "%.2f", r_lim_);
00657   }
00658 };
00659 
00660 int main(int argc, char** argv)
00661 {
00662   ros::init(argc, argv, "safety_limiter");
00663 
00664   SafetyLimiterNode limiter;
00665   limiter.spin();
00666 
00667   return 0;
00668 }


safety_limiter
Author(s): Atsushi Watanabe
autogenerated on Sat Jun 22 2019 20:07:21