forked from xiaoming-sun6/sw2urdf_ros2
-
Notifications
You must be signed in to change notification settings - Fork 1
/
conversion_urdf_ros_2_ros2.py
176 lines (160 loc) · 8.02 KB
/
conversion_urdf_ros_2_ros2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
import os
import shutil
import sys
import xml.etree.ElementTree as ET
from os import listdir
from os.path import isfile, join
import argparse
def run_command_dir(command_dir, command):
os.system("cd " + command_dir + " && " + command)
def replace_str(file, old_str, new_str):
file_data = ""
with open(file, "r", encoding="utf-8") as f:
for line in f:
if old_str in line:
line = line.replace(old_str, new_str)
file_data += line
with open(file, "w", encoding="utf-8") as f:
f.write(file_data)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument(
"--source-dir", help="The path to the solidworks output folder containing the urdf files", type=str, required=True)
parser.add_argument("--package-name",
help="The name for the ROS 2 package", type=str, required=True)
parser.add_argument("--package-directory",
help="The path for the ROS 2 package", type=str, required=True)
parser.add_argument("--source-urdf-file-name",
help="The name of the URDF file that SolidWorks outputs", type=str, default="robot")
parser.add_argument("--target-urdf-file-name",
help="Defines the name of the target URDF in the ROS 2 package", type=str, default="robot")
parser.add_argument("--maintainer-name",
help="Will be added to the package xml and setup.py", type=str, default="")
parser.add_argument("--maintainer-mail",
help="Will be added to the package xml and setup.py", type=str, default="")
parser.add_argument(
"--description", help="Will be added to the package xml and setup.py", type=str, default="")
parser.add_argument(
"--license_type", help="Will be added to the package xml and setup.py", type=str, default="")
parser.add_argument(
"--version_number", help="Will be added to the package xml and setup.py", type=str, default="0.0.0")
args = parser.parse_args()
source_dir = args.source_dir
package_name = args.package_name
package_directory = args.package_directory
target_dir = package_directory + package_name + '/'
source_urdf_file_name = args.source_urdf_file_name
target_urdf_file_name = args.target_urdf_file_name
maintainer_name = args.maintainer_name
maintainer_mail = args.maintainer_mail
description = args.description
license_type = args.license_type
version_number = args.version_number
if not os.path.exists(source_dir):
sys.stdout.write(f"Source directory {source_dir} does not exist. Exiting...")
exit()
# Check if this is a new package or if we might override something
if os.path.exists(target_dir) and (os.listdir(target_dir) != 0):
sys.stdout.write(
f"Target directory {target_dir} is not empty. Previous files might be deleted to create the ROS2 package. Continue? [Y/n]")
choice = input().lower()
if choice not in ["y", ""]:
exit()
if not os.path.exists(target_dir):
# create the directory of the package
run_command_dir(package_directory, f"mkdir {package_name}")
# Remove old folders to ensure that we don't keep old files from previous version (i.e. meshes with different names)
folders = ["launch", "meshes", "meshes/collision",
"meshes/visual", "urdf", "rviz", "resource"]
for folder in folders:
if os.path.exists(f"{target_dir}/{folder}"):
run_command_dir(target_dir, f"rm -r {folder}")
# Create new empty folders
for folder in folders:
run_command_dir(target_dir, f"mkdir {folder}")
# Copy files
# Copy stl files
run_command_dir(source_dir, "cp -r ./meshes/* " +
target_dir + "meshes/visual")
run_command_dir(source_dir, "cp -r ./meshes/* " +
target_dir + "meshes/collision")
# Copy urdf files
run_command_dir(source_dir, "cp ./urdf/" +
source_urdf_file_name + ".urdf " + target_dir + "urdf/")
# rename URDF
run_command_dir(target_dir, "mv ./urdf/" +
source_urdf_file_name + ".urdf " + "./urdf/" + target_urdf_file_name + ".xacro")
# create empty file to install marker in the package index
run_command_dir(target_dir, f"touch ./resource/{package_name}")
# replace file
os.system("cp -f ./replace_files/setup.py " + target_dir)
os.system("cp -f ./replace_files/setup.cfg " + target_dir)
os.system("cp -f ./replace_files/package.xml " + target_dir)
os.system("cp -f ./replace_files/launch.py " + target_dir + "launch")
os.system("cp -f ./replace_files/default.rviz " + target_dir + "rviz")
# Change file content
# launch.py
replace_str(target_dir + "launch/launch.py", "PACKAGE_NAME", package_name)
replace_str(target_dir + "launch/launch.py", "robot.urdf.xacro",
target_urdf_file_name + ".xacro")
# setup.py
replace_str(target_dir + "setup.py", "PACKAGE_NAME", package_name)
replace_str(target_dir + "setup.cfg", "PACKAGE_NAME", package_name)
# package.xml
replace_str(target_dir + "package.xml", "PACKAGE_NAME", package_name)
replace_str(target_dir + "package.xml", "MAINTAINER_NAME", maintainer_name)
replace_str(target_dir + "package.xml", "MAINTAINER_MAIL", maintainer_mail)
replace_str(target_dir + "package.xml", "LICENSE_TYPE", license_type)
replace_str(target_dir + "package.xml", "VERSION_NUMBER", version_number)
replace_str(target_dir + "package.xml", "DESCRIPTION_TEXT", description)
# urdf files
replace_str(target_dir + "urdf/" + target_urdf_file_name + ".xacro", source_urdf_file_name + "/meshes",
package_name + "/meshes/visual")
replace_str(target_dir + "urdf/" + target_urdf_file_name + ".xacro", source_urdf_file_name,
target_urdf_file_name)
# Insert base_footprint
keyword = "name=\"" + target_urdf_file_name + "\">"
str = ""
with open("./replace_files/insert_content.txt", "r", encoding="utf-8") as f:
str = f.read()
file = open(target_dir + "/urdf/" +
target_urdf_file_name + ".xacro", 'r')
content = file.read()
post = content.find(keyword)
if post != -1:
# add xacro note and insert_content.txt
content = content[:post] + "xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n " + keyword + \
"\n" + str + content[post + len(keyword):]
file = open(target_dir + "/urdf/" +
target_urdf_file_name + ".xacro", "w")
file.write(content)
file.close()
# remove non existing link visuals and collision files
stl_path = source_dir + "/meshes/"
stl_files = [f for f in listdir(stl_path) if isfile(join(stl_path, f))]
ET.register_namespace('xacro', "http://www.ros.org/wiki/xacro")
tree = ET.parse(target_dir + "/urdf/" + target_urdf_file_name + ".xacro",
ET.XMLParser(target=ET.TreeBuilder(insert_comments=True)))
root = tree.getroot()
# we want to keep those links who have an existing mesh file
links_to_keep = []
for stl_file in stl_files:
links_to_keep = links_to_keep + \
root.findall(".//mesh[@filename='package://" + package_name +
"/meshes/visual/" + stl_file + "']/../../..")
# get all links to find out for which we need to delete the meshes
links = root.findall(".//mesh/../../..")
links_to_delete = set(links) - set(links_to_keep)
for link in links_to_delete:
elements = link.findall(".//mesh/../..")
for element in elements:
link.remove(element)
# correct the path to the stl for collision files
collision_meshes = root.findall(".//collision/geometry/mesh")
for collision_mesh in collision_meshes:
old_path = collision_mesh.get('filename')
new_path = old_path.replace('visual', 'collision')
collision_mesh.set('filename', new_path)
# save modified URDF
tree.write(target_dir + "urdf/" + target_urdf_file_name + ".xacro")
print("conversion success!")