bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const
{
const std::vector<std::string> &alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
const std::vector<std::string> &blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
std::set<std::string> a_updates(alinks.begin(), alinks.end());
std::set<std::string> b_updates(blinks.begin(), blinks.end());
bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(),
b_updates.begin(), b_updates.end());
bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(),
a_updates.begin(), a_updates.end());
//a contains b and sets are not equal
if (a_contains_b && !b_contains_a)
return true;
if (b_contains_a && !a_contains_b)
return false;
//sets are equal or disjoint
bool a_depends_on_b = false;
bool b_depends_on_a = false;
const std::vector<std::string> &fda = a->getFrameDependency();
const std::vector<std::string> &fdb = b->getFrameDependency();
for (std::size_t i = 0 ; i < fda.size() && !a_depends_on_b ; ++i)
for (std::size_t j = 0 ; j < blinks.size() ; ++j)
if (blinks[j] == fda[i])
{
a_depends_on_b = true;
break;
}
for (std::size_t i = 0 ; i < fdb.size() && !b_depends_on_a ; ++i)
for (std::size_t j = 0 ; j < alinks.size() ; ++j)
if (alinks[j] == fdb[i])
{
b_depends_on_a = true;
break;
}
if (b_depends_on_a && a_depends_on_b)
{
logWarn("Circular frame dependency! Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
return true;
}
if (b_depends_on_a && !a_depends_on_b)
return true;
if(a_depends_on_b && !b_depends_on_a)
return false;
// prefer sampling JointConstraints first
JointConstraintSampler *ja = dynamic_cast<JointConstraintSampler*>(a.get());
JointConstraintSampler *jb = dynamic_cast<JointConstraintSampler*>(b.get());
if (ja && jb == NULL)
return true;
if (jb && ja == NULL)
return false;
// neither depends on either, so break ties based on group name
return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
}
void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler,
const robot_state::RobotState& reference_state,
const std::string& link_name, unsigned int sample_count,
visualization_msgs::MarkerArray& markers)
{
if (!sampler)
{
ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples");
return;
}
const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name);
if (!lm)
return;
robot_state::RobotState ks(reference_state);
std_msgs::ColorRGBA color;
color.r = 1.0f;
color.g = 0.0f;
color.b = 0.0f;
color.a = 1.0f;
for (unsigned int i = 0; i < sample_count; ++i)
{
if (!sampler->sample(ks))
continue;
const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation();
visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
mk.ns = "constraint_samples";
mk.id = i;
mk.type = visualization_msgs::Marker::SPHERE;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = pos.x();
mk.pose.position.y = pos.y();
mk.pose.position.z = pos.z();
mk.pose.orientation.w = 1.0;
mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
mk.color = color;
mk.lifetime = ros::Duration(30.0);
markers.markers.push_back(mk);
}
}
double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sampler,
const robot_state::RobotState& reference_state)
{
if (!sampler)
{
ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for counting samples per second");
return 0.0;
}
robot_state::RobotState ks(reference_state);
unsigned long int valid = 0;
unsigned long int total = 0;
ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0);
do
{
static const unsigned int N = 10;
total += N;
for (unsigned int i = 0; i < N; ++i)
{
if (sampler->sample(ks, 1))
valid++;
}
} while (ros::WallTime::now() < end);
return (double)valid / (double)total;
}