@@ -82,34 +82,34 @@ def chain_link_names(self, root_link_name: str, end_link_name: str) -> List[str]
8282 link_names .append (child_link_name )
8383 return link_names
8484
85- def link_names_with_meshes (self , visual : bool = False ) -> List [str ]:
85+ def link_names_with_meshes (self , collision : bool = False ) -> List [str ]:
8686 r"""Get link names that have meshes.
8787
8888 Args:
89- visual (bool): If True, get visual meshes, else collision meshes.
89+ collision (bool): If True, get collision meshes, else visual meshes.
9090
9191 Returns:
9292 List[str]: List of link names with meshes.
9393 """
9494 links = [link .name for link in self ._robot .links ]
9595 for link in links :
96- if visual :
97- if not self ._robot .link_map [link ].visual :
96+ if collision :
97+ if not self ._robot .link_map [link ].collision :
9898 links .remove (link )
9999 else :
100- if not self ._robot .link_map [link ].collision :
100+ if not self ._robot .link_map [link ].visual :
101101 links .remove (link )
102102 return links
103103
104104 def raw_mesh_paths (
105- self , root_link_name : str , end_link_name : str , visual : bool = False
105+ self , root_link_name : str , end_link_name : str , collision : bool = False
106106 ) -> Dict [str , str ]:
107107 r"""Get the raw mesh paths as specified in URDF.
108108
109109 Args:
110110 root_link_name (str): Root link name.
111111 end_link_name (str): End link name.
112- visual (bool): If True, get visual mesh paths, else collision mesh paths.
112+ collision (bool): If True, get collision mesh paths, else visual mesh paths.
113113
114114 Returns:
115115 Dict[str,str]: Dictionary of link names and raw mesh paths.
@@ -121,31 +121,33 @@ def raw_mesh_paths(
121121 # lookup paths
122122 for link_name in link_names :
123123 link : urdf_parser_py .urdf .Link = self ._robot .link_map [link_name ]
124- if visual :
125- if link .visual is None :
126- continue
127- raw_mesh_paths [link_name ] = link .visual .geometry .filename
128- else :
124+ if collision :
129125 if link .collision is None :
130126 continue
131127 raw_mesh_paths [link_name ] = link .collision .geometry .filename
128+ else :
129+ if link .visual is None :
130+ continue
131+ raw_mesh_paths [link_name ] = link .visual .geometry .filename
132132 return raw_mesh_paths
133133
134134 def ros_package_mesh_paths (
135- self , root_link_name : str , end_link_name : str , visual : bool = False
135+ self , root_link_name : str , end_link_name : str , collision : bool = False
136136 ) -> Dict [str , str ]:
137137 r"""Get the absolute mesh paths by resolving package within ROS.
138138
139139 Args:
140140 root_link_name (str): Root link name.
141141 end_link_name (str): End link name.
142- visual (bool): If True, get visual mesh paths, else collision mesh paths.
142+ collision (bool): If True, get collision mesh paths, else visual mesh paths.
143143
144144 Returns:
145145 Dict[str,str]: Dictionary of link names and absolute mesh paths.
146146 """
147147 raw_mesh_paths = self .raw_mesh_paths (
148- root_link_name = root_link_name , end_link_name = end_link_name , visual = visual
148+ root_link_name = root_link_name ,
149+ end_link_name = end_link_name ,
150+ collision = collision ,
149151 )
150152 from ament_index_python import get_package_share_directory
151153
@@ -163,14 +165,14 @@ def ros_package_mesh_paths(
163165 return ros_package_mesh_paths
164166
165167 def mesh_origins (
166- self , root_link_name : str , end_link_name : str , visual : bool = False
168+ self , root_link_name : str , end_link_name : str , collision : bool = False
167169 ) -> Dict [str , np .ndarray ]:
168170 r"""Get mesh origins.
169171
170172 Args:
171173 root_link_name (str): Root link name.
172174 end_link_name (str): End link name.
173- visual (bool): If True, get visual mesh origins, else collision mesh origins.
175+ collision (bool): If True, get collision mesh origins, else visual mesh origins.
174176
175177 Returns:
176178 Dict[str,np.ndarray]: Dictionary of link names and mesh origins.
@@ -183,14 +185,14 @@ def mesh_origins(
183185 mesh_origins = {}
184186 for link_name in link_names :
185187 link : urdf_parser_py .urdf .Link = self ._robot .link_map [link_name ]
186- if visual :
187- if link .visual is None :
188- continue
189- link_origin = link .visual .origin
190- else :
188+ if collision :
191189 if link .collision is None :
192190 continue
193191 link_origin = link .collision .origin
192+ else :
193+ if link .visual is None :
194+ continue
195+ link_origin = link .visual .origin
194196 origin = transformations .euler_matrix (
195197 link_origin .rpy [0 ], link_origin .rpy [1 ], link_origin .rpy [2 ], "sxyz"
196198 )
0 commit comments