def talker():
pub = rospy.Publisher('/nubot1/nubotcontrol/velcmd', VelCmd, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
vec = VelCmd()
vec.Vx = 50
vec.Vy = 0
vec.w = 0
pub.publish(vec)
rate.sleep()
def pubNubotCtrl(self, x, y, yaw):
angle = yaw
velocity = math.hypot(x, y)
if x != 0:
alpha = math.degrees(math.atan2(y, x))
else:
alpha = 0
dis_max = 2
dis_min = 0.3
velocity_max = 70
velocity_min = 50
angular_velocity_max = 2
angular_velocity_min = 0.5
angle_max = 144
angle_min = 20
angle_out = angle
if velocity == 0:
pass
elif velocity > dis_max:
velocity = velocity_max
elif velocity < dis_min:
velocity = velocity_min
else:
velocity = (velocity_max - velocity_min) * (math.cos(
(((velocity - dis_min) /
(dis_max - dis_min) - 1) * math.pi)) + 1) / 2 + velocity_min
if angle == 0:
pass
elif abs(angle) > angle_max:
angle_out = angular_velocity_max
elif abs(angle) < angle_min:
angle_out = angular_velocity_min
else:
angle_out = (angular_velocity_max -
angular_velocity_min) * (math.cos(
(((angle - angle_min) /
(angle_max - angle_min) - 1) * math.pi)) +
1) / 2 + angular_velocity_min
if angle < 0:
angle_out = -angle_out
x = velocity * math.cos(math.radians(alpha))
y = velocity * math.sin(math.radians(alpha))
yaw = angle_out
vel = VelCmd()
vel.Vx = x
vel.Vy = y
vel.w = yaw
self.nubot_cmd_pub.publish(vel)
def talker():
pub = rospy.Publisher('/nubot1/nubotcontrol/velcmd', VelCmd, queue_size=10)
#對 '...' 之Topic發送資料 10爲緩衝 發送形態爲VelCmd
rospy.init_node('talker', anonymous=True)
#初始化ROS並初始化一個節點叫'talker'
rate = rospy.Rate(10) # 10hz
#update rate of node
while not rospy.is_shutdown():
#無窮迴圈
vec = VelCmd()
vec.Vx = 50
vec.Vy = 0
vec.w = 0
pub.publish(vec)
#publish()內資料
rate.sleep()
def __init__(self):
self.RadHead2Ball = 0.0
self.RadHead = 0.0
self.BallPosX = 0.0
self.BallPosY = 0.0
self.kick_count = 0
self.vec = VelCmd()
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('strategy_node', anonymous=True)
#初始化一個節點,命名爲'strategy_node'
vel_pub = rospy.Publisher('/nubot1/nubotcontrol/velcmd', VelCmd, queue_size=10)
#Publisher
rospy.Subscriber("/nubot1/omnivision/OmniVisionInfo", OminiVisionInfo, callback)
#Subscriber with call back
#
rospy.wait_for_service('/nubot1/BallHandle')
call_Handle = rospy.ServiceProxy('/nubot1/BallHandle', BallHandle)
#
rospy.wait_for_service('/nubot1/Shoot')
call_Shoot = rospy.ServiceProxy('/nubot1/Shoot', Shoot)
# service
rate = rospy.Rate(10)
# resp1 = call_Shoot(1, 1) #cause 2 input in this Shoot case
while not rospy.is_shutdown():
res = call_Handle(1) #start holding device
print(turn(100))
#print(res)
if not call_Handle(1).BallIsHolding: # BallIsHolding = 0
vec = VelCmd()
vec.Vx = MaxSpd * math.cos(A2B)
vec.Vy = MaxSpd * math.sin(A2B)
vec.w = A2B
vel_pub.publish(vec)
else: #when it holds ball
# if
# vec.w =
# y1 = # from GAFuzzy
# vec.w = vec.w # 轉成踢球方向 from GAFuzzy
call_Shoot(50, 1) # from GAFuzzy
#rospy.spin()
rate.sleep()
def __init__(self, team):
self.sac_cal = sac_calculate()
self.a = []
self.s = []
self.r = 0.0
self.done = False
self.avg_arr = np.zeros(64)
self.team = team
self.RadHead2Ball = 0.0
self.RadHead = 0.0
self.NunbotAPosX = 0.0
self.NunbotAPosY = 0.0
self.NunbotBPosX = 0.0
self.BallPosX = 0.0
self.BallPosY = 0.0
self.GoalX = 900.0
self.GoalY = 0.0
self.StartX = -900.0
self.StartY = 0.0
self.kick_count = 0
self.kick_num = 0
self.score = 0
self.RadTurn = 0.0
self.Steal = False
self.dis2start = 0.0
self.dis2goal = 0.0
self.vec = VelCmd()
self.A_info = np.array([1.0, 1.0, 1.0, 1.0, 0, 0, 0, 0])
self.game_count = 2
self.A_z = 0.0
self.B_z = 0.0
self.HowEnd = 0
self.B_dis = 0.0
self.ep_rwd = 0
self.is_kick = False
self.ready2restart = True
self.list_rate = list(np.zeros(128))
self.milestone=[0, 0, 0, 0, 0, 0, 0, 0 , 0 , 0 , 0 ]
self.step_milestone=[0, 0, 0, 0, 0, 0, 0, 0 , 0 , 0 , 0 ]
self.milestone_idx =0
self.is_in = False
self.is_out = False
self.is_steal = False
self.is_fly = False
self.is_stealorfly = False
self.real_resart = True
self.step_count = 0
self.LeftAng = 0
self.Dis2Ball = 0
def __init__(self, team):
self.team = team
self.RadHead2Ball = 0.0
self.RadHead = 0.0
self.NunbotAPosX = 0.0
self.NunbotAPosY = 0.0
self.BallPosX = 0.0
self.BallPosY = 0.0
self.GoalX = 900.0
self.GoalY = 0.0
self.StartX = 0.0
self.StartY = 0.0
self.kick_count = 0
self.kick_num = 0
self.score = 0
self.RadTurn = 0.0
self.Steal = False
self.dis2start = 0.0
self.dis2goal = 0.0
self.vec = VelCmd()
self.A_info = [0]
self.game_count = 2
def Pub_Cmdvel(self, vec, action_dim=1):
velcmd = VelCmd()
if (action_dim == 1):
r"""
vec
vec value
vec ang
yaw 0
"""
velcmd.Vx, velcmd.Vy = self.Calculate_Vel(vec[:2])
velcmd.w = vec[2]
else:
r"""
vec
vec_x
vec_y
yaw
"""
velcmd.Vx = vec[0]
velcmd.Vy = vec[1]
velcmd.w = vec[2]
self.pub_cmdvel.publish(velcmd)
def Pub_Cmdvel(self,vec,action_dim = 1):
velcmd = VelCmd()
if(self.a_info == 'angle'):
velcmd.Vx,velcmd.Vy = self.Calculate_Vel(vec[:2])
velcmd.w = vec[2]
elif(self.a_info == 'scalar_ang'):
velcmd.Vx,velcmd.Vy = self.Calculate_Vel(vec[:2])
velcmd.w = vec[2]
elif(self.a_info == 'linear'):
velcmd.Vx = vec[0]
velcmd.Vy = vec[1]
velcmd.w = 0
elif(self.a_info == 'linear_yaw'):
velcmd.Vx = vec[0]
velcmd.Vy = vec[1]
velcmd.w = vec[2]
self.pub_cmdvel.publish(velcmd)
status = (status + 1) % 15
elif key==' ':
try:
shoot_client_ = rospy.ServiceProxy('Shoot',Shoot)
rsp = shoot_client_(1,1)
except rospy.ServiceException as e:
rospy.logwarn("Service call failed: %s"%e)
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
cmd = VelCmd()
cmd.Vx = -y*speed*200
cmd.Vy = x*speed*200
cmd.w = th*turn*50
if cmd.Vx==0 and cmd.Vy==0 and cmd.w==0:
cmd.stop_ = True
else:
cmd.stop_ = False
cmd_pub_.publish(cmd)
#rospy.loginfo("V: %d %d %d", cmd.Vx, cmd.Vy, cmd.w)
#print("V: %d %d %d" % (cmd.Vx,cmd.Vy,cmd.w))
except Exception as e:
print(e)
def RobotCtrl(self, x, y, yaw): msg = VelCmd() msg.Vx = x msg.Vy = y msg.w = yaw self.cmdvel_pub.publish(msg)
def Pub_Cmdvel(self, vec):
velcmd = VelCmd()
velcmd.Vx = vec[0]
velcmd.Vy = vec[1]
velcmd.w = vec[2]
self.pub_cmdvel.publish(velcmd)