Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

referee + minor changes #81

Open
wants to merge 19 commits into
base: fsm
Choose a base branch
from
Open

referee + minor changes #81

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
72771fe
Merge pull request #1 from KRSSG/fsm
sahil132jindal Feb 29, 2020
6ed2a60
Update ssl.sh
sahil132jindal Feb 29, 2020
086912d
Update CMakeLists.txt
sahil132jindal Feb 29, 2020
b9c6c69
Update GoToBall.py
sahil132jindal Feb 29, 2020
bdfa839
Update GoToPoint.py
sahil132jindal Feb 29, 2020
6d2ec0f
Add files via upload
sahil132jindal Feb 29, 2020
4723f0f
Add files via upload
sahil132jindal Feb 29, 2020
c8c9415
ssl ref
sahil132jindal Feb 29, 2020
12e7af9
ref
sahil132jindal Feb 29, 2020
3428982
Add files via upload
sahil132jindal Feb 29, 2020
bd30ada
Add files via upload
sahil132jindal Feb 29, 2020
2432178
Create package.xml
sahil132jindal Feb 29, 2020
5e5056b
Add files via upload
sahil132jindal Feb 29, 2020
f6cfd8f
Create util.h
sahil132jindal Feb 29, 2020
73c7cc3
Add files via upload
sahil132jindal Feb 29, 2020
9748472
Create ssl_referee.proto
sahil132jindal Feb 29, 2020
5f18041
Add files via upload
sahil132jindal Feb 29, 2020
5e6c61b
Create netraw.cpp
sahil132jindal Feb 29, 2020
e2cd1e0
Add files via upload
sahil132jindal Feb 29, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
371 changes: 371 additions & 0 deletions callRef.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,371 @@
import rospy,sys
from utils.geometry import Vector2D
from utils.functions import *
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 krssg_ssl_msgs.msg import Ref
from role import _GoToPoint_, GoToPoint
from multiprocessing import Process
from kubs import kubs
from krssg_ssl_msgs.srv import *
from math import atan2,pi
from utils.functions import *
import multiprocessing
import threading
import time
#pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
command = 5
flag = 0
pcommand = 12
rospy.wait_for_service('ref_comm',)
getrState = rospy.ServiceProxy('ref_comm',ref_comm)

def kickoff_Blue(id_1,state,pub, target):
kub = kubs.kubs(id_1,state,pub)
kub.update_state(state)
print("HEREEE")
# time.sleep(2)
gotopoint_fsm = GoToPoint.GoToPoint()
gotopoint_fsm.add_kub(kub)
gotopoint_fsm.add_point(target, 0)
gotopoint_fsm.spin()
kub.reset()
kub.execute()
# _GoToPoint_.init(kub,target,0)
# start_time = rospy.Time.now()
# start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
# generatingfunction = _GoToPoint_.execute(start_time,DISTANCE_THRESH/3,True)
# #self.behavior_failed = False
# for gf in generatingfunction:
# kub,target_point = gf
# # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
# if not vicinity_points(target,target_point,BOT_RADIUS*2.0):
# # self.behavior_failed = True
# break
# if vicinity_points(kub.get_pos(), target,BOT_RADIUS*1.5):
# print("bot", id_1)
# print("reached")
# kub.reset()
# kub.execute()
# time.sleep(2)
# break

def main1(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 1",command)
if command == 5:
target = Vector2D(-1500,0)
kickoff_Blue(1,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()

def main2(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 2 ",command)
if command == 5:
target = Vector2D(-1500,-1120)
kickoff_Blue(2,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()
def main3(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 3 ",command)
if command == 5:
target = Vector2D(-550,0)
kickoff_Blue(3,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()
def main4(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 4 ",command)
if command == 5:
target = Vector2D(-2500,0)
kickoff_Blue(4,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()
def main5(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 5 ",command)
if command == 5:
target = Vector2D(-3600,0)
kickoff_Blue(5,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()
def main6(process_id,pub):
global command
# pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
try :
rospy.init_node('node' + str(process_id),anonymous=False)
except:
pass

while True:
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("chutiya")
if state:
#print('lasknfcjscnajnstate',state.stateB.homePos)
#p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, ))
kub = kubs.kubs(process_id,state,pub)
kub.reset()
kub.execute()
print("process 6 ",command)
if command == 5:
target = Vector2D(-1500,1120)
kickoff_Blue(0,state.stateB,pub,target)
kub.reset()
kub.execute()
print(process_id)
print("COMPLETED")
time.sleep(1)
break
else:
break
# break
#p2.start()
#p1.join()
#p2.join()
# print('chal ja')
# break
# rospy.spin()
# def main(comm,pub):

# global command
# command = comm
# # print(command)
# # time.sleep(2)
# # print(pub.get_num_connections())
# # print("##########################################################################")
# # time.sleep(3)
# p1 = multiprocessing.Process(target=main1, args=(1,pub,))
# p2 = multiprocessing.Process(target=main2, args=(2,pub,))
# p3 = multiprocessing.Process(target=main3, args=(3,pub,))
# p4 = multiprocessing.Process(target=main4, args=(4,pub,))
# p5 = multiprocessing.Process(target=main4, args=(5,pub,))
# p6 = multiprocessing.Process(target=main6, args=(0,pub,))
# p1.start()
# p2.start()
# p3.start()
# p4.start()
# p5.start()
# p6.start()
# p1.join()
# p2.join()
# p3.join()
# p4.join()
# p5.join()
# p6.join()
# print("COMMAND COMPLETED")
# time.sleep(2)
pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
while True:
ref_state = None
ref_state = getrState(ref_state).stateB
print ref_state.command
# time.sleep(2)
if ref_state.command == 2 or ref_state.command == 3 or pcommand==ref_state.command:
continue
command = ref_state.command
print("command: ",command)
print("pcommand: ", pcommand)
pcommand = command
# time.sleep(2)
p1 = multiprocessing.Process(target=main1, args=(1,pub))
p2 = multiprocessing.Process(target=main2, args=(2,pub))
p3 = multiprocessing.Process(target=main3, args=(3,pub))
p4 = multiprocessing.Process(target=main4, args=(4,pub))
p5 = multiprocessing.Process(target=main5, args=(5,pub))
p6 = multiprocessing.Process(target=main6, args=(0,pub))
p1.start()
p2.start()
p3.start()
p4.start()
p5.start()
p6.start()
p1.join()
p2.join()
p3.join()
p4.join()
p5.join()
p6.join()
print("COMMAND COMPLETED")
#main1()

# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000)
10 changes: 10 additions & 0 deletions kgpkubs_launch/launch/ref_data.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@

<launch>
<node pkg="referee_comm" type="ref_data" name="ref_data" output="screen">


<!-- <remap from="~image" to="/head/kinect2/qhd/image_color"/> -->
<!-- <remap from="~camera_info" to="/head/kinect2/qhd/camera_info"/> -->

</node>
</launch>
1 change: 1 addition & 0 deletions kgpkubs_launch/scripts/ssl.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ pr_launcher "vision" "vision.launch"
pr_launcher "belief_state" "belief_state.launch"
pr_launcher "grsim_comm" "grsim_comm.launch"
pr_launcher "path_planner_ompl" "ompl_planner.launch"
pr_launcher "ref_data" "ref_data.launch"

screen -S "GUI" -dm python run_gui.py
screen -S "bs_memcache" -dm python bs.py
Expand Down
Loading