using srdf info for #6

kinematic_model/src/kinematic_model.cpp

@@ -264,28 +264,61 @@ void kinematic_model::KinematicModel::buildGroupInfo(const boost::shared_ptr<con
       {
         // if it is, mark it as such
         it->second->is_end_effector_ = true;
+
+        JointModelGroup *eef_parent_group = NULL;
 
-        // check to see if there are groups that contain the parent link of this end effector.
-        // record this information if found
-        std::vector<JointModelGroup*> possible_parent_groups;
-        for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
-          if (jt->first != it->first)
+        // if a parent group is specified in SRDF, try to use it
+        if (!eefs[k].parent_group_.empty())
+        {
+          std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
+          if (jt != joint_model_group_map_.end())
           {
             if (jt->second->hasLinkModel(eefs[k].parent_link_))
-              possible_parent_groups.push_back(jt->second);
+            {
+              if (jt->second != it->second)
+                eef_parent_group = jt->second;
+              else
+                logError("Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
+            }
+            else
+              logError("Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'",
+                       eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
+          }
+          else
+            logError("Group name '%s' not found (specified as parent group for end-effector '%s')",
+                     eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
+        }
+
+        if (eef_parent_group == NULL)
+        {
+          // check to see if there are groups that contain the parent link of this end effector.
+          // record this information if found
+          std::vector<JointModelGroup*> possible_parent_groups;
+          for (std::map<std::string, JointModelGroup*>::const_iterator jt = joint_model_group_map_.begin() ; jt != joint_model_group_map_.end(); ++jt)
+            if (jt->first != it->first)
+            {
+              if (jt->second->hasLinkModel(eefs[k].parent_link_))
+                possible_parent_groups.push_back(jt->second);
+            }
+          if (!possible_parent_groups.empty())
+          {
+            // if there are multiple options for the group that contains this end-effector, 
+            // we pick the group with fewest joints.
+            std::size_t best = 0;
+            for (std::size_t g = 1 ; g < possible_parent_groups.size() ; ++g)
+              if (possible_parent_groups[g]->getJointModels().size() < possible_parent_groups[best]->getJointModels().size())
+                best = g;
+            eef_parent_group = possible_parent_groups[best];
           }
-        if (!possible_parent_groups.empty())
+        }
+        if (eef_parent_group)
         {
-          // if there are multiple options for the group that contains this end-effector, 
-          // we pick the group with fewest joints.
-          std::size_t best = 0;
-          for (std::size_t g = 1 ; g < possible_parent_groups.size() ; ++g)
-            if (possible_parent_groups[g]->getJointModels().size() < possible_parent_groups[best]->getJointModels().size())
-              best = g;
-          possible_parent_groups[best]->attached_end_effector_group_name_ = it->first;
-          it->second->end_effector_parent_.first = possible_parent_groups[best]->getName();
-          it->second->end_effector_parent_.second = eefs[k].parent_link_;
+          eef_parent_group->attached_end_effector_group_name_ = it->first;
+          it->second->end_effector_parent_.first = eef_parent_group->getName();
         }
+        else
+          logWarn("Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
+        it->second->end_effector_parent_.second = eefs[k].parent_link_;
         break;
       }
     // check to see if the group is a chain