# ----------------------------------------------
# Draw transform chain for each Sensor
# ----------------------------------------------
listener = TransformListener()
rospy.sleep(0.2)
g2 = Digraph('G', filename='calibration_per_sensor')
for i, xml_sensor in enumerate(xml_robot.sensors):
print(Fore.BLUE + '\n\nSensor name is ' + xml_sensor.name +
Style.RESET_ALL)
rgb = matplotlib.colors.rgb2hex(cmap[i, 0:3])
chain = listener.chain(xml_sensor.parent, rospy.Time(),
args['world_link'], rospy.Time(),
args['world_link'])
print('Complete chain is: ' + str(chain))
for parent, child in zip(chain[0::], chain[1::]):
# if parent == 'model':
# continue
print(parent + '->' + child)
if parent == xml_sensor.calibration_parent and child == xml_sensor.calibration_child:
# g2.edge(parent, child, color=rgb, style='solid', _attributes={'penwidth': '1', 'fontcolor':rgb},
# label=xml_sensor.name + '\\n calibration transform')
g2.edge(parent,
child,
color=rgb,
style='solid',
