00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00418
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
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 }