def callback(data):
msg = strategy_output_msg()
if data.x[number_of_robots - 1] < 0.6:
msg.x[number_of_robots - 1] = 0.7
msg.y[number_of_robots - 1] = 0
elif data.y[number_of_robots - 1] > 0.2 or data.y[number_of_robots -
1] < -0.2:
msg.x[number_of_robots - 1] = 0.7
msg.y[number_of_robots - 1] = 0
else:
msg.x[number_of_robots - 1] = 0.7
msg.y[number_of_robots - 1] = goalnorm(data)
msg.control_options[number_of_robots - 1] = control_options.position
#for robot in range (number_of_robots - 1):
# if distance_to_ball(data, robot) > 0.08:
# msg.x[robot] = data.ball_x
# msg.y[robot] = data.ball_y
# else:
# msg.x[robot] = -0.75
# msg.y[robot] = 0
# msg.control_options[robot] = control_options.position
pub.publish(msg)
def callback(data):
msg = target_positions_msg()
msg2 = strategy_output_msg()
msg.x[0] = 2
msg2.x[0] = 3
msg2.control_options[0] = control_options.position
pub.publish(msg)
pub2.publish(msg2)
def assembly_msg(output):
aux = strategy_output_msg()
for i in xrange(len(output)):
aux.x[i] = output[i][0]
aux.y[i] = output[i][1]
aux.th[i] = output[i][2]
aux.u[i] = output[i][3]
aux.w[i] = output[i][4]
aux.vx[i] = output[i][5]
aux.vy[i] = output[i][6]
aux.control_options[i] = output[i][7]
return aux
def callback(data):
msg = target_positions_msg()
msg2 = strategy_output_msg()
ball = [data.ball_x, data.ball_y]
for robot in range(number_of_robots):
msg.x[robot] = data.ball_x
msg.y[robot] = data.ball_y
msg2.x[robot] = data.ball_x
msg2.y[robot] = data.ball_y
msg2.control_options[robot] = control_options.position
pub.publish(msg)
pub2.publish(msg2)
def callback(data):
msg = strategy_output_msg()
msg.control_options = [
control_options.pose, control_options.pose_line,
control_options.pose_line
]
msg.x = [-fixed_x, -fixed_x, fixed_x]
ball = [data.ball_x, data.ball_y]
state = 1
states(ball[1], data.y[2], state)
state_machine(state, ball[1], msg)
pub.publish(msg)
def callback(data):
msg = strategy_output_msg()
msg.control_options = [
control_options.position, control_options.position,
control_options.position
]
msg.x[0] = -0.5
msg.y[0] = 0.5
msg.x[1] = 0
msg.y[1] = 0.5
msg.x[2] = 0.5
msg.y[2] = 0.5
pub.publish(msg)
def callback(data):
msg = strategy_output_msg()
msg.control_options = [
control_options.pose, control_options.pose, control_options.pose
]
msg.x[0] = -0.5
msg.y[0] = 0.5
msg.th[0] = pi / 2
msg.x[1] = 0
msg.y[1] = 0.5
msg.th[1] = pi
msg.x[2] = 0.5
msg.y[2] = 0.5
msg.th[2] = pi * (3. / 2)
pub.publish(msg)
def start(team):
global output_msg
pub = rospy.Publisher('strategy_output_topic', strategy_output_msg, queue_size=1)
rospy.init_node('strategy')
rospy.Subscriber('measurement_system_topic', measurement_msg, callback, team)
rospy.Subscriber('keyboard_topic', String, keyboardListener)
rospy.Subscriber('field_side_topic', String, fieldSideListener)
rospy.Subscriber('keyboard_event_topic', Char, keyboardEventListener)
rate = rospy.Rate(30)
output_msg = strategy_output_msg()
try:
while not rospy.is_shutdown():
if (paused == False):
team.run()
else:
team.stop()
output = team.output()
output_msg = assembly_msg(output)
pub.publish(output_msg)
rate.sleep()
except rospy.ROSInterruptException:
exit(1)