39#include <pluginlib/class_loader.hpp>
42#include <boost/graph/adjacency_list.hpp>
43#include <boost/graph/depth_first_search.hpp>
56 template <
class Edge,
class Graph>
69 config_data_->registerType(
"group_meta",
"moveit_setup::srdf_setup::GroupMetaConfig");
76 rename(old_group_name, new_group_name);
82 for (srdf::Model::Group& group :
srdf_config_->getGroups())
85 for (std::string& subgroup : group.subgroups_)
88 if (subgroup == old_group_name)
90 subgroup.assign(new_group_name);
97 for (srdf::Model::EndEffector& eef :
srdf_config_->getEndEffectors())
100 if (eef.parent_group_ == old_group_name)
102 RCLCPP_DEBUG_STREAM(*
logger_,
"Changed eef '" << eef.name_ <<
"' to new parent group name " << new_group_name);
103 eef.parent_group_ = new_group_name;
108 if (eef.component_group_.compare(old_group_name) == 0)
110 RCLCPP_DEBUG_STREAM(*
logger_,
"Changed eef '" << eef.name_ <<
"' to new group name " << new_group_name);
111 eef.component_group_ = new_group_name;
117 for (srdf::Model::GroupState& gs :
srdf_config_->getGroupStates())
120 if (gs.group_ == old_group_name)
122 RCLCPP_DEBUG_STREAM(*
logger_,
"Changed group state group '" << gs.group_ <<
"' to new parent group name "
124 gs.group_ = new_group_name;
148 auto it = eefs.begin();
149 while (it != eefs.end())
151 if (it->component_group_ == group_name)
166 for (srdf::Model::Group& group :
srdf_config_->getGroups())
168 auto& subgroups = group.subgroups_;
169 std::vector<std::string>::iterator subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name);
170 while (subgroup_it != subgroups.end())
172 subgroups.erase(subgroup_it);
174 subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name);
186 srdf::Model::Group* searched_group =
find(group_name);
189 searched_group->joints_ = joint_names;
198 srdf::Model::Group* searched_group =
find(group_name);
201 searched_group->links_ = link_names;
210 if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty()))
212 throw std::runtime_error(
"You must specify a link for both the base and tip, or leave both "
217 if (!tip.empty() && !base.empty())
220 if (tip.compare(base) == 0)
222 throw std::runtime_error(
"Tip and base link cannot be the same link.");
225 bool found_tip =
false;
226 bool found_base =
false;
229 for (
const std::string& link : links)
232 if (link.compare(tip) == 0)
236 else if (link.compare(base) == 0)
242 if (found_tip && found_base)
247 if (!found_tip || !found_base)
249 throw std::runtime_error(
"Tip or base link(s) were not found in kinematic chain.");
254 srdf::Model::Group* searched_group =
find(group_name);
257 searched_group->chains_.clear();
260 if (!tip.empty() && !base.empty())
262 searched_group->chains_.push_back(std::pair<std::string, std::string>(base, tip));
274 std::map<std::string, int> group_nodes;
281 group_nodes.insert(std::pair<std::string, int>(group_name, node_id));
286 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS> Graph;
287 Graph g(group_nodes.size());
291 for (srdf::Model::Group& group :
srdf_config_->getGroups())
294 if (group.name_ == selected_group_name)
297 for (
const std::string& to_string : subgroups)
300 int to_id = group_nodes[to_string];
303 add_edge(from_id, to_id, g);
309 for (
const std::string& to_string : group.subgroups_)
313 int to_id = group_nodes[to_string];
316 add_edge(from_id, to_id, g);
324 bool has_cycle =
false;
326 boost::depth_first_search(g, visitor(vis));
330 throw std::runtime_error(
"Depth first search reveals a cycle in the subgroups");
334 srdf::Model::Group* searched_group =
find(selected_group_name);
337 searched_group->subgroups_ = subgroups;
366 std::vector<std::string> pose_names;
367 for (
const srdf::Model::GroupState& pose :
srdf_config_->getGroupStates())
369 if (pose.group_ == group_name)
371 pose_names.push_back(pose.name_);
379 std::vector<std::string> eef_names;
380 for (
const srdf::Model::EndEffector& eef :
srdf_config_->getEndEffectors())
382 if (eef.component_group_ == group_name)
384 eef_names.push_back(eef.name_);
393 std::unique_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase>> loader;
396 loader = std::make_unique<pluginlib::ClassLoader<kinematics::KinematicsBase>>(
"moveit_core",
397 "kinematics::KinematicsBase");
399 catch (pluginlib::PluginlibException& ex)
401 throw std::runtime_error(std::string(
"Exception while creating class loader for kinematic "
402 "solver plugins: ") +
406 std::vector<std::string> planners(loader->getDeclaredClasses());
409 if (planners.empty())
411 throw std::runtime_error(
"No MoveIt-compatible kinematics solvers found. Try "
412 "installing moveit_kinematics (sudo apt-get install "
413 "ros-${ROS_DISTRO}-moveit-kinematics)");
420 std::vector<std::string> planner_names;
423 planner_names.push_back(
"AnytimePathShortening");
424 planner_names.push_back(
"SBL");
425 planner_names.push_back(
"EST");
426 planner_names.push_back(
"LBKPIECE");
427 planner_names.push_back(
"BKPIECE");
428 planner_names.push_back(
"KPIECE");
429 planner_names.push_back(
"RRT");
430 planner_names.push_back(
"RRTConnect");
431 planner_names.push_back(
"RRTstar");
432 planner_names.push_back(
"TRRT");
433 planner_names.push_back(
"PRM");
434 planner_names.push_back(
"PRMstar");
435 planner_names.push_back(
"FMT");
436 planner_names.push_back(
"BFMT");
437 planner_names.push_back(
"PDST");
438 planner_names.push_back(
"STRIDE");
439 planner_names.push_back(
"BiTRRT");
440 planner_names.push_back(
"LBTRRT");
441 planner_names.push_back(
"BiEST");
442 planner_names.push_back(
"ProjEST");
443 planner_names.push_back(
"LazyPRM");
444 planner_names.push_back(
"LazyPRMstar");
445 planner_names.push_back(
"SPARS");
446 planner_names.push_back(
"SPARStwo");
448 return planner_names;
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
std::string getTypeName() const
Get the type of joint as a string.
DataWarehousePtr config_data_
virtual void onInit()
Overridable initialization method.
std::shared_ptr< rclcpp::Logger > logger_
void setChain(const std::string &group_name, const std::string &base, const std::string &tip)
Set the specified group's kinematic chain.
void onInit() override
Overridable initialization method.
std::vector< std::string > getGroupNames() const
void setLinks(const std::string &group_name, const std::vector< std::string > &link_names)
Set the specified group's link names.
void setSubgroups(const std::string &selected_group_name, const std::vector< std::string > &subgroups)
Set the specified group's subgroups.
void renameGroup(const std::string &old_group_name, const std::string &new_group_name)
std::string getChildOfJoint(const std::string &joint_name) const
void deleteGroup(const std::string &group_name)
std::vector< std::string > getPosesByGroup(const std::string &group_name) const
std::string getJointType(const std::string &joint_name) const
std::shared_ptr< GroupMetaConfig > group_meta_config_
std::vector< std::string > getKinematicPlanners() const
std::vector< std::string > getOMPLPlanners() const
const std::vector< std::string > & getLinkNames() const
std::vector< std::string > getEndEffectorsByGroup(const std::string &group_name) const
void setJoints(const std::string &group_name, const std::vector< std::string > &joint_names)
Set the specified group's joint names.
LinkNameTree getLinkNameTree() const
std::shared_ptr< SRDFConfig > srdf_config_
srdf::Model::Group * find(const std::string &name)
Return a pointer to an item with the given name if it exists, otherwise null.
srdf::Model::Group * rename(const std::string &old_name, const std::string &new_name)
Renames an item and returns a pointer to the item.
bool remove(const std::string &name)
Delete an item with the given name from the list.
LinkNameTree buildLinkNameTree(const moveit::core::LinkModel *link)
CycleDetector(bool &has_cycle)
void backEdge(Edge, Graph &)