def __init__(self):
rospy.init_node(NODE_NAME)
self.iris_drive_command_topic = rospy.get_param(
"~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
self.ground_station_drive_command_topic = \
rospy.get_param("~ground_station_drive_command_topic", DEFAULT_GROUND_STATION_DRIVE_COMMAND_TOPIC)
self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic",
DEFAULT_REAR_BOGIE_TOPIC)
self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic",
DEFAULT_LEFT_BOGIE_TOPIC)
self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic",
DEFAULT_RIGHT_BOGIE_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
# Drive data
self.drive_command_data = {
"iris": {
"message": DriveCommandMessage(),
"last_time": time()
},
"ground_station": {
"message": DriveCommandMessage(),
"last_time": time()
}
}
# ########## Class Variables ##########
self.iris_drive_command_subscriber = rospy.Subscriber(
self.iris_drive_command_topic, DriveCommandMessage,
self.iris_drive_command_callback)
self.ground_station_command_subscriber = rospy.Subscriber(
self.ground_station_drive_command_topic, DriveCommandMessage,
self.ground_station_drive_command_callback)
self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic,
DriveControlMessage,
queue_size=1)
self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic,
DriveControlMessage,
queue_size=1)
self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic,
DriveControlMessage,
queue_size=1)
self.last_message_time = time()
# ########## Run the Class ##########
self.run()
def broadcast_drive_if_current_mode(self):
if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
command = DriveCommandMessage()
left_y_axis = self.registers[MODBUS_REGISTERS["LEFT_STICK_Y_AXIS"]]
right_y_axis = self.registers[MODBUS_REGISTERS["RIGHT_STICK_Y_AXIS"]]
if left_y_axis == 0 and right_y_axis == 0:
command.controller_present = False
command.ignore_drive_control = True
command.drive_twist.linear.x = 0
command.drive_twist.angular.z = 0
else:
left = (left_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
"SBUS_RANGE"]
right = (right_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
"SBUS_RANGE"]
command.controller_present = True
command.ignore_drive_control = \
self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["IGNORE_CONTROL"]]] > SBUS_VALUES["SBUS_MID"]
command.drive_twist.linear.x = (left + right) / 2.0
command.drive_twist.angular.z = (right - left) / 2.0
self.drive_command_publisher.publish(command)
def get_drive_message(self, speed_limit):
drive_message = DriveCommandMessage()
left_y_axis = self.joystick.controller_states["left_y_axis"] if abs(self.joystick.controller_states["left_y_axis"]) > STICK_DEADBAND else 0
right_y_axis = self.joystick.controller_states["right_y_axis"] if abs(self.joystick.controller_states["right_y_axis"]) > STICK_DEADBAND else 0
left_y_axis = speed_limit * (-(left_y_axis - STICK_OFFSET) / STICK_MAX)
right_y_axis = speed_limit * (-(right_y_axis - STICK_OFFSET) / STICK_MAX)
drive_message.drive_twist.linear.x = (left_y_axis + right_y_axis) / 2.0
drive_message.drive_twist.angular.z = (right_y_axis - left_y_axis) / 2.0
return drive_message
def publish_drive_command(self):
if self.drive_paused:
drive_message = DriveCommandMessage()
else:
drive_message = self.get_drive_message(self.speed_limit)
left_output = abs(drive_message.drive_twist.linear.x - drive_message.drive_twist.angular.z)
right_output = abs(drive_message.drive_twist.linear.x + drive_message.drive_twist.angular.z)
self.set_speed_limit__signal.emit(self.speed_limit * 100)
self.set_left_drive_output__signal.emit(left_output * 100)
self.set_right_drive_output__signal.emit(right_output * 100)
self.drive_command_publisher.publish(drive_message)
def publish_drive_command(self):
throttle_axis = max(
(255 - self.joystick.controller_states["throttle_axis"]) / 255.0,
THROTTLE_MIN)
if self.drive_paused:
drive_message = DriveCommandMessage()
else:
drive_message = self.get_drive_message(throttle_axis)
left_output = abs(drive_message.drive_twist.linear.x -
drive_message.drive_twist.angular.z)
right_output = abs(drive_message.drive_twist.linear.x +
drive_message.drive_twist.angular.z)
self.set_speed_limit__signal.emit(throttle_axis * 100)
self.set_left_drive_output__signal.emit(left_output * 100)
self.set_right_drive_output__signal.emit(right_output * 100)
self.drive_command_publisher.publish(drive_message)
def get_drive_message(self, throttle_axis):
drive_message = DriveCommandMessage()
y_axis = throttle_axis * (
-(self.joystick.controller_states["y_axis"] - 512) / 512.0)
z_axis = throttle_axis * (
-(self.joystick.controller_states["z_axis"] - 128) / 128.0)
x_axis = throttle_axis * (
-(self.joystick.controller_states["x_axis"] - 512) / 512.0)
if abs(y_axis) < Y_AXIS_DEADBAND:
y_axis = 0
if abs(x_axis) < X_AXIS_DEADBAND and y_axis == 0:
twist = z_axis
else:
twist = x_axis if y_axis >= 0 else -x_axis
drive_message.drive_twist.linear.x = y_axis
drive_message.drive_twist.angular.z = twist
return drive_message
def send_drive_control_command(self, drive_command_data):
if (time() - drive_command_data["last_time"]) > WATCHDOG_TIMEOUT:
drive_command = DriveCommandMessage()
else:
drive_command = drive_command_data["message"]
rear_drive = DriveControlMessage()
left_drive = DriveControlMessage()
right_drive = DriveControlMessage()
left = drive_command.drive_twist.linear.x - drive_command.drive_twist.angular.z
right = drive_command.drive_twist.linear.x + drive_command.drive_twist.angular.z
left_direction = 1 if left >= 0 else 0
rear_drive.first_motor_direction = left_direction
left_drive.first_motor_direction = left_direction
left_drive.second_motor_direction = left_direction
right_direction = 1 if right >= 0 else 0
rear_drive.second_motor_direction = right_direction
right_drive.first_motor_direction = right_direction
right_drive.second_motor_direction = right_direction
left_speed = min(abs(left * UINT16_MAX), UINT16_MAX)
right_speed = min(abs(right * UINT16_MAX), UINT16_MAX)
rear_drive.first_motor_speed = left_speed
left_drive.first_motor_speed = left_speed
left_drive.second_motor_speed = left_speed
rear_drive.second_motor_speed = right_speed
right_drive.first_motor_speed = right_speed
right_drive.second_motor_speed = right_speed
self.rear_bogie_publisher.publish(rear_drive)
self.left_bogie_publisher.publish(left_drive)
self.right_bogie_publisher.publish(right_drive)