def sensor_imu(self,bodyname):
self.robot.append(self.gazebo_ros_imu(bodyname))
gazebo = element('gazebo',reference=bodyname+'_imuframe')
sensor = element('sensor',name=bodyname+'_imu',type='imu')
sub(sensor,'imuTransform').text='0 0 0 0 0 0'
sub(sensor,'always_on').text='true'
sub(sensor,'pose').text='0 0 0 0 0 0'
imu = sub(sensor,'imu')
imu.append(self.noise_gaussian())
self.robot.append(gazebo)
def gazebo_ros_imu(self,bodyname):
#ref:https://gitlab.com/nasa-jsc-robotics/val_description/blob/ddaf8378741839f26ae6d734dbe6e4441b9b1e23/model/robots/common/xacro/sensors/microstrain.xacro
gazebo = element('gazebo')
plugin = element('plugin',name='gazebo_ros_imu',filename='libgazebo_ros_imu.so')
sub(plugin,'topicName').text=bodyname+'/imu'
sub(plugin,'bodyName').text=bodyname
sub(plugin,'serviceName').text=bodyname+'/imu_service'
sub(plugin,'frameName').text=bodyname+'_imuframe'
sub(plugin,'updateRate').text='100.0'
sub(plugin,'gaussianNoise').text='0.0'
#sub(plugin,'xyzOffset').text='0,0,0'
#sub(plugin,'rpyOffset').text='0,0,0'
gazebo.append(plugin)
return gazebo
def write_xml(self, node_name):
codable_value = etree.element(node_name)
codable_value.text = self.text
code = etree.Element('code')
value = etree.Element('value')
value.text = self.value
code.append(value)
family = etree.Element('family')
family.text = self.family
code.append(family)
_type = etree.Element('type')
_type.text = self._type
code.append(_type)
version = etree.Element('version')
version.text = self.version
code.append(version)
codable_value.append(code)
return codable_value
def mean_stddev_bias(self,name):
parent = element(name)
sub(parent,'mean').text='0'
sub(parent,'stddev').text='0.0002'
sub(parent,'bias_mean').text='7.5e-06'
sub(parent,'bias_stddev').text='8e-07'
return parent
def rewrap(tag: str, source: Element) -> Element:
"""copy all the contents of one element into another"""
new = element(tag)
new.text = source.text
for e in source.getchildren():
new.append(deepcopy(e))
return new
def robot_state_publisher(self, robotname, pkgname):
sub(self.launch,
'arg',
name='model',
default='$(find {})/urdf/{}.urdf'.format(pkgname, robotname))
sub(self.launch,
'param',
name='robot_description',
textfile='$(arg model)')
sub(self.launch,
'rosparam',
file='$(find {})/config/{}.yaml'.format(pkgname, robotname),
command='load') #very important
jsp = self.node('joint_state_publisher', 'joint_state_publisher',
'joint_state_publisher')
rsp = self.node('robot_state_publisher',
'robot_state_publisher',
'robot_state_publisher',
respawn='false',
output='screen')
od = OrderedDict(
) #Because element(and python's func) cannot take 'from'(like import,def and so on) as an argument.
od['from'] = '/joint_states'
od['to'] = '/' + robotname + '/joint_states'
rsp.append(element('remap', od))
self.launch.append(rsp)
def origin_geometry(self,linkname,tagtype,origin,geometry):
##<origin/>
##<geometry/>
og = element(tagtype,name=linkname)#+'_'+tagtype)
og.append(origin)
og.append(geometry)
return og
def geometry(self,shapegeom):
##<geometry>
###<shape/>
##</geometry>
geometry = element('geometry')
geometry.append(shapegeom)
return geometry
def node(self, nodename, pkgname, typename, **args):
od = OrderedDict()
od['name'] = nodename #program name as a node on ros
od['pkg'] = pkgname #included package name (like family name)
od['type'] = typename #truth name the program (like last name)
for i, j in args.items():
od[i] = j #additional arguments
return element('node', od) #add launch file
def inertial(self,mass,inertia):
#<inertial>
##<mass value="mass">
##<inertia/>
#</inertial>
inertial = element('inertial')
sub(inertial,'mass',value=str(mass))
inertial.append(inertia)
return inertial
def gazebo(self,refname,color=None,selfcollide=None,sensor=None):
#<gazebo reference="refname">
##<material></material>
##<selfCollide></self...>
##<plugin/>
gazebo = element('gazebo',reference=refname)
if not color is None:
sub(gazebo,'material').text = 'Gazebo/'+color
if not selfcollide is None:
sub(gazebo,'selfCollide').text='true'
#!!!now printing...!!!#
if sensor == 'imu':
self.sensor_imu(refname)
return gazebo
def noise_gaussian(self):
#<noise>
##<type>gaussian</>
##<rate>
###<mean>0.0</>
###<stddev>0.03</>
###<bias_mean>7.5e-06</>
###<bias_stddev>8e-07</>
##</rate>
##<accel>
###...
##</accel>
#</noise>
noise = element('noise')
sub(noise,'type').text='gaussian'
rate = self.mean_stddev_bias('rate')
accel = self.mean_stddev_bias('accel')
noise.extend((rate,accel))
return noise
def transmission(self,jointname):
#<transmission name="..._tr">
##<type></type>
##<joint>
###<hardwareInterface/>
##</joint>
##<actuator>
###<hardwareInterface/>
##</actuator>
#</transmission>
tr = element('transmission',name=jointname+'_tr')
#type
sub(tr,'type').text = 'transmission_interface/SimpleTransmission'
#joint
joint = sub(tr,'joint',name=jointname)
joint.append(self.hardwareInterface('EffortJointInterface'))
#actuator
actuator = sub(tr,'actuator',name=jointname+'_mtr')
actuator.append(self.hardwareInterface('EffortJointInterface'))
sub(actuator,'mechanicalReduction').text = '1'
return tr
def __init__(self,robotname,ros_control=True):
#robot init
self.robot = element('robot',name=robotname)
if ros_control:
self.robot.append(self.gazebo_ros_control(robotname))#MUST write FIRST!!
def empty_world(self, **args):
include = element('include',
file='$(find gazebo_ros)/launch/empty_world.launch')
for i, j in args.items():
sub(include, 'arg', name=i, value=j)
self.launch.append(include)
def inertia(self,iner):
#<inertia ixx="i[0]" .../>
i = [str(n) for n in iner]
return element('inertia',ixx=i[0],ixy=i[1],ixz=i[2],iyy=i[3],iyz=i[4],izz=i[5])
def gazebo_ros_control(self,robotname):
gazebo = element('gazebo')
plugin = sub(gazebo,'plugin',name='gazebo_ros_control',filename='libgazebo_ros_control.so')
sub(plugin,'robotNamespace').text = robotname
sub(plugin,'robotSimType').text = 'gazebo_ros_control/DefaultRobotHWSim'
return gazebo
def origin(self,xyz,rpy):
return element('origin',xyz=exBrackets(xyz),rpy=exBrackets(rpy))
def hardwareInterface(self,interface):#this is not smart. I recommend you to rewrite this.
#<hardwareInterface>interface</hardwareInterface>
hi = element('hardwareInterface')
hi.text = interface
return hi
def limits_revolute(self,limits):
#<limit effort="l[0]" lower="l[1]" upper.../>
l = [str(limit) for limit in limits]
return element('limit',effort=l[0],lower=l[1],upper=l[2],velocity=l[3])
def boxgeometry(self,whd):
#<box size="w h d"/>
return element('box',size=exBrackets(whd))
def cylindergeometry(self,lengrad):
#<cylinder length="" radius=""/>
return element('cylinder',length=str(lengrad[0]),radius=str(lengrad[1]))
def parentchild(self,parentname,childname):
parent = element('parent',link=parentname)
child = element('child',link=childname)
return parent,child
def axis(self,axisxyz):
return element('axis',xyz=exBrackets(axisxyz))
def __init__(self):
self.launch = element('launch')
