def send_command(pub, team, bot_id, v_x, v_y, v_w, kick_power, dribble, chip_power = 0):
"""
Publish the command packet
team : 'True' if the team is yellow
"""
gr_command = gr_Robot_Command()
final_command = gr_Commands()
"""
Set the command to each bot
"""
gr_command.id = bot_id
gr_command.wheelsspeed = 0
gr_command.veltangent = v_x/1000
gr_command.velnormal = v_y/1000
gr_command.velangular = v_w
gr_command.kickspeedx = kick_power
gr_command.kickspeedz = chip_power
gr_command.spinner = dribble
final_command.timestamp = rospy.get_rostime().secs
final_command.isteamyellow = team
final_command.robot_commands = gr_command
def debug():
"""
Log the commands
"""
print 'botid: {}: [{}]\n'.format(bot_id, final_command.timestamp)
print 'vel_x: {}\nvel_y: {}\nvel_w: {}\n'.format(v_x, v_y, v_w)
print 'kick_power: {}\nchip_power: {}\ndribble_speed:{}\n\n'.format(kick_power, chip_power, dribble)
pub.publish(final_command)
def CallbackPID(msg):
global pub
global lastTime
global pso
# print("PID Callback OK")
velX = msg.velX * 1000
velY = msg.velY * 1000
flag = msg.flag
if flag:
print("Replanned")
pso = PSO(15, 20, 1000, 1, 1, 0.5)
errorInfo.errorX = msg.errorX
errorInfo.errorY = msg.errorY
# print("INitial",velX,velY)
vX, vY = pid(velX, velY, errorInfo, pso)
# vX,vY = velX,velY
# print("Changed",vX,vY)
botAngle = msg.botAngle
# print("BotAngle ", botAngle)
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
# print("Velocity ",vXBot,vYBot)
command_msgs = gr_Robot_Command()
final_msgs = gr_Commands()
command_msgs.id = 0
command_msgs.wheelsspeed = 0
command_msgs.veltangent = vXBot / 1000
command_msgs.velnormal = vYBot / 1000
command_msgs.velangular = 0
command_msgs.kickspeedx = 0
command_msgs.kickspeedz = 0
command_msgs.spinner = False
if (msg.velX == msg.velY and msg.velX == 0):
command_msgs.velnormal = command_msgs.veltangent = 0
# t = rospy.get_rostime()
# currTime = t.secs + t.nsecs/pow(10,9)
# diffT = float(currTime - lastTime)
# command_msgs.nextExpectedX = vXBot*diffT + homePos[0][0];
# command_msgs.nextExpectedY = vYBot*diffT + homePos[0][1];
# final_msgs.timestamp = ros::Time::now().toSec()
final_msgs.isteamyellow = False
final_msgs.robot_commands = command_msgs
# lastTime = currTime
pub.publish(final_msgs)
def send_command(pub,
team,
bot_id,
v_x,
v_y,
v_w,
kick_power,
dribble,
chip_power=0):
"""
team : 'True' if the team is yellow
"""
gr_command = gr_Robot_Command()
final_command = gr_Commands()
# Set the command to each bot
gr_command.id = bot_id
gr_command.wheelsspeed = 0
gr_command.veltangent = v_x / 1000.0
gr_command.velnormal = v_y / 1000.0
gr_command.velangular = v_w
gr_command.kickspeedx = kick_power
gr_command.kickspeedz = chip_power
gr_command.spinner = dribble
final_command.timestamp = rospy.get_rostime().secs
final_command.isteamyellow = team
final_command.robot_commands = gr_command
# print("Skill node dribler ",dribble)
# # Log the commands
# # print 'botid: {}: [{}]\n'.format(bot_id, final_command.timestamp)
# print("sabse niche nahi")
#print 'vel_x: {}\nvel_y: {}\nvel_w: {}\n'.format(gr_command.velnormal, gr_command.veltangent, gr_command.velangular)
# print 'kick_power: {}\nchip_power: {}\ndribble_speed:{}\n\n'.format(kick_power, chip_power, dribble)
# Publish the command packet
pub.publish(final_command)
def send_command(team,
bot_id,
v_x,
v_y,
v_w,
kick_power,
dribble,
speed,
chip_power=0):
""" ,
Publish the command packet
team : 'True' if the team is yellow
"""
pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
gr_command = gr_Robot_Command()
final_command = gr_Commands()
"""
Set the command to each bot
"""
state = None
#state=shared.get('state')
rospy.wait_for_service('bsServer', )
# getState = rospy.ServiceProxy('bsServer',bsServer)
# try:
# state = getState(state)
# except rospy.ServiceException, e:
# print("hello")
# if state:
# #print('lasknfcjscnajnstate',state.stateB.homePos)
# state=state.stateB
# kub = kubs.kubs(0,state,pub)
# kub.update_state(state)
# print(kub.kubs_id)
# angle=atan2(state.homePos[0].y,state.homePos[0].x)
# #sprint('chal ja')
# print fa,"-------------------- ",angle
# v_x=0
# #v_y=v*math.cos(fa-angle)/math.cos(fa)
# v_y=v
if (v_x > 0):
v_x += speed
elif (v_x < 0):
v_x -= speed
if (v_y > 0):
v_y += speed
elif (v_y < 0):
v_y -= speed
gr_command.id = bot_id
gr_command.wheelsspeed = 0
gr_command.veltangent = v_x
gr_command.velnormal = v_y
gr_command.velangular = v_w
gr_command.kickspeedx = kick_power
gr_command.kickspeedz = chip_power
gr_command.spinner = dribble
final_command.timestamp = rospy.get_rostime().secs
final_command.isteamyellow = team
final_command.robot_commands = gr_command
def debug():
print 'botid: {}: [{}]\n'.format(bot_id, final_command.timestamp)
print 'vel_x: {}\nvel_y: {}\nvel_w: {}\n'.format(v_x, v_y, v_w)
print 'kick_power: {}\nchip_power: {}\ndribble_speed:{}\n\n'.format(
kick_power, chip_power, dribble)
pub.publish(final_command)
def init_module():
pub = rospy.Publisher('/bot_data', gr_Commands, queue_size=1000)
rospy.init_node('skills_node', anonymous=False)
gr_command = gr_Robot_Command()
final_command = gr_Commands()