-
Notifications
You must be signed in to change notification settings - Fork 0
/
_GoToPoint.py
100 lines (74 loc) · 2.66 KB
/
_GoToPoint.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
from kubs import kubs, cmd_node
from velocity.run import *
from velocity.run_w import *
import rospy,sys
from krssg_ssl_msgs.msg import point_2d
from krssg_ssl_msgs.msg import BeliefState
from krssg_ssl_msgs.msg import gr_Commands
from krssg_ssl_msgs.msg import gr_Robot_Command
from utils.geometry import Vector2D
from utils.config import *
from utils.math_functions import *
import memcache
shared = memcache.Client(BS_ADDRESS,debug=0)
kub = None
start_time = None
GOAL_POINT = None
FLAG_move = False
FLAG_turn = False
rotate = 0
FIRST_CALL = True
def init(_kub,target,theta):
global kub,GOAL_POINT,rotate,FLAG_turn,FLAG_move,FIRST_CALL
kub = _kub
GOAL_POINT = point_2d()
rotate = theta
GOAL_POINT.x = target.x
GOAL_POINT.y = target.y
FLAG_move = False
FLAG_turn = False
FIRST_CALL = True
def reset():
global start_time
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
def execute(startTime,DIST_THRESH,avoid_ball=False):
global GOAL_POINT, start_time,FIRST_CALL,FLAG_turn,FLAG_move,kub
# print DIST_THRESH
if FIRST_CALL:
start_time = startTime
FIRST_CALL = False
while not (FLAG_move and FLAG_turn):
# print not (FLAG_move and FLAG_turn)
kub.state = shared.get('state')
# print " in _ ",kub.state.ballPos.x
t = rospy.Time.now()
t = t.secs + 1.0*t.nsecs/pow(10,9)
[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, kub.kubs_id, GOAL_POINT, kub.state.homePos, kub.state.awayPos, avoid_ball)
vw = Get_Omega(kub.kubs_id,rotate,kub.state.homePos)
if not vw:
# print "Didn't receive Omega"
vw = 0
if(REPLANNED):
reset()
kub.move(vx, vy)
kub.turn(vw)
print vx,vy,vw
# print radian_2_deg(kub.state.homePos[kub.kubs_id].theta-rotate),radian_2_deg(ROTATION_FACTOR)
# print dist(kub.state.homePos[kub.kubs_id], GOAL_POINT),DIST_THRESH
# print kub.state.homePos[kub.kubs_id].x,kub.state.homePos[kub.kubs_id].y
# print GOAL_POINT.x,GOAL_POINT.y
# print FLAG_move,"flag mobve fujfgciuygvb"
if abs(kub.state.homePos[kub.kubs_id].theta-rotate)<ROTATION_FACTOR:
kub.turn(0)
FLAG_turn = True
if dist(kub.state.homePos[kub.kubs_id], GOAL_POINT)<DIST_THRESH :
kub.move(0,0)
FLAG_move = True
kub.execute()
yield kub,GOAL_POINT
kub.execute()
kub.execute()
# print "sd", not (FLAG_move and FLAG_turn)
# print kub.get_pos().x,kub.get_pos().y,GOAL_POINT.x,GOAL_POINT.y,
yield kub,GOAL_POINT