void initialize(UAS &uas_)
{
PluginBase::initialize(uas_);
bool manual_def = false;
double p1x, p1y, p1z,
p2x, p2y, p2z;
if (safety_nh.getParam("p1/x", p1x) &&
safety_nh.getParam("p1/y", p1y) &&
safety_nh.getParam("p1/z", p1z)) {
manual_def = true;
ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P1(%f %f %f)",
p1x, p1y, p1z);
}
if (manual_def &&
safety_nh.getParam("p2/x", p2x) &&
safety_nh.getParam("p2/y", p2y) &&
safety_nh.getParam("p2/z", p2z)) {
manual_def = true;
ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P2(%f %f %f)",
p2x, p2y, p2z);
}
else
manual_def = false;
if (manual_def)
send_safety_set_allowed_area(
Eigen::Vector3d(p1x, p1y, p1z),
Eigen::Vector3d(p2x, p2y, p2z));
safetyarea_sub = safety_nh.subscribe("set", 10, &SafetyAreaPlugin::safetyarea_cb, this);
}
void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req) {
if (req->polygon.points.size() != 2) {
ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points");
return;
}
send_safety_set_allowed_area(
req->polygon.points[0].x,
req->polygon.points[0].y,
req->polygon.points[0].z,
req->polygon.points[1].x,
req->polygon.points[1].y,
req->polygon.points[1].z);
}
void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req)
{
if (req->polygon.points.size() != 2) {
ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points");
return;
}
// eigen_conversions do not have convertor for Point32
// [[[cog:
// for p in range(2):
// cog.outl("Eigen::Vector3d p%d(%s);" % (p + 1, ', '.join([
// 'req->polygon.points[%d].%s' % (p, v) for v in ('x', 'y', 'z')
// ])))
// ]]]
Eigen::Vector3d p1(req->polygon.points[0].x, req->polygon.points[0].y, req->polygon.points[0].z);
Eigen::Vector3d p2(req->polygon.points[1].x, req->polygon.points[1].y, req->polygon.points[1].z);
// [[[end]]] (checksum: c3681d584e02f7d91d6b3b48f87b1771)
send_safety_set_allowed_area(p1, p2);
}
void initialize(UAS &uas_,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
bool manual_def = false;
double p1x, p1y, p1z,
p2x, p2y, p2z;
uas = &uas_;
safety_nh = ros::NodeHandle(nh, "safety_area");
if (safety_nh.getParam("p1/x", p1x) &&
safety_nh.getParam("p1/y", p1y) &&
safety_nh.getParam("p1/z", p1z)) {
manual_def = true;
ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P1(%f %f %f)",
p1x, p1y, p1z);
}
if (manual_def &&
safety_nh.getParam("p2/x", p2x) &&
safety_nh.getParam("p2/y", p2y) &&
safety_nh.getParam("p2/z", p2z)) {
manual_def = true;
ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P2(%f %f %f)",
p2x, p2y, p2z);
}
else
manual_def = false;
if (manual_def)
send_safety_set_allowed_area(
p1x, p1y, p1z,
p2x, p2y, p2z);
safetyarea_sub = safety_nh.subscribe("set", 10, &SafetyAreaPlugin::safetyarea_cb, this);
}
