add some of the functions suggested in #86

moveit · Sep 10, 2013 · c367214 · c367214
1 parent 81814cf
commit c367214
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 4 deletions.
diff --git a/robot_model/include/moveit/robot_model/robot_model.h b/robot_model/include/moveit/robot_model/robot_model.h
@@ -130,10 +130,13 @@ class RobotModel
   /** \brief Check if a joint exists. Return true if it does. */
   bool hasJointModel(const std::string &name) const;
 
-  /** \brief Get a joint by its name. Throw an exception when the joint is missing. */
+  /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */
   const JointModel* getJointModel(const std::string &joint) const;
 
-  /** \brief Get a joint by its name. Throw an exception when the joint is missing. */
+  /** \brief Get a joint by its index. Output error and return NULL when the link is missing. */
+  const JointModel* getJointModel(int index) const;
+
+  /** \brief Get a joint by its name. Output error and return NULL when the joint is missing. */
   JointModel* getJointModel(const std::string &joint);
 
   /** \brief Get the array of joints, in the order they appear
@@ -228,10 +231,13 @@ class RobotModel
   /** \brief Check if a link exists. Return true if it does. */
   bool hasLinkModel(const std::string &name) const;
 
-  /** \brief Get a link by its name. Throw an exception when the link is missing. */
+  /** \brief Get a link by its name. Output error and return NULL when the link is missing. */
   const LinkModel* getLinkModel(const std::string &link) const;
 
-  /** \brief Get a link by its name. Throw an exception when the link is missing. */
+  /** \brief Get a link by its index. Output error and return NULL when the link is missing. */
+  const LinkModel* getLinkModel(int index) const;
+
+  /** \brief Get a link by its name. Output error and return NULL when the link is missing. */
   LinkModel* getLinkModel(const std::string &link);
 
   /** \brief Get the array of links  */

diff --git a/robot_model/src/robot_model.cpp b/robot_model/src/robot_model.cpp
@@ -1053,6 +1053,17 @@ const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const st
   return NULL;
 }
 
+const moveit::core::JointModel* moveit::core::RobotModel::getJointModel(int index) const
+{
+  if (index < 0 || index >= joint_model_vector_.size())
+  {
+    logError("Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
+    return NULL;
+  } 
+  assert(joint_model_vector_[index]->getJointIndex() == index);
+  return joint_model_vector_[index];
+}
+
 moveit::core::JointModel* moveit::core::RobotModel::getJointModel(const std::string &name)
 {
   JointModelMap::const_iterator it = joint_model_map_.find(name);
@@ -1071,6 +1082,17 @@ const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std:
   return NULL;  
 }
 
+const moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(int index) const
+{
+  if (index < 0 || index >= link_model_vector_.size())
+  {
+    logError("Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
+    return NULL;
+  } 
+  assert(link_model_vector_[index]->getLinkIndex() == index);
+  return link_model_vector_[index];
+}
+
 moveit::core::LinkModel* moveit::core::RobotModel::getLinkModel(const std::string &name)
 {
   LinkModelMap::const_iterator it = link_model_map_.find(name);