From e58f012c9d7663ca0595923782e5fbd1d7a6e2a6 Mon Sep 17 00:00:00 2001
From: Frantisek Brabec <frantisek.brabec@robotika.cz>
Date: Sat, 17 Apr 2021 11:14:04 -0400
Subject: [PATCH] add support for spherical wheels

---
 src/urdf_geometry_parser.cpp | 66 +++++++++++++++++++++++++++++-------
 1 file changed, 53 insertions(+), 13 deletions(-)

diff --git a/src/urdf_geometry_parser.cpp b/src/urdf_geometry_parser.cpp
index c375673..d351767 100644
--- a/src/urdf_geometry_parser.cpp
+++ b/src/urdf_geometry_parser.cpp
@@ -39,15 +39,15 @@
 namespace urdf_geometry_parser{
 
   /*
-   * \brief Check if the link is modeled as a cylinder
-   * \param link Link
-   * \return true if the link is modeled as a Cylinder; false otherwise
-   */
-  static bool isCylinder(urdf::LinkConstSharedPtr& link)
+  * \brief Check that a link exists and has a geometry collision.
+  * \param link The link
+  * \return true if the link has a collision element with geometry
+  */
+  static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link)
   {
     if (!link)
     {
-      ROS_ERROR("Link == NULL.");
+      ROS_ERROR("Link pointer is null.");
       return false;
     }
 
@@ -62,10 +62,45 @@ namespace urdf_geometry_parser{
       ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
       return false;
     }
+    return true;
+  }
+
+  /*
+  * \brief Check if the link is modeled as a cylinder
+  * \param link Link
+  * \return true if the link is modeled as a Cylinder; false otherwise
+  */
+  static bool isCylinder(const urdf::LinkConstSharedPtr& link)
+  {
+    if (!hasCollisionGeometry(link))
+    {
+      return false;
+    }
 
     if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
     {
-      ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
+      ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry");
+      return false;
+    }
+
+    return true;
+  }
+
+  /*
+  * \brief Check if the link is modeled as a sphere
+  * \param link Link
+  * \return true if the link is modeled as a Sphere; false otherwise
+  */
+  static bool isSphere(const urdf::LinkConstSharedPtr& link)
+  {
+    if (!hasCollisionGeometry(link))
+    {
+      return false;
+    }
+
+    if (link->collision->geometry->type != urdf::Geometry::SPHERE)
+    {
+      ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere geometry");
       return false;
     }
 
@@ -78,16 +113,21 @@ namespace urdf_geometry_parser{
    * \param [out] wheel_radius Wheel radius [m]
    * \return true if the wheel radius was found; false otherwise
    */
-  static bool getWheelRadius(urdf::LinkConstSharedPtr wheel_link, double& wheel_radius)
+  static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
   {
-    if (!isCylinder(wheel_link))
+    if (isCylinder(wheel_link))
     {
-      ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
-      return false;
+      wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
+      return true;
+    }
+    else if (isSphere(wheel_link))
+    {
+      wheel_radius = (static_cast<urdf::Sphere*>(wheel_link->collision->geometry.get()))->radius;
+      return true;
     }
 
-    wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
-    return true;
+    ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!");
+    return false;
   }