Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/constraint_samplers/constraint_sampler_tools.h>
00038 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
00039
00040 void constraint_samplers::visualizeDistribution(const moveit_msgs::Constraints &constr, const planning_scene::PlanningSceneConstPtr &scene, const std::string &group,
00041 const std::string &link_name, unsigned int sample_count, visualization_msgs::MarkerArray &markers)
00042 {
00043 visualizeDistribution(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState(), link_name, sample_count, markers);
00044 }
00045
00046
00047 double constraint_samplers::countSamplesPerSecond(const moveit_msgs::Constraints &constr, const planning_scene::PlanningSceneConstPtr &scene, const std::string &group)
00048 {
00049 return countSamplesPerSecond(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState());
00050 }
00051
00052 double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr &sampler, const robot_state::RobotState &reference_state)
00053 {
00054 if (!sampler)
00055 {
00056 logError("No sampler specified for counting samples per second");
00057 return 0.0;
00058 }
00059 robot_state::RobotState ks(reference_state);
00060 unsigned long int valid = 0;
00061 unsigned long int total = 0;
00062 ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0);
00063 do
00064 {
00065 static const unsigned int n = 10;
00066 total += n;
00067 for (unsigned int i = 0 ; i < n ; ++i)
00068 {
00069 if (sampler->sample(ks, 1))
00070 valid++;
00071 }
00072 }
00073 while (ros::WallTime::now() < end);
00074 return (double)valid / (double)total;
00075 }
00076
00077 void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr &sampler, const robot_state::RobotState &reference_state,
00078 const std::string &link_name, unsigned int sample_count, visualization_msgs::MarkerArray &markers)
00079 {
00080 if (!sampler)
00081 {
00082 logError("No sampler specified for visualizing distribution of samples");
00083 return;
00084 }
00085 const robot_state::LinkModel *lm = reference_state.getLinkModel(link_name);
00086 if (!lm)
00087 return;
00088 robot_state::RobotState ks(reference_state);
00089 std_msgs::ColorRGBA color;
00090 color.r = 1.0f;
00091 color.g = 0.0f;
00092 color.b = 0.0f;
00093 color.a = 1.0f;
00094 for (unsigned int i = 0 ; i < sample_count ; ++i)
00095 {
00096 if (!sampler->sample(ks))
00097 continue;
00098 const Eigen::Vector3d &pos = ks.getGlobalLinkTransform(lm).translation();
00099 visualization_msgs::Marker mk;
00100 mk.header.stamp = ros::Time::now();
00101 mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
00102 mk.ns = "constraint_samples";
00103 mk.id = i;
00104 mk.type = visualization_msgs::Marker::SPHERE;
00105 mk.action = visualization_msgs::Marker::ADD;
00106 mk.pose.position.x = pos.x();
00107 mk.pose.position.y = pos.y();
00108 mk.pose.position.z = pos.z();
00109 mk.pose.orientation.w = 1.0;
00110 mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
00111 mk.color = color;
00112 mk.lifetime = ros::Duration(30.0);
00113 markers.markers.push_back(mk);
00114 }
00115 }