39 #include <rclcpp/logger.hpp>
40 #include <rclcpp/logging.hpp>
56 bool operator()(
const ConstraintSamplerPtr& a,
const ConstraintSamplerPtr& b)
const
58 const std::vector<std::string>& alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
59 const std::vector<std::string>& blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
60 std::set<std::string> a_updates(alinks.begin(), alinks.end());
61 std::set<std::string> b_updates(blinks.begin(), blinks.end());
63 bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(), b_updates.begin(), b_updates.end());
65 bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(), a_updates.begin(), a_updates.end());
68 if (a_contains_b && !b_contains_a)
70 if (b_contains_a && !a_contains_b)
74 bool a_depends_on_b =
false;
75 bool b_depends_on_a =
false;
76 const std::vector<std::string>& fda = a->getFrameDependency();
77 const std::vector<std::string>& fdb = b->getFrameDependency();
78 for (std::size_t i = 0; i < fda.size() && !a_depends_on_b; ++i)
80 for (
const std::string& blink : blinks)
84 a_depends_on_b =
true;
89 for (std::size_t i = 0; i < fdb.size() && !b_depends_on_a; ++i)
91 for (
const std::string& alink : alinks)
95 b_depends_on_a =
true;
100 if (b_depends_on_a && a_depends_on_b)
103 "Circular frame dependency! "
104 "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
105 a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
108 if (b_depends_on_a && !a_depends_on_b)
110 if (a_depends_on_b && !b_depends_on_a)
116 if (ja && jb ==
nullptr)
118 if (jb && ja ==
nullptr)
122 return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
127 const std::string& group_name,
128 const std::vector<ConstraintSamplerPtr>& samplers)
134 for (ConstraintSamplerPtr& sampler :
samplers_)
136 const std::vector<std::string>& fds = sampler->getFrameDependency();
137 for (
const std::string& fd : fds)
140 RCLCPP_DEBUG(
getLogger(),
"Union sampler for group '%s' includes sampler for group '%s'",
jmg_->
getName().c_str(),
141 sampler->getJointModelGroup()->getName().c_str());
146 unsigned int max_attempts)
148 state = reference_state;
149 for (ConstraintSamplerPtr& sampler :
samplers_)
155 if (!sampler->sample(state, max_attempts))
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn.
const moveit::core::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces a sample from all configured samplers.
std::vector< ConstraintSamplerPtr > samplers_
Holder for sorted internal list of samplers.
UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::vector< ConstraintSamplerPtr > &samplers)
Constructor, which will re-order its internal list of samplers on construction.
const std::string & getName() const
Get the name of the joint group.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
rclcpp::Logger getLogger()
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const