39 #include <rclcpp/clock.hpp>
40 #include <rclcpp/duration.hpp>
41 #include <rclcpp/logger.hpp>
42 #include <rclcpp/logging.hpp>
43 #include <rclcpp/time.hpp>
57 const planning_scene::PlanningSceneConstPtr& scene,
const std::string& group,
58 const std::string& link_name,
unsigned int sample_count,
59 visualization_msgs::msg::MarkerArray& markers)
62 link_name, sample_count, markers);
66 const planning_scene::PlanningSceneConstPtr& scene,
const std::string& group)
69 scene->getCurrentState());
76 RCLCPP_ERROR(
getLogger(),
"No sampler specified for counting samples per second");
80 unsigned long int valid = 0;
81 unsigned long int total = 0;
82 rclcpp::Time end = rclcpp::Clock().now() + rclcpp::Duration::from_seconds(1);
85 static const unsigned int N = 10;
87 for (
unsigned int i = 0; i < N; ++i)
89 if (sampler->sample(ks, 1))
92 }
while (rclcpp::Clock().now() < end);
93 return static_cast<double>(valid) /
static_cast<double>(total);
97 const std::string& link_name,
unsigned int sample_count,
98 visualization_msgs::msg::MarkerArray& markers)
102 RCLCPP_ERROR(
getLogger(),
"No sampler specified for visualizing distribution of samples");
109 std_msgs::msg::ColorRGBA color;
114 for (
unsigned int i = 0; i < sample_count; ++i)
116 if (!sampler->sample(ks))
119 visualization_msgs::msg::Marker mk;
120 mk.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
121 mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
122 mk.ns =
"constraint_samples";
124 mk.type = visualization_msgs::msg::Marker::SPHERE;
125 mk.action = visualization_msgs::msg::Marker::ADD;
126 mk.pose.position.x = pos.x();
127 mk.pose.position.y = pos.y();
128 mk.pose.position.z = pos.z();
129 mk.pose.orientation.w = 1.0;
130 mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
132 mk.lifetime = rclcpp::Duration::from_seconds(30);
133 markers.markers.push_back(mk);
static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::msg::Constraints &constr)
Default logic to select a ConstraintSampler given a constraints message.
A link from the robot. Contains the constant transform applied to the link and its geometry.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
rclcpp::Logger getLogger()
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
double countSamplesPerSecond(const ConstraintSamplerPtr &sampler, const moveit::core::RobotState &reference_state)
void visualizeDistribution(const ConstraintSamplerPtr &sampler, const moveit::core::RobotState &reference_state, const std::string &link_name, unsigned int sample_count, visualization_msgs::msg::MarkerArray &markers)
Vec3fX< details::Vec3Data< double > > Vector3d
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.