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;
}