41 const planning_scene::PlanningSceneConstPtr& scene,
42 const std::string& group,
const std::string& link_name,
43 unsigned int sample_count, visualization_msgs::MarkerArray& markers)
45 visualizeDistribution(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState(),
46 link_name, sample_count, markers);
50 const planning_scene::PlanningSceneConstPtr& scene,
51 const std::string& group)
54 scene->getCurrentState());
62 ROS_ERROR_NAMED(
"constraint_samplers",
"No sampler specified for counting samples per second");
66 unsigned long int valid = 0;
67 unsigned long int total = 0;
71 static const unsigned int N = 10;
73 for (
unsigned int i = 0; i < N; ++i)
75 if (sampler->sample(ks, 1))
79 return (
double)valid / (double)total;
84 const std::string& link_name,
unsigned int sample_count,
85 visualization_msgs::MarkerArray& markers)
89 ROS_ERROR_NAMED(
"constraint_samplers",
"No sampler specified for visualizing distribution of samples");
96 std_msgs::ColorRGBA color;
101 for (
unsigned int i = 0; i < sample_count; ++i)
103 if (!sampler->sample(ks))
106 visualization_msgs::Marker mk;
108 mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
109 mk.ns =
"constraint_samples";
111 mk.type = visualization_msgs::Marker::SPHERE;
112 mk.action = visualization_msgs::Marker::ADD;
113 mk.pose.position.x = pos.x();
114 mk.pose.position.y = pos.y();
115 mk.pose.position.z = pos.z();
116 mk.pose.orientation.w = 1.0;
117 mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
120 markers.markers.push_back(mk);