def __init__(self):
self._SENSORS = {"left": 0, "mid_left": 0, "mid_right": 0, "right": 0}
self._sensor_line_values = dict(self._SENSORS)
self._sensor_field_values = dict(self._SENSORS)
self._line_thresholds = dict(self._SENSORS)
self._line_is_detected_by_sensor = dict(self._SENSORS)
self._present_sensor_values = dict(self._SENSORS)
self._line_values_are_sampled = False
self._field_values_are_sampled = False
self._can_publish_cmdvel = False
self._mouse_buttons = ButtonValues()
self._pub_cmdvel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self._pub_buzzer = rospy.Publisher('buzzer', UInt16, queue_size=1)
self._pub_leds = rospy.Publisher('leds', LedValues, queue_size=1)
self._sub_lightsensor = rospy.Subscriber('lightsensors',
LightSensorValues,
self._callback_lightsensor,
queue_size=1)
self._sub_buttons = rospy.Subscriber('buttons',
ButtonValues,
self._callback_buttons,
queue_size=1)
try:
rospy.wait_for_service("motor_on", timeout=5)
rospy.wait_for_service("motor_off", timeout=5)
except rospy.exceptions.ROSException as e:
rospy.logerr("Service not found")
rospy.signal_shutdown(e.message)
else:
rospy.on_shutdown(self._on_shutdown)
def __init__(self):
self._mouse_buttons = ButtonValues()
self._imu_data_raw = Imu()
# for acceleration low pass filter
self._filtered_acc = Vector3()
self._prev_acc = Vector3()
# for self.update()
self._current_mode = self._MODE_NONE
self._has_motor_enabled = False
# for angle control
self._omega_pid_controller = PIDController(10, 0, 20)
self._target_angle = 0.0
self._increase_target_angle = True
# for heading_angle calculation
self._heading_angle = 0.0
self._omega_bias = 0.0
self._prev_imu_timestamp = rospy.Time()
self._sub_buttons = rospy.Subscriber('buttons',
ButtonValues,
self._callback_buttons,
queue_size=1)
self._sub_imu = rospy.Subscriber('imu/data_raw',
Imu,
self._callback_imu,
queue_size=1)
self._pub_cmdvel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self._pub_buzzer = rospy.Publisher('buzzer', UInt16, queue_size=1)
try:
rospy.wait_for_service("motor_on", timeout=5)
rospy.wait_for_service("motor_off", timeout=5)
except rospy.exceptions.ROSException as e:
rospy.logerr("Service not found")
rospy.signal_shutdown(e.message)
else:
rospy.on_shutdown(self._motor_off)
def __init__(self):
self._BUTTON_SHUTDOWN_1 = rospy.get_param('~button_shutdown_1')
self._BUTTON_SHUTDOWN_2 = rospy.get_param('~button_shutdown_2')
self._BUTTON_MOTOR_ON = rospy.get_param('~button_motor_on')
self._BUTTON_MOTOR_OFF = rospy.get_param('~button_motor_off')
self._BUTTON_CMD_ENABLE = rospy.get_param('~button_cmd_enable')
self._AXIS_CMD_LINEAR_X = rospy.get_param('~axis_cmd_linear_x')
self._AXIS_CMD_ANGULAR_Z = rospy.get_param('~axis_cmd_angular_z')
self._ANALOG_D_PAD = rospy.get_param('~analog_d_pad')
self._D_PAD_UP = rospy.get_param('~d_pad_up')
self._D_PAD_DOWN = rospy.get_param('~d_pad_down')
self._D_PAD_LEFT = rospy.get_param('~d_pad_left')
self._D_PAD_RIGHT = rospy.get_param('~d_pad_right')
self._D_UP_IS_POSITIVE = rospy.get_param('~d_pad_up_is_positive')
self._D_RIGHT_IS_POSITIVE = rospy.get_param('~d_pad_right_is_positive')
self._BUTTON_BUZZER_ENABLE = rospy.get_param('~button_buzzer_enable')
self._DPAD_BUZZER0 = rospy.get_param('~dpad_buzzer0')
self._DPAD_BUZZER1 = rospy.get_param('~dpad_buzzer1')
self._DPAD_BUZZER2 = rospy.get_param('~dpad_buzzer2')
self._DPAD_BUZZER3 = rospy.get_param('~dpad_buzzer3')
self._BUTTON_BUZZER4 = rospy.get_param('~button_buzzer4')
self._BUTTON_BUZZER5 = rospy.get_param('~button_buzzer5')
self._BUTTON_BUZZER6 = rospy.get_param('~button_buzzer6')
self._BUTTON_BUZZER7 = rospy.get_param('~button_buzzer7')
self._BUTTON_SENSOR_SOUND_EN = rospy.get_param('~button_sensor_sound_en')
self._BUTTON_CONFIG_ENABLE = rospy.get_param('~button_config_enable')
# for _joy_velocity_config()
self._MAX_VEL_LINEAR_X = 2.0 # m/s
self._MAX_VEL_ANGULAR_Z = 2.0 * math.pi # rad/s
self._DEFAULT_VEL_LINEAR_X = 0.5 # m/s
self._DEFAULT_VEL_ANGULAR_Z = 1.0 * math.pi # rad/s
self._joy_msg = None
self._lightsensor = LightSensorValues()
self._mouse_buttons = ButtonValues()
self._cmdvel_has_value = False
self._buzzer_has_value = False
self._sensor_sound_has_value = False
self._vel_linear_x = self._DEFAULT_VEL_LINEAR_X
self._vel_angular_z = self._DEFAULT_VEL_ANGULAR_Z
self._pub_cmdvel = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self._pub_buzzer = rospy.Publisher('buzzer', UInt16, queue_size=1)
self._pub_leds = rospy.Publisher('leds', LedValues, queue_size=1)
self._sub_joy = rospy.Subscriber('joy', Joy, self._callback_joy, queue_size=1)
self._sub_lightsensor = rospy.Subscriber(
'lightsensors', LightSensorValues, self._callback_lightsensor, queue_size=1)
self._sub_buttons = rospy.Subscriber(
'buttons', ButtonValues, self._callback_buttons, queue_size=1)
try:
rospy.wait_for_service("motor_on", timeout=5)
rospy.wait_for_service("motor_off", timeout=5)
except rospy.exceptions.ROSException as e:
rospy.logerr("Service not found")
rospy.signal_shutdown(e.message)
else:
rospy.on_shutdown(self._motor_off)
self._motor_on()
if btn_msg.rear_toggle:
global sens
cmd = Twist()
data = sens.reshape(1, -1, 4)
predicted = model.predict(data)
cmd.linear.x = predicted[0][0]
cmd.angular.z = predicted[0][1]
pub.publish(cmd)
if __name__ == '__main__':
rospy.init_node('replay')
sens = [[0] * 4] * 10
historys = [0, 0, 0, 0]
historym = [0, 0]
sensor_values = LightSensorValues()
episode = Event()
on = ButtonValues()
model = Sequential()
rospy.Subscriber('/event', Event, event_callback)
rospy.Subscriber('/lightsensors', LightSensorValues, sensor_callback)
rospy.Subscriber('/buttons', ButtonValues, button_callback, queue_size=1)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
led_pub = rospy.Publisher('/leds', LedValues, queue_size=1)
rospy.spin()