def arm_speed_callback(data):
global arm_command_list
global arm_speeed_pub
global arm_speeds
arm_speeds = data.data
arm_command_list = RosiMovementArray()
arm_command = RosiMovement()
arm_command.nodeID = 1
arm_command.joint_var = arm_speeds[0]
arm_command_list.movement_array.append(arm_command)
arm_command = RosiMovement()
arm_command.nodeID = 2
arm_command.joint_var = arm_speeds[1]
arm_command_list.movement_array.append(arm_command)
arm_command = RosiMovement()
arm_command.nodeID = 3
arm_command.joint_var = arm_speeds[2]
arm_command_list.movement_array.append(arm_command)
arm_command = RosiMovement()
arm_command.nodeID = 4
arm_command.joint_var = arm_speeds[3]
arm_command_list.movement_array.append(arm_command)
def traction_speed_callback(data):
global traction_command_list
global traction_speeed_pub
global traction_speeds
traction_speeds = data.data
#print(traction_speeds)
traction_command_list = RosiMovementArray()
traction_command = RosiMovement()
traction_command.nodeID = 1
traction_command.joint_var = traction_speeds[0]
traction_command_list.movement_array.append(traction_command)
traction_command = RosiMovement()
traction_command.nodeID = 2
traction_command.joint_var = traction_speeds[1]
traction_command_list.movement_array.append(traction_command)
traction_command = RosiMovement()
traction_command.nodeID = 3
traction_command.joint_var = traction_speeds[2]
traction_command_list.movement_array.append(traction_command)
traction_command = RosiMovement()
traction_command.nodeID = 4
traction_command.joint_var = traction_speeds[3]
traction_command_list.movement_array.append(traction_command)
def Arms_Control(arms_joint_position):
global front_arms_angle
global back_arms_angle
right_front_arm = arms_joint_position.movement_array[0].joint_var
right_back_arm = arms_joint_position.movement_array[1].joint_var
left_front_arm = arms_joint_position.movement_array[2].joint_var
left_back_arm = arms_joint_position.movement_array[3].joint_var
feedback = rospy.Publisher('/rosi/command_arms_speed',
RosiMovementArray,
queue_size=1)
if (right_front_arm < (-np.pi / 2)):
right_front_arm += 2 * np.pi
if (left_front_arm > (np.pi / 2)):
left_front_arm += -2 * np.pi
front_arms_angle *= np.pi / 180
back_arms_angle *= np.pi / 180
ang_ref1, ang_ref3 = front_arms_angle, -front_arms_angle
ang_ref2, ang_ref4 = back_arms_angle, -back_arms_angle
#calculates the difference between the ideal position and the actual position of the arms and amplifies it
error1 = (right_front_arm - ang_ref1) * 10
error3 = (ang_ref3 - left_front_arm) * 10
error2 = (right_back_arm - ang_ref2) * 10
error4 = (ang_ref4 - left_back_arm) * 10
speed = 0
front_right_engine = RosiMovement()
front_right_engine.nodeID = 1
front_right_engine.joint_var = error1
back_right_engine = RosiMovement()
back_right_engine.nodeID = 2
back_right_engine.joint_var = error2
front_left_engine = RosiMovement()
front_left_engine.nodeID = 3
front_left_engine.joint_var = error3
back_left_engine = RosiMovement()
back_left_engine.nodeID = 4
back_left_engine.joint_var = error4
engine = RosiMovementArray()
engine.movement_array = (front_right_engine, back_right_engine,
front_left_engine, back_left_engine)
feedback.publish(engine)
front_arms_angle *= 180 / np.pi
back_arms_angle *= 180 / np.pi
def __init__(self):
# initializing some attributes
self.omega_left = 0
self.omega_right = 0
self.arm_front_rotSpeed = 0
self.arm_rear_rotSpeed = 0
# computing the kinematic A matrix
self.kin_matrix_A = self.compute_kinematicAMatrix(
self.var_lambda, self.wheel_radius, self.ycir)
# sends a message to the user
rospy.loginfo('Rosi_joy node started')
# registering to publishers
self.pub_traction = rospy.Publisher('/rosi/command_traction_speed',
RosiMovementArray,
queue_size=1)
# registering to subscribers
self.sub_joy = rospy.Subscriber('/joy', Joy, self.callback_Joy)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
arm_command_list = RosiMovementArray()
traction_command_list = RosiMovementArray()
# mounting the lists
for i in range(4):
# ----- treating the traction commands
traction_command = RosiMovement()
# mount traction command list
traction_command.nodeID = i + 1
# separates each traction side command
if i < 2:
traction_command.joint_var = self.omega_right
else:
traction_command.joint_var = self.omega_left
# appending the command to the list
traction_command_list.movement_array.append(traction_command)
# publishing
self.pub_traction.publish(traction_command_list)
# sleeps for a while
node_sleep_rate.sleep()
def __init__(self):
# Comandos que serao enviados para as rodas da direita e da esquerda
self.omega_left = 0
self.omega_right = 0
# Atalhos para informar que deve mandar velocidade 0 quando nao houver comandos de velocidade
self.last = 0
self.TIME_OUT = 0.1
# Mensagem de inicializacao
rospy.loginfo('Controle de baixo nivel iniciado')
# Publicar em command_traction_speed
self.pub_traction = rospy.Publisher('/rosi/command_traction_speed',
RosiMovementArray,
queue_size=1)
# Subscrever em aai_rosi_cmd_vel
self.sub_cmd_vel = rospy.Subscriber('/aai_rosi_cmd_vel', Twist,
self.callback_cmd_vel)
# Frequencia de publicacao
node_sleep_rate = rospy.Rate(10)
# Loop principal, responsavel pelos procedimentos chaves do programa
while not rospy.is_shutdown():
# Comando de tracao a ser publicado
traction_command_list = RosiMovementArray()
# Criar traction_command_list como uma soma de traction_commands
for i in range(4):
# Um comando de tracao por roda
traction_command = RosiMovement()
# ID da roda
traction_command.nodeID = i + 1
# Publicar 0 caso nao haja comandos de velociade
if rospy.get_rostime().to_sec() - self.last >= self.TIME_OUT:
traction_command.joint_var = 0
else:
# Separa as rodas do lado direito do esquerdo
if i < 2:
traction_command.joint_var = self.omega_right
else:
traction_command.joint_var = self.omega_left
# Adiciona o comando ao comando de tracao final
traction_command_list.movement_array.append(traction_command)
# Publicacao
self.pub_traction.publish(traction_command_list)
# Pausa
node_sleep_rate.sleep()
def talker():
global velocity
pub_rosi_movement = rospy.Publisher('/rosi/command_traction_speed',
RosiMovementArray,
queue_size=1)
sub_twist = rospy.Subscriber('cmd_vel', Twist, twist_callback)
rospy.init_node('twist_to_rosi_movement')
rate = rospy.Rate(10)
kin_matrix_A = compute_kinematicAMatrix(var_lambda, wheel_radius, ycir)
while not rospy.is_shutdown():
vel_linear = velocity.linear.x * 100
vel_angular = velocity.angular.z * 50
print('celocidade', velocity)
b = np.array([[vel_linear], [vel_angular]])
# finds the joints control
x = np.linalg.lstsq(kin_matrix_A, b, rcond=-1)[0]
print(x)
# query the sides velocities
omega_right = np.deg2rad(x[0][0])
omega_left = np.deg2rad(x[1][0])
print('omegas', omega_right, ' ', omega_left)
traction_command_list = RosiMovementArray()
for i in range(4):
# ----- treating the traction commands
traction_command = RosiMovement()
# mount traction command list
traction_command.nodeID = i + 1
# separates each traction side command
if i < 2:
traction_command.joint_var = omega_right
else:
traction_command.joint_var = omega_left
# appending the command to the list
traction_command_list.movement_array.append(traction_command)
#print('traction_list ',traction_command_list)
# publishing
pub_rosi_movement.publish(traction_command_list)
# sleeps for a while
rate.sleep()
def __init__(self):
# sends a message to the user
rospy.loginfo('Rosi_joy node started')
# registering to publishers
self.pub_traction = rospy.Publisher('/rosi/command_traction_speed',
RosiMovementArray,
queue_size=1)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
traction_command_list = RosiMovementArray()
# mounting the lists
for i in range(4):
# ----- treating the traction commands
traction_command = RosiMovement()
# mount traction command list
traction_command.nodeID = i + 1
# separates each traction side command
if i < 2:
traction_command.joint_var = 5
else:
traction_command.joint_var = 5
# appending the command to the list
traction_command_list.movement_array.append(traction_command)
# ----- treating the arms commands
self.pub_traction.publish(traction_command_list)
# sleeps for a while
node_sleep_rate.sleep()
def __init__(self):
# Comandos que serao enviados para as rodas da frente e de tras
self.omega_front = 0
self.omega_back = 0
# Representacao da posicao desejada dos bracos
self.desired_front = 0
self.desired_back = 0
# Variavel de controle
self.state = 0
# Mensagem de inicializacao
rospy.loginfo('Controle das escadas iniciado')
# Topicos em que publica e subscreve
self.pub_arms_vel = rospy.Publisher('/rosi/command_arms_speed', RosiMovementArray, queue_size=1)
self.sub_arms_pos = rospy.Subscriber('/rosi/arms_joints_position', RosiMovementArray, self.callback_arms)
self.sub_pose = rospy.Subscriber('/aai_rosi_pose', Pose, self.callback_pose)
# Frequencia de publicacao
node_sleep_rate = rospy.Rate(10)
# Loop principal do algoritmo
while not rospy.is_shutdown():
# Comando de tracao a ser publicado
traction_command_list = RosiMovementArray()
# Criar traction_command_list como uma soma de traction_commands
for i in range(4):
# Um comando de tracao por roda
traction_command = RosiMovement()
# ID da roda
traction_command.nodeID = i+1
# Separa as rodas da frente e de tras
if i == 0 or i == 2:
traction_command.joint_var = self.omega_front
else:
traction_command.joint_var = self.omega_back
# Adiciona o comando ao comando de tracao final
traction_command_list.movement_array.append(traction_command)
# Publicacao
self.pub_arms_vel.publish(traction_command_list)
# Pausa
node_sleep_rate.sleep()
def __init__(self):
self.arm_front_right_rotSpeed = 0
self.arm_front_left_rotSpeed = 0
self.arm_rear_right_rotSpeed = 0
self.arm_rear_left_rotSpeed = 0
self.arm_front_direction = 0
self.arm_rear_direction = 1
self.arm_front_setPoint = 2.7
self.arm_rear_setPoint = 0.5
self.arm_front_right_position = 0
self.arm_front_left_position = 0
self.arm_rear_right_position = 0
self.arm_rear_left_position = 0
# sends a message to the user
rospy.loginfo('Control_arm_speed node started')
# registering to publisher
self.pub_arm = rospy.Publisher('/rosi/command_arms_speed',
RosiMovementArray,
queue_size=1)
# registering to subscribers
self.sub_Arm_sp = rospy.Subscriber('/arm_sp', ArmsSetPoint,
self.callback_Arm_sp)
self.sub_Arm_position = rospy.Subscriber('/rosi/arms_joints_position',
RosiMovementArray,
self.callback_Arm_position)
# defining the eternal loop frequency
node_sleep_rate = rospy.Rate(10)
# eternal loop (until second order)
while not rospy.is_shutdown():
if self.arm_front_direction == 0:
if (abs(self.arm_front_setPoint -
self.arm_front_right_position) < 0.06):
self.arm_front_right_rotSpeed = 0
elif (self.arm_front_setPoint < self.arm_front_right_position):
self.arm_front_right_rotSpeed = -(
(2 * math.pi + self.arm_front_setPoint) -
self.arm_front_right_position)
else:
self.arm_front_right_rotSpeed = -(
self.arm_front_setPoint -
self.arm_front_right_position)
if (abs(self.arm_front_setPoint -
abs(self.arm_front_left_position)) < 0.06):
self.arm_front_left_rotSpeed = 0
elif (self.arm_front_setPoint < abs(
self.arm_front_left_position)):
self.arm_front_left_rotSpeed = -(
(2 * math.pi + self.arm_front_setPoint) -
abs(self.arm_front_left_position))
else:
self.arm_front_left_rotSpeed = -(
self.arm_front_setPoint -
abs(self.arm_front_left_position))
else:
if (abs(self.arm_front_setPoint -
self.arm_front_right_position) < 0.06):
self.arm_front_right_rotSpeed = 0
elif (self.arm_front_setPoint < self.arm_front_right_position):
self.arm_front_right_rotSpeed = (
self.arm_front_right_position -
self.arm_front_setPoint)
else:
self.arm_front_right_rotSpeed = (
(2 * math.pi + self.arm_front_right_position) -
self.arm_front_setPoint)
if (abs(self.arm_front_setPoint -
abs(self.arm_front_left_position)) < 0.06):
self.arm_front_left_rotSpeed = 0
elif (self.arm_front_setPoint < abs(
self.arm_front_left_position)):
self.arm_front_left_rotSpeed = (
abs(self.arm_front_left_position) -
self.arm_front_setPoint)
else:
self.arm_front_left_rotSpeed = (
(2 * math.pi + abs(self.arm_front_left_position)) -
self.arm_front_setPoint)
if self.arm_rear_direction == 1:
if (abs(self.arm_rear_setPoint -
abs(self.arm_rear_right_position)) < 0.06):
self.arm_rear_right_rotSpeed = 0
elif (self.arm_rear_setPoint < abs(
self.arm_rear_right_position)):
self.arm_rear_right_rotSpeed = (
(2 * math.pi + self.arm_rear_setPoint) -
abs(self.arm_rear_right_position))
else:
self.arm_rear_right_rotSpeed = (
self.arm_rear_setPoint -
abs(self.arm_rear_right_position))
if (abs(self.arm_rear_setPoint - self.arm_rear_left_position) <
0.06):
self.arm_rear_left_rotSpeed = 0
elif (self.arm_rear_setPoint < self.arm_rear_left_position):
self.arm_rear_left_rotSpeed = (
(2 * math.pi + self.arm_rear_setPoint) -
self.arm_rear_left_position)
else:
self.arm_rear_left_rotSpeed = (self.arm_rear_setPoint -
self.arm_rear_left_position)
else:
if (abs(self.arm_rear_setPoint -
abs(self.arm_rear_right_position)) < 0.06):
self.arm_rear_right_rotSpeed = 0
elif (self.arm_rear_setPoint < abs(
self.arm_rear_right_position)):
self.arm_rear_right_rotSpeed = -(abs(
self.arm_rear_right_position) - self.arm_rear_setPoint)
else:
self.arm_rear_right_rotSpeed = -(
(2 * math.pi + abs(self.arm_rear_right_position)) -
self.arm_rear_setPoint)
if (abs(self.arm_rear_setPoint - self.arm_rear_left_position) <
0.06):
self.arm_rear_left_rotSpeed = 0
elif (self.arm_rear_setPoint < self.arm_rear_left_position):
self.arm_rear_left_rotSpeed = -(
self.arm_rear_left_position - self.arm_rear_setPoint)
else:
self.arm_rear_left_rotSpeed = -(
(2 * math.pi + self.arm_rear_left_position) -
self.arm_rear_setPoint)
#self.arm_rear_left_rotSpeed = 0
print(self.arm_rear_left_rotSpeed)
arm_command_list = RosiMovementArray()
# mounting the lists
for i in range(4):
# ----- treating the arms commands
arm_command = RosiMovement()
# mounting arm command list
arm_command.nodeID = i + 1
# separates each arm side command
if i == 0:
arm_command.joint_var = 1.5 * self.arm_front_right_rotSpeed
elif i == 2:
arm_command.joint_var = 1.5 * self.arm_front_left_rotSpeed
elif i == 1:
arm_command.joint_var = 1.5 * self.arm_rear_right_rotSpeed
else:
arm_command.joint_var = 1.5 * self.arm_rear_left_rotSpeed
# appending the command to the list
arm_command_list.movement_array.append(arm_command)
# publishing
self.pub_arm.publish(arm_command_list)
# sleeps for a while
node_sleep_rate.sleep()
def Orientation_Control(Imu_data):
global x_gps
global y_gps
global z_gps
global y_ref
global x_ref
global gps_map
global speed
global z_quaternion_reference_gps, resolution
global horizontal_reference_gps
#calculates orientation and adjust with the reference angle
z_quaternion_reference = z_quaternion_reference_gps
horizontal_reference = horizontal_reference_gps
x = x_gps
y = y_gps
z = z_gps
feedback = rospy.Publisher('/rosi/command_traction_speed',
RosiMovementArray,
queue_size=1)
resolution = 0.01
gps_map_x = int(x / resolution) + 6000
gps_map_y = int(y / resolution) + 600
if (gps_map_x < 6000 and gps_map_x >= 0) and (gps_map_y < 1200
and gps_map_y >= 0):
gps_map[gps_map_x, gps_map_y] = (255, 0, 0)
cv2.imwrite('gps_map.png', gps_map)
b = Imu_data.orientation.x
c = Imu_data.orientation.y
d = Imu_data.orientation.z
a = Imu_data.orientation.w
error = 0.0
if (a / np.absolute(a) * d < 0):
left_angle = -1
else:
left_angle = 1
angle_ref = np.arcsin(z_quaternion_reference) * 2
angle = np.arcsin(np.absolute(d)) * 2
if (speed < 0):
horizontal_reference *= speed / np.absolute(speed)
angle_ref = np.pi - angle_ref
right_orientation_angle = horizontal_reference * angle_ref - left_angle * angle
if (right_orientation_angle < 0):
right_orientation_angle += 2 * np.pi
left_orientation_angle = 2 * np.pi - right_orientation_angle
constant = 5
if (right_orientation_angle < left_orientation_angle):
error = right_orientation_angle * constant
else:
error = -left_orientation_angle * constant
if (speed == 0):
error = 0
error12 = 0
error34 = 0
elif (speed > 0):
if (error > np.absolute(speed)):
error12 = np.absolute(speed)
error34 = 3 * np.absolute(speed)
elif (error < -np.absolute(speed)):
error12 = -3 * np.absolute(speed)
error34 = -np.absolute(speed)
else:
error12 = error
error34 = error
else:
if (error > np.absolute(speed)):
error34 = np.absolute(speed)
error12 = 3 * np.absolute(speed)
elif (error < -np.absolute(speed)):
error34 = -3 * np.absolute(speed)
error12 = -np.absolute(speed)
else:
error12 = error
error34 = error
speed1, speed3, speed2, speed4 = speed, speed, speed, speed
front_right_engine = RosiMovement()
front_right_engine.nodeID = 1
front_right_engine.joint_var = speed1 + error12
back_right_engine = RosiMovement()
back_right_engine.nodeID = 2
back_right_engine.joint_var = speed2 + error12
front_left_engine = RosiMovement()
front_left_engine.nodeID = 3
front_left_engine.joint_var = speed3 - error34
back_left_engine = RosiMovement()
back_left_engine.nodeID = 4
back_left_engine.joint_var = speed4 - error34
engine = RosiMovementArray()
engine.movement_array = (front_right_engine, back_right_engine,
front_left_engine, back_left_engine)
feedback.publish(engine)
print "xref: ", x_ref, "yref: ", y_ref
print 'x: ', x, 'y: ', y
print "error = d_ref - d: ", error
print "error12: ", error12
print "error34: ", error34
print "right_orientation_angle: ", right_orientation_angle
print "left_orientation_angle: ", left_orientation_angle
print "angle_ref: ", angle_ref
print "horizontal_ref: ", horizontal_reference
print "angle = z: ", angle
print "left_angle: ", left_angle
print '-------------------------'