From d4da298b0b245cff901b2b6b760a1277d6b195b8 Mon Sep 17 00:00:00 2001
From: Chris Iverach-Brereton <civerachb@clearpathrobotics.com>
Date: Wed, 30 Apr 2025 16:35:45 -0400
Subject: [PATCH 1/2] Add `rgba` parameter to links so additional shapes aren't
 always dark grey

---
 .../description/links.py                       | 18 ++++++++++++------
 .../urdf/links/box.urdf.xacro                  |  7 ++++---
 .../urdf/links/cylinder.urdf.xacro             |  7 ++++---
 .../urdf/links/mesh.urdf.xacro                 |  7 ++++---
 .../urdf/links/sphere.urdf.xacro               |  7 ++++---
 5 files changed, 28 insertions(+), 18 deletions(-)

diff --git a/clearpath_generator_common/clearpath_generator_common/description/links.py b/clearpath_generator_common/clearpath_generator_common/description/links.py
index 827badaa..eccb5605 100644
--- a/clearpath_generator_common/clearpath_generator_common/description/links.py
+++ b/clearpath_generator_common/clearpath_generator_common/description/links.py
@@ -48,6 +48,7 @@ class BaseDescription():
 
         NAME = 'name'
         PARENT_LINK = 'parent_link'
+        RGBA = 'rgba'
 
         def __init__(self, link: BaseLink) -> None:
             self.link = link
@@ -57,7 +58,7 @@ def __init__(self, link: BaseLink) -> None:
 
             self.parameters = {
                 self.NAME: self.link.name,
-                self.PARENT_LINK: self.link.parent
+                self.PARENT_LINK: self.link.parent,
             }
 
         @property
@@ -74,7 +75,8 @@ class BoxDescription(BaseDescription):
         def __init__(self, link: Box) -> None:
             super().__init__(link)
             self.parameters.update({
-                self.SIZE: str(link.size).strip('[]').replace(',', '')
+                self.SIZE: str(link.size).strip('[]').replace(',', ''),
+                self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
             })
 
     class CylinderDescription(BaseDescription):
@@ -85,7 +87,8 @@ def __init__(self, link: Cylinder) -> None:
             super().__init__(link)
             self.parameters.update({
                 self.RADIUS: link.radius,
-                self.LENGTH: link.length
+                self.LENGTH: link.length,
+                self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
             })
 
     class SphereDescription(BaseDescription):
@@ -94,7 +97,8 @@ class SphereDescription(BaseDescription):
         def __init__(self, link: Sphere) -> None:
             super().__init__(link)
             self.parameters.update({
-                self.RADIUS: link.radius
+                self.RADIUS: link.radius,
+                self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
             })
 
     class MeshDescription(BaseDescription):
@@ -105,11 +109,13 @@ def __init__(self, link: Mesh) -> None:
             if (link.visual.package):
                 self.parameters.update({
                     self.VISUAL: os.path.join('package://' + link.visual.package,
-                                              File.clean(link.visual.path, make_abs=False))
+                                              File.clean(link.visual.path, make_abs=False)),
+                    self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
                 })
             else:
                 self.parameters.update({
-                    self.VISUAL: 'file://' + File.clean(link.visual.path, make_abs=False)
+                    self.VISUAL: 'file://' + File.clean(link.visual.path, make_abs=False),
+                    self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
                 })
 
     MODEL = {
diff --git a/clearpath_platform_description/urdf/links/box.urdf.xacro b/clearpath_platform_description/urdf/links/box.urdf.xacro
index aa65d880..ff56ab8f 100644
--- a/clearpath_platform_description/urdf/links/box.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/box.urdf.xacro
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="box" params="name parent_link size *origin">
+  <xacro:macro name="box" params="name parent_link size rgba *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
           <box size="${size}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
+        <material name="${name}_rgba">
+          <color rgba="${rgba}" />
+        </material>
       </visual>
       <collision>
         <geometry>
           <box size="${size}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
       </collision>
     </link>
 
diff --git a/clearpath_platform_description/urdf/links/cylinder.urdf.xacro b/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
index 6dcc4cae..1dcbc2e6 100644
--- a/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="cylinder" params="name parent_link radius length *origin">
+  <xacro:macro name="cylinder" params="name parent_link radius length rgba *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
           <cylinder radius="${radius}" length="${length}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
+        <material name="${name}_rgba">
+          <color rgba="${rgba}" />
+        </material>
       </visual>
       <collision>
         <geometry>
           <cylinder radius="${radius}" length="${length}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
       </collision>
     </link>
 
diff --git a/clearpath_platform_description/urdf/links/mesh.urdf.xacro b/clearpath_platform_description/urdf/links/mesh.urdf.xacro
index 4b175ff5..b94a9a17 100644
--- a/clearpath_platform_description/urdf/links/mesh.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/mesh.urdf.xacro
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="mesh" params="name parent_link visual *origin">
+  <xacro:macro name="mesh" params="name parent_link visual rgba *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
           <mesh filename="${visual}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
+        <material name="${name}_rgba">
+          <color rgba="${rgba}" />
+        </material>
       </visual>
       <collision>
         <geometry>
           <mesh filename="${visual}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
       </collision>
     </link>
 
diff --git a/clearpath_platform_description/urdf/links/sphere.urdf.xacro b/clearpath_platform_description/urdf/links/sphere.urdf.xacro
index 5f57fe3a..e6df95e4 100644
--- a/clearpath_platform_description/urdf/links/sphere.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/sphere.urdf.xacro
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="sphere" params="name parent_link radius *origin">
+  <xacro:macro name="sphere" params="name parent_link radius rgba *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
           <sphere radius="${radius}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
+        <material name="${name}_rgba">
+          <color rgba="${rgba}" />
+        </material>
       </visual>
       <collision>
         <geometry>
           <sphere radius="${radius}"/>
         </geometry>
-        <material name="clearpath_dark_grey"/>
       </collision>
     </link>
 

From 479b1190235a562f1b0ebe24e29149e0e01becbb Mon Sep 17 00:00:00 2001
From: Chris Iverach-Brereton <civerachb@clearpathrobotics.com>
Date: Thu, 1 May 2025 13:29:57 -0400
Subject: [PATCH 2/2] Add collision attribute to optionally disable collision
 geometry for links

---
 .../description/links.py                           | 10 ++++++++--
 .../urdf/links/box.urdf.xacro                      | 14 ++++++++------
 .../urdf/links/cylinder.urdf.xacro                 | 14 ++++++++------
 .../urdf/links/mesh.urdf.xacro                     | 14 ++++++++------
 .../urdf/links/sphere.urdf.xacro                   | 14 ++++++++------
 5 files changed, 40 insertions(+), 26 deletions(-)

diff --git a/clearpath_generator_common/clearpath_generator_common/description/links.py b/clearpath_generator_common/clearpath_generator_common/description/links.py
index eccb5605..38f26a8d 100644
--- a/clearpath_generator_common/clearpath_generator_common/description/links.py
+++ b/clearpath_generator_common/clearpath_generator_common/description/links.py
@@ -49,6 +49,7 @@ class BaseDescription():
         NAME = 'name'
         PARENT_LINK = 'parent_link'
         RGBA = 'rgba'
+        COLLISION_ENABLE = 'collision'
 
         def __init__(self, link: BaseLink) -> None:
             self.link = link
@@ -89,6 +90,7 @@ def __init__(self, link: Cylinder) -> None:
                 self.RADIUS: link.radius,
                 self.LENGTH: link.length,
                 self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
+                self.COLLISION_ENABLE: self.link.collision_enable,
             })
 
     class SphereDescription(BaseDescription):
@@ -99,6 +101,7 @@ def __init__(self, link: Sphere) -> None:
             self.parameters.update({
                 self.RADIUS: link.radius,
                 self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
+                self.COLLISION_ENABLE: self.link.collision_enable,
             })
 
     class MeshDescription(BaseDescription):
@@ -106,16 +109,19 @@ class MeshDescription(BaseDescription):
 
         def __init__(self, link: Mesh) -> None:
             super().__init__(link)
+            self.parameters.update({
+                self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
+                self.COLLISION_ENABLE: self.link.collision_enable,
+            })
             if (link.visual.package):
                 self.parameters.update({
                     self.VISUAL: os.path.join('package://' + link.visual.package,
                                               File.clean(link.visual.path, make_abs=False)),
-                    self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
+
                 })
             else:
                 self.parameters.update({
                     self.VISUAL: 'file://' + File.clean(link.visual.path, make_abs=False),
-                    self.RGBA: str(self.link.rgba).strip('[]').replace(',', ''),
                 })
 
     MODEL = {
diff --git a/clearpath_platform_description/urdf/links/box.urdf.xacro b/clearpath_platform_description/urdf/links/box.urdf.xacro
index ff56ab8f..19bdfc9f 100644
--- a/clearpath_platform_description/urdf/links/box.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/box.urdf.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="box" params="name parent_link size rgba *origin">
+  <xacro:macro name="box" params="name parent_link size rgba collision *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
@@ -10,11 +10,13 @@
           <color rgba="${rgba}" />
         </material>
       </visual>
-      <collision>
-        <geometry>
-          <box size="${size}"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${collision}">
+        <collision>
+          <geometry>
+            <box size="${size}"/>
+          </geometry>
+        </collision>
+      </xacro:if>
     </link>
 
     <joint name="${name}_joint" type="fixed">
diff --git a/clearpath_platform_description/urdf/links/cylinder.urdf.xacro b/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
index 1dcbc2e6..d6dcfacf 100644
--- a/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/cylinder.urdf.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="cylinder" params="name parent_link radius length rgba *origin">
+  <xacro:macro name="cylinder" params="name parent_link radius length rgba collision *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
@@ -10,11 +10,13 @@
           <color rgba="${rgba}" />
         </material>
       </visual>
-      <collision>
-        <geometry>
-          <cylinder radius="${radius}" length="${length}"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${collision}">
+        <collision>
+          <geometry>
+            <cylinder radius="${radius}" length="${length}"/>
+          </geometry>
+        </collision>
+      </xacro:if>
     </link>
 
     <joint name="${name}_joint" type="fixed">
diff --git a/clearpath_platform_description/urdf/links/mesh.urdf.xacro b/clearpath_platform_description/urdf/links/mesh.urdf.xacro
index b94a9a17..484dddeb 100644
--- a/clearpath_platform_description/urdf/links/mesh.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/mesh.urdf.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="mesh" params="name parent_link visual rgba *origin">
+  <xacro:macro name="mesh" params="name parent_link visual rgba collision *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
@@ -10,11 +10,13 @@
           <color rgba="${rgba}" />
         </material>
       </visual>
-      <collision>
-        <geometry>
-          <mesh filename="${visual}"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${collision}">
+        <collision>
+          <geometry>
+            <mesh filename="${visual}"/>
+          </geometry>
+        </collision>
+      </xacro:if>
     </link>
 
     <joint name="${name}_joint" type="fixed">
diff --git a/clearpath_platform_description/urdf/links/sphere.urdf.xacro b/clearpath_platform_description/urdf/links/sphere.urdf.xacro
index e6df95e4..a56643de 100644
--- a/clearpath_platform_description/urdf/links/sphere.urdf.xacro
+++ b/clearpath_platform_description/urdf/links/sphere.urdf.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="sphere" params="name parent_link radius rgba *origin">
+  <xacro:macro name="sphere" params="name parent_link radius rgba collision *origin">
     <link name="${name}_link">
       <visual>
         <geometry>
@@ -10,11 +10,13 @@
           <color rgba="${rgba}" />
         </material>
       </visual>
-      <collision>
-        <geometry>
-          <sphere radius="${radius}"/>
-        </geometry>
-      </collision>
+      <xacro:if value="${collision}">
+        <collision>
+          <geometry>
+            <sphere radius="${radius}"/>
+          </geometry>
+        </collision>
+      </xacro:if>
     </link>
 
     <joint name="${name}_joint" type="fixed">