Skip to content

Add rgba, collision attributes to links #205

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 2 commits into
base: rc/jazzy/2.5
Choose a base branch
from
Draft
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -48,6 +48,8 @@ class BaseDescription():

NAME = 'name'
PARENT_LINK = 'parent_link'
RGBA = 'rgba'
COLLISION_ENABLE = 'collision'

def __init__(self, link: BaseLink) -> None:
self.link = link
@@ -57,7 +59,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 +76,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 +88,9 @@ 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(',', ''),
self.COLLISION_ENABLE: self.link.collision_enable,
})

class SphereDescription(BaseDescription):
@@ -94,22 +99,29 @@ 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(',', ''),
self.COLLISION_ENABLE: self.link.collision_enable,
})

class MeshDescription(BaseDescription):
VISUAL = 'visual'

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))
File.clean(link.visual.path, make_abs=False)),

})
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),
})

MODEL = {
19 changes: 11 additions & 8 deletions clearpath_platform_description/urdf/links/box.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,19 +1,22 @@
<?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 collision *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>
<xacro:if value="${collision}">
<collision>
<geometry>
<box size="${size}"/>
</geometry>
</collision>
</xacro:if>
</link>

<joint name="${name}_joint" type="fixed">
19 changes: 11 additions & 8 deletions clearpath_platform_description/urdf/links/cylinder.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,19 +1,22 @@
<?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 collision *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>
<xacro:if value="${collision}">
<collision>
<geometry>
<cylinder radius="${radius}" length="${length}"/>
</geometry>
</collision>
</xacro:if>
</link>

<joint name="${name}_joint" type="fixed">
19 changes: 11 additions & 8 deletions clearpath_platform_description/urdf/links/mesh.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,19 +1,22 @@
<?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 collision *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>
<xacro:if value="${collision}">
<collision>
<geometry>
<mesh filename="${visual}"/>
</geometry>
</collision>
</xacro:if>
</link>

<joint name="${name}_joint" type="fixed">
19 changes: 11 additions & 8 deletions clearpath_platform_description/urdf/links/sphere.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,19 +1,22 @@
<?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 collision *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>
<xacro:if value="${collision}">
<collision>
<geometry>
<sphere radius="${radius}"/>
</geometry>
</collision>
</xacro:if>
</link>

<joint name="${name}_joint" type="fixed">