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')