void kinematic_constraints::VisibilityConstraint::getMarkers(const planning_models::KinematicState &state, visualization_msgs::MarkerArray &markers) const
{
shapes::Mesh *m = getVisibilityCone(state);
visualization_msgs::Marker mk;
shapes::constructMarkerFromShape(m, mk);
delete m;
mk.header.frame_id = kmodel_->getModelFrame();
mk.header.stamp = ros::Time::now();
mk.ns = "constraints";
mk.id = 1;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = 0;
mk.pose.position.y = 0;
mk.pose.position.z = 0;
mk.pose.orientation.x = 0;
mk.pose.orientation.y = 0;
mk.pose.orientation.z = 0;
mk.pose.orientation.w = 1;
mk.lifetime = ros::Duration(60);
mk.color.a = 0.5;
mk.color.r = 1.0;
mk.color.g = 0.0;
mk.color.b = 0.0;
markers.markers.push_back(mk);
const Eigen::Affine3d &sp = mobile_sensor_frame_ ? tf_->getTransform(state, sensor_frame_id_) * sensor_pose_ : sensor_pose_;
const Eigen::Affine3d &tp = mobile_target_frame_ ? tf_->getTransform(state, target_frame_id_) * target_pose_ : target_pose_;
visualization_msgs::Marker mka;
mka.type = visualization_msgs::Marker::LINE_STRIP;
mka.action = visualization_msgs::Marker::ADD;
mka.color = mk.color;
mka.pose = mk.pose;
mka.header = mk.header;
mka.ns = mk.ns;
mka.id = 2;
mka.lifetime = mk.lifetime;
mka.scale.x = mka.scale.y = mka.scale.z = 0.05;
mka.colors.resize(2, mk.color);
mka.colors[1].g = 1.0;
mka.colors[1].r = 0.0;
mka.points.resize(2);
Eigen::Vector3d d = tp.translation() - tp.rotation().col(2) * 0.5;
mka.points[0].x = tp.translation().x();
mka.points[0].y = tp.translation().y();
mka.points[0].z = tp.translation().z();
mka.points[1].x = d.x();
mka.points[1].y = d.y();
mka.points[1].z = d.z();
markers.markers.push_back(mka);
mka.id = 3;
d = sp.translation() + sp.rotation().col(0) * 0.5;
mka.points[0].x = sp.translation().x();
mka.points[0].y = sp.translation().y();
mka.points[0].z = sp.translation().z();
mka.points[1].x = d.x();
mka.points[1].y = d.y();
mka.points[1].z = d.z();
markers.markers.push_back(mka);
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::VisibilityConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
if (target_radius_ <= std::numeric_limits<double>::epsilon())
return ConstraintEvaluationResult(true, 0.0);
if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0)
{
const Eigen::Affine3d &sp = mobile_sensor_frame_ ? tf_->getTransform(state, sensor_frame_id_) * sensor_pose_ : sensor_pose_;
const Eigen::Affine3d &tp = mobile_target_frame_ ? tf_->getTransform(state, target_frame_id_) * target_pose_ : target_pose_;
const Eigen::Vector3d &normal2 = sp.rotation().col(sensor_view_direction_);
if (max_view_angle_ > 0.0)
{
const Eigen::Vector3d &normal1 = tp.rotation().col(2); // along Z axis
double dp = normal2.dot(normal1);
if (dp < 0.0)
{
if (verbose)
ROS_INFO("Visibility constraint is violated because the sensor is looking at the wrong side");
return ConstraintEvaluationResult(false, 0.0);
}
double ang = acos(dp);
if (max_view_angle_ < ang)
{
if (verbose)
ROS_INFO("Visibility constraint is violated because the view angle is %lf (above the maximum allowed of %lf)", ang, max_view_angle_);
return ConstraintEvaluationResult(false, 0.0);
}
}
if (max_range_angle_ > 0.0)
{
const Eigen::Vector3d &dir = (tp.translation() - sp.translation()).normalized();
double dp = normal2.dot(dir);
if (dp < 0.0)
{
if (verbose)
ROS_INFO("Visibility constraint is violated because the sensor is looking at the wrong side");
return ConstraintEvaluationResult(false, 0.0);
}
double ang = acos(dp);
if (max_range_angle_ < ang)
{
if (verbose)
ROS_INFO("Visibility constraint is violated because the range angle is %lf (above the maximum allowed of %lf)", ang, max_view_angle_);
return ConstraintEvaluationResult(false, 0.0);
}
}
}
shapes::Mesh *m = getVisibilityCone(state);
if (!m)
return ConstraintEvaluationResult(false, 0.0);
// add the visibility cone as an object
collision_detection::CollisionWorldFCL collision_world;
collision_world.addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity());
// check for collisions between the robot and the cone
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
collision_detection::AllowedCollisionMatrix acm;
acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
req.contacts = true;
req.verbose = verbose;
req.max_contacts = 1;
collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm);
if (verbose)
{
std::stringstream ss;
m->print(ss);
ROS_INFO("Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", res.collision ? "not " : "", ss.str().c_str());
}
return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0);
}
void kinematic_constraints::VisibilityConstraint::getMarkers(const robot_state::RobotState &state, visualization_msgs::MarkerArray &markers) const
{
shapes::Mesh *m = getVisibilityCone(state);
visualization_msgs::Marker mk;
shapes::constructMarkerFromShape(m, mk);
delete m;
mk.header.frame_id = kmodel_->getModelFrame();
mk.header.stamp = ros::Time::now();
mk.ns = "constraints";
mk.id = 1;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = 0;
mk.pose.position.y = 0;
mk.pose.position.z = 0;
mk.pose.orientation.x = 0;
mk.pose.orientation.y = 0;
mk.pose.orientation.z = 0;
mk.pose.orientation.w = 1;
mk.lifetime = ros::Duration(60);
// this scale necessary to make results look reasonable
mk.scale.x = .01;
mk.color.a = 1.0;
mk.color.r = 1.0;
mk.color.g = 0.0;
mk.color.b = 0.0;
markers.markers.push_back(mk);
const Eigen::Affine3d &sp = mobile_sensor_frame_ ? tf_->getTransform(state, sensor_frame_id_) * sensor_pose_ : sensor_pose_;
const Eigen::Affine3d &tp = mobile_target_frame_ ? tf_->getTransform(state, target_frame_id_) * target_pose_ : target_pose_;
visualization_msgs::Marker mka;
mka.type = visualization_msgs::Marker::ARROW;
mka.action = visualization_msgs::Marker::ADD;
mka.color = mk.color;
mka.pose = mk.pose;
mka.header = mk.header;
mka.ns = mk.ns;
mka.id = 2;
mka.lifetime = mk.lifetime;
mka.scale.x = 0.05;
mka.scale.y = .15;
mka.scale.z = 0.0;
mka.points.resize(2);
Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5;
mka.points[0].x = tp.translation().x();
mka.points[0].y = tp.translation().y();
mka.points[0].z = tp.translation().z();
mka.points[1].x = d.x();
mka.points[1].y = d.y();
mka.points[1].z = d.z();
markers.markers.push_back(mka);
mka.id = 3;
mka.color.b = 1.0;
mka.color.r = 0.0;
d = sp.translation() + sp.rotation().col(2-sensor_view_direction_) * 0.5;
mka.points[0].x = sp.translation().x();
mka.points[0].y = sp.translation().y();
mka.points[0].z = sp.translation().z();
mka.points[1].x = d.x();
mka.points[1].y = d.y();
mka.points[1].z = d.z();
markers.markers.push_back(mka);
}