def set_led_gesture(self, gesture, leds, wave, period, length, mirror,
mask):
period = max(-32678, min(32678, int(period * 1000)))
data = [gesture, leds, wave, period, length, mirror] + mask[:8]
data += [1] * (14 - len(data))
self.aseba_led_gesture_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, data))
def callback(msg: ColorRGBA) -> None:
r = int(msg.r * 32)
g = int(msg.g * 32)
b = int(msg.b * 32)
aseba_msg = AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=[r, g, b])
publisher.publish(aseba_msg)
def set_led_gesture(self, gesture: int, leds: int, wave: int,
period: float, length: int, mirror: int,
mask: List[int]) -> None:
period = max(-32678, min(32678, int(period * 1000)))
data = [gesture, leds, wave, period, length, mirror] + mask[:8]
data += [1] * (14 - len(data))
self.aseba_led_gesture_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=data))
def on_led_off(self, msg):
for i in LED_NUMBER.keys():
print 'off ', i
self.aseba_led_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [i] + 8 * [0]))
# sleep to avoid that aseba or ros do not process all messages.
# could be improved by having 6 separate aseba topics where to send messages
rospy.sleep(0.005)
def on_led(self, msg):
i = msg.id
num = LED_NUMBER.get(i, 0)
if num <= len(msg.values):
data = [i] + [int(32 * v) for v in msg.values[:8]]
data += [0] * (9 - len(data))
self.aseba_led_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, data))
def on_sound_threshold(self, msg):
value = msg * 255
if value < 0:
value = 1
if value > 255:
value = 0
self.sound_threshold_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [value]))
def on_sound_threshold(self, msg: AsebaEvent) -> None:
value = msg * 255
if value < 0:
value = 1
if value > 255:
value = 0
self.sound_threshold_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(), source=0,
data=[value]))
def on_sound_play(self, msg: Sound) -> None:
freq = max(1, int(msg.frequency))
duration = rclpy.duration.Duration.from_msg(
msg.duration).nanoseconds / 1e9
duration = max(1, int(duration * 60))
self.aseba_play_sound_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=[freq, duration]))
def on_led(self, msg: Led) -> None:
i = msg.id
num = LED_NUMBER.get(i, 0)
if num <= len(msg.values):
data = [i] + [int(32 * v) for v in msg.values[:8]]
data += [0] * (9 - len(data))
self.aseba_led_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=data))
def on_led_off(self, msg: Empty) -> None:
for i in LED_NUMBER.keys():
self.aseba_led_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=[i] + 8 * [0]))
# sleep to avoid that aseba or ros do not process all messages.
# could be improved by having 6 separate aseba topics where to send
# messages
time.sleep(0.005)
def on_led(self, msg: Led) -> None:
# if Led.CIRCLE != msg.id:
# self.get_logger('warn')('The e-puck only support CIRCLE leds.')
# return
num = 8
if num <= len(msg.values):
data = [0] + [1 if v > 0 else 0 for v in msg.values[:8]]
data += [0] * (9 - len(data))
self.aseba_led_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=data))
def motor_speed(self, value: int) -> None:
if self._motor_speed == value:
return
self._motor_speed = value
if self.motor == 'right':
data = [0, value]
else:
data = [value, 0]
self.pub.publish(AsebaEvent(data=data))
c = 8 * value / 500
leds = [max(0, min(1, c - i)) for i in range(8)]
self.led_pub.publish(Led(id=Led.CIRCLE, values=leds))
def run(self):
standing_wheel = 'right' if self.motor == 'left' else 'left'
rospy.loginfo(
'Place the robot with the %s wheel at the center of the T '
'and press the central button when the robot is ready',
standing_wheel)
self.ping()
rospy.wait_for_message('buttons/center', std_msgs.msg.Bool)
rospy.sleep(3)
msg = rospy.wait_for_message('aseba/events/ground', AsebaEvent)
self.ths = [d - self.gap for d in msg.data]
# count(3)
rospy.loginfo('Start calibrating %s motor using thresholds %s',
self.motor, self.ths)
_n = 1
self.pick = None
for motor_speed in self.motor_speeds:
rospy.loginfo('Set motor speed to %s', motor_speed)
_n += self.target_number_of_samples
self.motor_speed = motor_speed
if self.motor == 'right':
data = [0, self.motor_speed]
else:
data = [self.motor_speed, 0]
self.pub.publish(AsebaEvent(data=data))
while len(self._samples) < _n:
rospy.sleep(0.05)
self.pub.publish(AsebaEvent(data=[0, 0]))
self.sub.unregister()
rospy.logdebug('Calculate with samples %s', self._samples)
self.save_samples()
samples = np.array(self._samples)
results = calibrate(samples)
rospy.loginfo('Calibration of motor %s done: %s', self.motor, results)
self.save_calibration(results)
def on_comm_tx_payload(self, msg: Int16) -> None:
msg = AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=[msg.data])
self.aseba_set_comm_tx_payload_publisher.publish(msg)
def set_speed(self, values):
self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(), 0, values))
def callback(msg):
r = int(msg.r * 32)
g = int(msg.g * 32)
b = int(msg.b * 32)
aseba_msg = AsebaEvent(rospy.get_rostime(), 0, [r, g, b])
publisher.publish(aseba_msg)
def on_set_comm(self, msg):
self.aseba_set_comm_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [enabled, payload]))
def on_shutdown_msg(self, msg: Empty) -> None:
self.aseba_shutdown_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[]))
def play_system_sound(self, sound: int) -> None:
self.aseba_play_system_sound_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(), source=0,
data=[sound]))
def on_comm_enable(self, msg: Bool) -> None:
msg = AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=[msg.data])
self.aseba_enable_comm_publisher.publish(msg)
def on_sound_play(self, msg):
freq = max(1, int(msg.frequency))
duration = max(1, int(msg.duration.to_sec() * 60))
self.aseba_play_sound_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [freq, duration]))
def play_system_sound(self, sound):
self.aseba_play_system_sound_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, [sound]))
def cb(msg: Bool) -> None:
data = [index, 1 if msg.data else 0]
self.aseba_led_publisher.publish(
AsebaEvent(stamp=self.clock.now().to_msg(),
source=0,
data=data))
def on_shutdown_msg(self, msg):
self.aseba_shutdown_publisher.publish(
AsebaEvent(rospy.get_rostime(), 0, []))