Skip to content

Commit 9096f1b

Browse files
Merge pull request #157 from clearpathrobotics/rc/jazzy-2.1
Jazzy 2.1 RC
2 parents a58bac5 + e93989f commit 9096f1b

File tree

27 files changed

+523
-11
lines changed

27 files changed

+523
-11
lines changed

clearpath_control/config/a200/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/a300/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/dd100/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/dd150/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/do100/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/do150/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/generic/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/j100/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/r100/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_control/config/w200/localization.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ ekf_node:
66
publish_tf: true
77
two_d_mode: true
88
use_sim_time: false
9+
print_diagnostics: true
910

1011
frequency: 50.0
1112

clearpath_generator_common/clearpath_generator_common/description/generator.py

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -205,6 +205,7 @@ def generate_manipulators(self) -> None:
205205
self.xacro_writer.write_comment('Manipulators')
206206
self.xacro_writer.write_newline()
207207
self.generate_arms()
208+
self.generate_lifts()
208209
self.generate_grippers()
209210

210211
def generate_arms(self) -> None:
@@ -257,6 +258,30 @@ def generate_grippers(self) -> None:
257258

258259
self.xacro_writer.write_newline()
259260

261+
def generate_lifts(self) -> None:
262+
lifts = self.clearpath_config.manipulators.get_all_lifts()
263+
for lift in lifts:
264+
lift_description = ManipulatorDescription(lift)
265+
266+
self.xacro_writer.write_comment(
267+
'{0}'.format(lift_description.name)
268+
)
269+
270+
self.xacro_writer.write_include(
271+
package=lift_description.package,
272+
file=lift_description.model,
273+
path=lift_description.path
274+
)
275+
276+
self.xacro_writer.write_macro(
277+
macro='{0}'.format(lift_description.model),
278+
parameters=lift_description.parameters,
279+
blocks=XacroWriter.add_origin(
280+
lift_description.xyz, lift_description.rpy)
281+
)
282+
283+
self.xacro_writer.write_newline()
284+
260285
def generate_extras(self) -> None:
261286
self.xacro_writer.write_comment('Extras')
262287
self.xacro_writer.write_newline()

clearpath_generator_common/clearpath_generator_common/description/manipulators.py

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,10 @@
4343
Robotiq2F140,
4444
Robotiq2F85
4545
)
46+
from clearpath_config.manipulators.types.lifts import (
47+
BaseLift,
48+
Ewellix
49+
)
4650
from clearpath_config.manipulators.types.manipulator import BaseManipulator
4751

4852

@@ -119,11 +123,23 @@ def __init__(self, arm: BaseArm) -> None:
119123
self.parameters.pop(self.PORT)
120124
self.parameters.update(arm.get_urdf_parameters())
121125

126+
class LiftDescription(BaseDescription):
127+
128+
def __init__(self, lift: BaseLift) -> None:
129+
super().__init__(lift)
130+
131+
class EwellixDescription(LiftDescription):
132+
133+
def __init__(self, lift: BaseLift) -> None:
134+
super().__init__(lift)
135+
self.parameters.update(lift.get_urdf_parameters())
136+
122137
MODEL = {
123138
KinovaGen3Dof6.MANIPULATOR_MODEL: KinovaArmDescription,
124139
KinovaGen3Dof7.MANIPULATOR_MODEL: KinovaArmDescription,
125140
KinovaGen3Lite.MANIPULATOR_MODEL: KinovaArmDescription,
126141
UniversalRobots.MANIPULATOR_MODEL: UniversalRobotsDescription,
142+
Ewellix.MANIPULATOR_MODEL: EwellixDescription,
127143
}
128144

129145
def __new__(cls, manipulator: BaseManipulator) -> BaseManipulator:

clearpath_generator_common/clearpath_generator_common/description/sensors.py

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ def __init__(self, sensor: BaseLidar2D) -> None:
101101
self.MAXIMUM_ANGLE: sensor.max_angle,
102102
self.MINIMUM_RANGE: 0.05,
103103
self.MAXIMUM_RANGE: 25.0,
104-
self.UPDATE_RATE: 50
104+
self.UPDATE_RATE: 40 # TODO: link to clearpath_config property
105105
})
106106

107107
class Lidar3dDescription(BaseDescription):
@@ -127,7 +127,7 @@ def __init__(self, sensor: BaseLidar3D) -> None:
127127
self.MAXIMUM_ANGLE_V: 0.261799,
128128
self.MINIMUM_RANGE: 0.9,
129129
self.MAXIMUM_RANGE: 130.0,
130-
self.UPDATE_RATE: 50
130+
self.UPDATE_RATE: 20 # TODO: link to clearpath_config property
131131
})
132132

133133
class ImuDescription(BaseDescription):
@@ -137,7 +137,7 @@ def __init__(self, sensor: BaseIMU) -> None:
137137
super().__init__(sensor)
138138

139139
self.parameters.update({
140-
self.UPDATE_RATE: 100
140+
self.UPDATE_RATE: sensor.update_rate
141141
})
142142

143143
class CameraDescription(BaseDescription):
@@ -152,14 +152,12 @@ def __init__(self, sensor: BaseCamera) -> None:
152152

153153
class AxisCameraDescription(CameraDescription):
154154
MODEL = 'model'
155-
UPDATE_RATE = 'update_rate'
156155

157156
def __init__(self, sensor: AxisCamera) -> None:
158157
super().__init__(sensor)
159158

160159
self.parameters.update({
161160
self.MODEL: sensor.device_type,
162-
self.UPDATE_RATE: 15,
163161
})
164162

165163
class IntelRealsenseDescription(CameraDescription):

clearpath_generator_common/clearpath_generator_common/param/manipulators.py

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,6 +127,25 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
127127
self.param_file.parameters = merge_dict(
128128
self.param_file.parameters, updated_parameters)
129129

130+
# Lifts
131+
for lift in self.clearpath_config.manipulators.get_all_lifts():
132+
# Lift Control Parameter File
133+
lift_param_file = ParamFile(
134+
name='control',
135+
package=Package('clearpath_manipulators_description'),
136+
path='config/%s/%s' % (
137+
lift.get_manipulator_type(),
138+
lift.get_manipulator_model()),
139+
parameters={}
140+
)
141+
lift_param_file.read()
142+
updated_parameters = replace_dict_items(
143+
lift_param_file.parameters,
144+
{r'${name}': lift.name}
145+
)
146+
self.param_file.parameters = merge_dict(
147+
self.param_file.parameters, updated_parameters)
148+
130149
def generate_parameter_file(self):
131150
param_writer = ParamWriter(self.param_file)
132151
param_writer.write_file()
@@ -258,6 +277,18 @@ def get_kinematics_parameters(self):
258277
r'${name}': gripper.name
259278
})
260279
parameter_file += kinematics_file
280+
# Lifts
281+
for lift in self.clearpath_config.manipulators.get_all_lifts():
282+
kinematics_file = MoveItParamFile(
283+
name=lift.get_manipulator_type(),
284+
path=parameter_directory,
285+
package=parameter_package
286+
)
287+
kinematics_file.read()
288+
kinematics_file.replace({
289+
r'${name}': lift.name
290+
})
291+
parameter_file += kinematics_file
261292
parameter_file.add_header('robot_description_kinematics')
262293
return parameter_file
263294

@@ -328,6 +359,26 @@ def get_moveit_controller_parameters(self, use_sim_time: bool = False) -> None:
328359
r'${name}': gripper.name
329360
})
330361
parameter_file += controller_file
362+
# Lifts
363+
for lift in self.clearpath_config.manipulators.get_all_lifts():
364+
controller_file = MoveItParamFile(
365+
name=parameter_name,
366+
path=os.path.join(
367+
parameter_directory,
368+
lift.get_manipulator_type(),
369+
lift.get_manipulator_model()
370+
),
371+
package=parameter_package
372+
)
373+
controller_name = lift.name
374+
if not use_sim_time:
375+
controller_name = 'manipulators/' + controller_name
376+
controller_file.read()
377+
controller_file.replace({
378+
r'${controller_name}': controller_name,
379+
r'${name}': lift.name
380+
})
381+
parameter_file += controller_file
331382
return parameter_file
332383

333384
# Joint Limits
@@ -375,6 +426,22 @@ def get_joint_limits_parameters(self):
375426
r'${name}': gripper.name
376427
})
377428
parameter_file += controller_file
429+
# Lifts
430+
for lift in self.clearpath_config.manipulators.get_all_lifts():
431+
controller_file = MoveItParamFile(
432+
name=parameter_name,
433+
path=os.path.join(
434+
parameter_directory,
435+
lift.get_manipulator_type(),
436+
lift.get_manipulator_model()
437+
),
438+
package=parameter_package
439+
)
440+
controller_file.read()
441+
controller_file.replace({
442+
r'${name}': lift.name
443+
})
444+
parameter_file += controller_file
378445
parameter_file.add_header('robot_description_planning')
379446
return parameter_file
380447

0 commit comments

Comments
 (0)