From 41318597353a99c6667daedef36e294f95b184ff Mon Sep 17 00:00:00 2001
From: Jean Laneurit <jean.laneurit@irstea.fr>
Date: Tue, 31 Oct 2017 12:52:56 +0100
Subject: [PATCH 1/2] add a method to retrieve maximal velocity of a
 transmission joint

---
 .../urdf_geometry_parser.h                    | 19 ++++++++++++++++---
 src/urdf_geometry_parser.cpp                  | 18 ++++++++++++++++++
 2 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/include/urdf_geometry_parser/urdf_geometry_parser.h b/include/urdf_geometry_parser/urdf_geometry_parser.h
index f4abb45..cdf0c5e 100644
--- a/include/urdf_geometry_parser/urdf_geometry_parser.h
+++ b/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_;
 };
diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp
index 3d7e582..550aa40 100644
--- a/src/urdf_geometry_parser.cpp
+++ b/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;
+  }
+
 }

From 9b598a2984bd9c8b1c3810b7aa7f593b4ec50b1f Mon Sep 17 00:00:00 2001
From: Jean Laneurit <jean.laneurit@irstea.fr>
Date: Thu, 4 Jan 2018 11:14:44 +0100
Subject: [PATCH 2/2] add get angular speed for revolute joint

---
 src/urdf_geometry_parser.cpp | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp
index 550aa40..8f462e8 100644
--- a/src/urdf_geometry_parser.cpp
+++ b/src/urdf_geometry_parser.cpp
@@ -208,13 +208,9 @@ namespace urdf_geometry_parser{
     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 ?");
+      angular_speed_limit = fabs(joint->limits->velocity);
+      ROS_DEBUG_STREAM("Joint "<<joint_name<<" angular speed limit is "<<angular_speed_limit*180.0/M_PI<<" in degrees");
+      return true;
     }
     return false;
   }