add a method to retrieve maximal velocity of a transmission joint

include/urdf_geometry_parser/urdf_geometry_parser.h

@@ -76,7 +76,7 @@ class UrdfGeometryParser {
    * \return true if the radius was found; false otherwise
    */
   bool getJointRadius(const std::string& joint_name,
-                                double& radius);
+                      double& radius);
 
   /**
    * \brief Get joint steering limit from the URDF
@@ -87,8 +87,21 @@ class UrdfGeometryParser {
    */
   bool getJointSteeringLimits(const std::string& joint_name,
                               double& steering_limit);
-private:
-  std::string base_link_;
+
+
+  /**
+   * \brief Get joint angular speed limit from the URDF
+   * \param joint_name Name of the joint
+   * \param angular_speed_limit [rad/s]
+   * \return true if the angular speed limit was found; false otherwise
+   */
+
+  bool getJointAngularSpeedLimit(const std::string& joint_name,
+                                 double & angular_speed_limit);
+
+
+  private:
+    std::string base_link_;
 
   urdf::ModelInterfaceSharedPtr model_;
 };

src/urdf_geometry_parser.cpp

@@ -201,4 +201,22 @@ namespace urdf_geometry_parser{
     }
     return false;
   }
+
+  bool UrdfGeometryParser::getJointAngularSpeedLimit(const std::string& joint_name,
+                                                     double & angular_speed_limit)
+  {
+    if(model_)
+    {
+      urdf::JointConstSharedPtr joint(model_->getJoint(joint_name));
+      if(joint->type == urdf::Joint::CONTINUOUS)
+      {
+        angular_speed_limit = fabs(joint->limits->velocity);
+        ROS_DEBUG_STREAM("Joint "<<joint_name<<" speed limit is "<<angular_speed_limit*180.0/M_PI<<" in degrees");
+        return true;
+      }
+      ROS_ERROR_STREAM("Couldn't get joint "<<joint_name<<" angular speed limit, is it of type CONTINOUS ?");
+    }
+    return false;
+  }
+
 }