def start_subscribe(self): def cb_func(twist_data): self.dev_write( self.vel_to_encoder(twist_data.linear.x, twist_data.linear.y, twist_data.angular.z)) ChassisDev.start_sub(self, cb_func)
def data_handler(self, bin_data_pack): counter0, counter1, counter2 = \ self.protocol.decode(bin_data_pack) self.counters.data = [counter0, counter1, counter2] ChassisDev.pub_data_update(self, self.counters)
def start_subscribe(self): def cb_func(twist_data): self.dev_write(self.vel_to_encoder(twist_data.linear.x, twist_data.linear.y, twist_data.angular.z)) ChassisDev.start_sub(self, cb_func)
def data_handler(self, bin_data_pack): wheel0, wheel1, wheel2 = \ self.protocol.decode(bin_data_pack) self.speeds.data = [wheel0, wheel1, wheel2] ChassisDev.pub_data_update(self, self.speeds)
def __init__(self, serial_dev): self.protocol = Protocol(self.TYPE, self.pack_pmt) self.serial_dev = serial_dev ChassisDev.__init__(self, "Servo", sub_topic = "/robot_chassis/Servo", sub_msg_class = Byte)
def __init__(self, serial_dev): self.protocol = Protocol(self.TYPE, self.pack_pmt) self.serial_dev = serial_dev ChassisDev.__init__(self, "move_ctrl", sub_topic = "cmd_vel", sub_msg_class = Twist)
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "smart_battery", pub_topic = "smart_battery", pub_msg_class = SmartBatteryStatus) self.battery_status = SmartBatteryStatus() ChassisDev.start_pub(self)
def __init__(self, serial_dev): self.protocol = Protocol(self.TYPE, self.pack_pmt) self.serial_dev = serial_dev ChassisDev.__init__(self, "move_ctrl", sub_topic="cmd_vel", sub_msg_class=Twist)
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "chassis", pub_topic="imu/data_raw", pub_rate=10, pub_msg_class=Imu) self.imu = Imu() ChassisDev.start_pub(self)
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "Chassis", pub_topic = "/encoder_cnts", pub_msg_class = Float32MultiArray, pub_rate = 20) self.counters = Float32MultiArray() ChassisDev.start_pub(self)
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "Chassis", pub_topic="/encoder_cnts", pub_msg_class=Float32MultiArray, pub_rate=20) self.counters = Float32MultiArray() ChassisDev.start_pub(self)
def data_handler(self, bin_data_pack): try: voltage, rate, _,_ = \ self.protocol.decode(bin_data_pack) except e: print e return self.battery_status.voltage = voltage self.battery_status.rate = rate self.battery_status.charge_state = 0 if voltage > 2000: self.battery_status.percentage = 100 - (2520 - voltage)/4 else: self.battery_status.percentage = 100 - (1260 - voltage)/1.6 ChassisDev.pub_data_update(self, self.battery_status)
def data_handler(self, bin_data_pack): if struct.unpack("20B", bin_data_pack)[2] == self.TYPE_GYRO: try: x, y, z = self.protocol.decode(bin_data_pack) self.imu.angular_velocity.x = x self.imu.angular_velocity.y = y self.imu.angular_velocity.z = z self.imu.header.frame_id = "base_footprint" self.imu.header.stamp = rospy.Time.now() ChassisDev.pub_data_update(self, self.imu) except Exception as e: print e return else: try: x, y, z = self.protocol.decode(bin_data_pack) self.imu.linear_acceleration.x = x self.imu.linear_acceleration.y = y self.imu.linear_acceleration.z = z except e: print e return
def data_handler(self, bin_data_pack): if struct.unpack("20B", bin_data_pack)[2] == self.TYPE_GYRO: try: x, y, z = \ self.protocol.decode(bin_data_pack) self.imu.angular_velocity.x = x self.imu.angular_velocity.y = y self.imu.angular_velocity.z = z self.imu.header.frame_id = 'base_footprint' self.imu.header.stamp = rospy.Time.now() ChassisDev.pub_data_update(self, self.imu) except Exception as e: print e return else: try: x, y, z = \ self.protocol.decode(bin_data_pack) self.imu.linear_acceleration.x = x self.imu.linear_acceleration.y = y self.imu.linear_acceleration.z = z except e: print e return
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "Speed", pub_topic = "/speed_wheel", pub_msg_class = Float32MultiArray, pub_rate = 20) self.speeds = Float32MultiArray() self.speeds.data = [0,0,0] ChassisDev.pub_data_update(self, self.speeds) ChassisDev.start_pub(self)
def __init__(self): self.protocol = Protocol(self.TYPE, self.pack_pmt) ChassisDev.__init__(self, "Speed", pub_topic="/speed_wheel", pub_msg_class=Float32MultiArray, pub_rate=20) self.speeds = Float32MultiArray() self.speeds.data = [0, 0, 0] ChassisDev.pub_data_update(self, self.speeds) ChassisDev.start_pub(self)
def start_subscribe(self): def cb_func(pwm_data): self.dev_write(self.pwm_to_encoder(pwm_data.data)) ChassisDev.start_sub(self, cb_func)