00001 import roslib; roslib.load_manifest('clearpath_base')
00002 import rospy
00003
00004 from clearpath_base.msg import *
00005
00006
00007 msgs = {
00008 'system_status': SystemStatus,
00009 'power_status': PowerStatus,
00010 'processor_status': ProcessorStatus,
00011 'safety_status': SafetyStatus,
00012 'differential_speed': DifferentialSpeed,
00013 'differential_control': DifferentialControl,
00014 'differential_output': DifferentialOutput,
00015 'velocity': VelocitySetpt,
00016 'platform_orientation': Orientation,
00017 'platform_rotation': RotateRate,
00018 'platform_magnetometer': Magnetometer,
00019 'encoders': Encoders,
00020 'raw_encoders': RawEncoders,
00021 'distance': Distance
00022 }
00023
00024
00025 def copy_attributes(source, dest, attrs):
00026 for attr in attrs:
00027 setattr(dest, attr, getattr(source, attr))
00028
00029 def pkg_robot_info(platform_name, platform_info, firmware_info):
00030 msg = ClearpathRobot();
00031 msg.name = platform_name.name
00032 msg.model = platform_info.model
00033 msg.platform_revision = platform_info.revision
00034 msg.serial = platform_info.serial
00035 msg.horizon_version = firmware_info.version
00036 msg.firmware_version = firmware_info.firmware
00037 msg.write_date = str(firmware_info.written)
00038 return msg
00039
00040 def pkg_system_status(payload):
00041 msg = SystemStatus()
00042 copy_attributes(payload, msg, ['uptime', 'voltages',
00043 'currents', 'temperatures'])
00044 return msg
00045
00046 def pkg_power_status(payload):
00047 msg = PowerStatus()
00048 charges = payload.charges
00049 capacities = payload.capacities
00050 descriptions = payload.descriptions
00051 for i in range(len(charges)):
00052 msg_s = PowerSource(charge=charges[i],
00053 capacity=capacities[i])
00054 msg_s.present, msg_s.in_use, msg_s.description = descriptions[i]
00055 msg.sources.append(msg_s)
00056 return msg
00057
00058 def pkg_processor_status(payload):
00059 msg = ProcessorStatus()
00060 msg.errors = payload.errors
00061 return msg
00062
00063 def pkg_differential_speed(payload):
00064 msg = DifferentialSpeed()
00065 copy_attributes(payload, msg, ['left_speed', 'right_speed',
00066 'left_accel', 'right_accel'])
00067 return msg
00068
00069 def pkg_differential_control(payload):
00070 msg = DifferentialControl()
00071 copy_attributes(payload, msg, ['left_p', 'left_i', 'left_d',
00072 'left_ffwd', 'left_stic', 'left_sat',
00073 'right_p', 'right_i', 'right_d',
00074 'right_ffwd', 'right_stic', 'right_sat'])
00075 return msg
00076
00077 def pkg_differential_output(payload):
00078 msg = DifferentialOutput()
00079 copy_attributes(payload, msg, ['left', 'right'])
00080 return msg
00081
00082 def pkg_ackermann_output(payload):
00083 msg = AckermannOutput()
00084 copy_attributes(payload, msg, ['steering', 'throttle', 'brake'])
00085 return msg
00086
00087 def pkg_velocity(payload):
00088 msg = VelocitySetpt()
00089 copy_attributes(payload, msg, ['trans', 'rot', 'accel'])
00090 return msg
00091
00092 def pkg_platform_orientation(payload):
00093 msg = Orientation()
00094 copy_attributes(payload, msg, ['roll', 'pitch', 'yaw'])
00095 return msg
00096
00097 def pkg_platform_rotation(payload):
00098 msg = RotateRate()
00099 copy_attributes(payload, msg, ['roll', 'pitch', 'yaw'])
00100 return msg
00101
00102 def pkg_platform_magnetometer(payload):
00103 msg = Magnetometer()
00104 copy_attributes(payload, msg, ['x', 'y', 'z'])
00105 return msg
00106
00107 def pkg_encoders(payload):
00108 msg = Encoders()
00109 travels = payload.travel
00110 speeds = payload.speed
00111 for i in range(len(travels)):
00112 msg.encoders.append(Encoder(travel=travels[i],
00113 speed=speeds[i]))
00114 return msg
00115
00116 def pkg_raw_encoders(payload):
00117 msg = RawEncoders()
00118 msg.ticks = payload.ticks
00119 return msg
00120
00121 def pkg_safety_status(payload):
00122 msg = SafetyStatus()
00123 msg.flags = payload.flags
00124 msg.estop = payload.flags & 0x8;
00125 return msg
00126
00127 def pkg_distance(payload):
00128 msg = Distance()
00129 msg.distances = payload.distances
00130 return msg
