Skip to content

Commit

Permalink
Merge branch 'fsm' into vision_int
Browse files Browse the repository at this point in the history
  • Loading branch information
@Snehal-Reddy
Snehal-Reddy authored Apr 6, 2019
2 parents d7fb816 + ca57424 commit 409e4a9
Show file tree
Hide file tree
Showing 34 changed files with 264 additions and 577 deletions.
20 changes: 0 additions & 20 deletions Digraph/GoToBall
View file

This file was deleted.

Binary file modified Digraph/GoToBall.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
4 changes: 2 additions & 2 deletions belief_state/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,8 +163,8 @@ include_directories(
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

# add_executable(beliefstate_node src/node.cpp ${src})
# target_link_libraries(beliefstate_node ${catkin_LIBRARIES})
add_executable(beliefstate_node src/node.cpp ${src})
target_link_libraries(beliefstate_node ${catkin_LIBRARIES})

add_executable(beliefstate_class src/node_class.cpp ${src})
target_link_libraries(beliefstate_class ${catkin_LIBRARIES})
38 changes: 15 additions & 23 deletions bot_comm/src/bot_wifi.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,32 +40,31 @@ def vel_convert(vel_3_wheel):
vy = vel_3_wheel[1]
vw = -vel_3_wheel[2]

#print("vx",vx,"vy",1,"vw",vw)
print("vx",vx,"vy",1,"vw",vw)
for i in range(4):
v_4_wheel[i] = ((bot_radius*vw) - (vx*math.sin(theta[i]*math.pi/180.0)) + (vy*math.cos(theta[i]*math.pi/180.0)))/(bot_wheel_radius * math.pi)
#print("Before Wheel: ", v_4_wheel)
for i in range(4):
if v_4_wheel[i] > 0 :
v_4_wheel[i] = int(126 + ((v_4_wheel[i]-max_vel_wheel)*126.0) / max_vel_wheel)
v_4_wheel[i] = int(126 + ((v_4_wheel[i]-max_vel_wheel)*126) / max_vel_wheel)
else :
v_4_wheel[i] = int(256 - (v_4_wheel[i]*(129-256)) / max_vel_wheel)

#print("Wheel: ", v_4_wheel)
return v_4_wheel

def gr_Commands_CB(msg):
print("...")
global buf
#print("Command received for bot_id : ",msg.robot_commands.id)
vel_xyw = [None]*3

vel_xyw[0] = int(msg.robot_commands.velnormal * FACTOR_T)
vel_xyw[1] = -1*int(msg.robot_commands.veltangent * FACTOR_N)
vel_xyw[2] =int(msg.robot_commands.velangular * FACTOR_W)
# vel_xyw[2] = 0
#print("Velocities before convert: ",vel_xyw)
# vel_xyw = [0, 300 ,0]
v_4_wheel = vel_convert(vel_xyw)
#print("Velocities ",vel_xyw)
print("Velocities ",vel_xyw)
global frame,ti

# print(frame, ti," ", time.time())
# print("Frame Rate ",frame*1.0/(time.time()-ti))
frame += 1
start = 1 + msg.robot_commands.id*5
for i in range(4):
Expand All @@ -78,18 +77,11 @@ def gr_Commands_CB(msg):
buf[8] = buf[7]
buf[7] = buf[6]
buf[6] = temp
#print("Data: ", buf)
print(buf)
###########################################################################
#For only dribller --> 254
#For only kicker --> 1
#For both dribbler and kicker --> 253
###########################################################################

print("Data: ", buf)
if msg.robot_commands.spinner and msg.robot_commands.kickspeedx :
buf[start+4] = 253
buf[start+4] = 3
elif msg.robot_commands.spinner :
buf[start+4] = 254
buf[start+4] = 2
elif msg.robot_commands.kickspeedx :
buf[start+4] = 1
else:
Expand All @@ -107,13 +99,13 @@ def gr_Commands_CB(msg):
#print("f#ck off ",i)
buff += chr(int(buf[i])%256)

#print("Sending signal to ESP")
print(buf)
data = s.recvfrom(BUFFER_SIZE)
#print("Received data from ESP")
if data:
#print('Client to server: ', data)
print('Client to server: ', data)
s.sendto(buff, data[1])
#print("Message Sent")
print("sent msg : ", buff)
print("done")



Expand Down
17 changes: 4 additions & 13 deletions bot_comm/src/test_wifi.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,26 +106,17 @@ def gr_Commands_CB(msg):
for i in xrange(1,32):
buf[i] = 0

#Only dribbler , buf[5] = 254
#For only kicker buf[5] = 1
#for running both dribble and kicker buf[5] = 253
buf[7], buf[8], buf[9], buf[6] = vel_convert([0, 0,0*90])

#buf[9], buf[6], buf[7], buf[8], buf[10] = [0,0,0,0,0]
buf[10]= 1
#Only dribbler , buf[5] = 127. For only kicker buf[5] = 1 and for running both dribbler and kicker buf[5] = 128.
buf[7], buf[8], buf[9], buf[6] = vel_convert([0, 30,0])
buf[9], buf[6], buf[7], buf[8] = [10,10,10,10]
#buf[6], buf[7], buf[8], buf[9] = 247, 247, 0, 0
for i in xrange(5,9):
buf[i] = int(buf[i])
for i in xrange(5,9):
buf[i] = int(buf[i])

for i in xrange(32):
buf[i] %= 256
buf[i] %= 255


for i in xrange(32):
buff += chr(int(buf[i]))
buff += chr(int(buf[i])%256)

print(buf)
print("waiting to receivg")
Expand Down
2 changes: 1 addition & 1 deletion grSim/include/net/robocup_ssl_client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class QNetworkInterface;
class RoboCupSSLClient
{
public:
RoboCupSSLClient(const quint16 & port=10002,
RoboCupSSLClient(const quint16 & port=10020,
const string & net_address="224.5.23.2",
const string & net_interface="");

Expand Down
2 changes: 1 addition & 1 deletion kgpkubs_launch/launch/ompl_planner.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,4 @@
<!-- <remap from="~camera_info" to="/head/kinect2/qhd/camera_info"/> -->

</node>
</launch>
</launch>
2 changes: 1 addition & 1 deletion kgpkubs_launch/launch/vision.launch
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<node pkg="vision_comm" type="vision_node" name="vision_node" args="0" output="screen">
<node pkg="vision_comm" type="vision_node" name="vision_node" args="1" output="screen">
<!-- args= 0 = ssl-vision camera input -->
<!-- args= 1 = grSim input -->

Expand Down
12 changes: 6 additions & 6 deletions krssg_ssl_msgs/msg/BeliefState.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@ geometry_msgs/Pose2D[] homeVel
bool ballDetected
bool[] homeDetected
bool[] awayDetected
# uint8 our_bot_closest_to_ball
# uint8 opp_bot_closest_to_ball
uint8 our_bot_closest_to_ball
uint8 opp_bot_closest_to_ball
uint8 our_goalie #returns 10 for no goalie
uint8 opp_goalie #returns 10 for no goalie
# uint8 opp_bot_marking_our_attacker
# bool ball_at_corners
# bool ball_in_our_half
# bool ball_in_our_possession
uint8 opp_bot_marking_our_attacker
bool ball_at_corners
bool ball_in_our_half
bool ball_in_our_possession
SSL_GeometryData geometryData

# add other stuff eg. referee signals, bot/ball velocities and acc, score count, predicates, etc.
Expand Down
10 changes: 4 additions & 6 deletions role/GoToPoint.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,10 +44,10 @@ def __init__(self,continuous=False):
GoToPoint.State.setup,lambda: True,'immediately')

self.add_transition(GoToPoint.State.setup,
GoToPoint.State.drive,lambda: self.target_present(),'setup')
GoToPoint.State.drive,lambda: self.target_present,'setup')

#self.add_transition(GoToPoint.State.drive,
# GoToPoint.State.drive,lambda: not self.at_new_point(),'restart')
self.add_transition(GoToPoint.State.drive,
GoToPoint.State.drive,lambda: not self.at_new_point(),'restart')

self.add_transition(GoToPoint.State.drive,
behavior.Behavior.State.completed,lambda:self.at_new_point(),'complete')
Expand Down Expand Up @@ -96,10 +96,8 @@ def terminate(self):
def execute_drive(self):
print("Execute drive")
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
_GoToPoint_.init(self.kub,self.target_point,self.theta)
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
generatingfunction = _GoToPoint_.execute(start_time,self.DISTANCE_THRESH)
print("Datatype of gf:",type(generatingfunction))
for gf in generatingfunction:
self.kub,target_point = gf

Expand Down
33 changes: 6 additions & 27 deletions role/_turnAround_.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,24 +16,11 @@
from utils.geometry import Vector2D
from utils.config import *
from utils.functions import *
from krssg_ssl_msgs.srv import bsServer
import math
pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
id_ = 0

import memcache
shared = memcache.Client(BS_ADDRESS,debug=0)

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")


kub = None
start_time = None
FLAG_turn = False
Expand All @@ -42,7 +29,7 @@
# print("Importing _gotopoint_")
FIRST_CALL = True

prev_state = state.stateB
prev_state = shared.get('state')
def init(_kub,theta):
global kub,rotate,FLAG_turn,FIRST_CALL
kub = _kub
Expand All @@ -68,16 +55,8 @@ def execute(startTime):


while not FLAG_turn:
global pub,id_
state = None
#kub = kubs.kubs(id_,state,pub)
rospy.wait_for_service('bsServer',)
getState = rospy.ServiceProxy('bsServer',bsServer)
try:
state = getState(state)
except rospy.ServiceException, e:
print("chutiya")
kub.state = state.stateB

kub.state = shared.get('state')
if not(prev_state == kub.state):
prev_state = kub.state

Expand All @@ -89,10 +68,10 @@ def execute(startTime):
if not vw:
vw = 0

#print("rotate : ", rotate)
#print(" not rotate : ", kub.state.homePos[kub.kubs_id].theta)
print("rotate : ", rotate)
print(" not rotate : ", kub.state.homePos[kub.kubs_id].theta)
print("diff : ",abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate)))
if (abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate))<2*ROTATION_FACTOR):
if (abs(kub.state.homePos[kub.kubs_id].theta-rotate)<2*ROTATION_FACTOR):
kub.turn(0)
print("Angle completed")
FLAG_turn = True
Expand Down
Binary file modified ssl-vision/bin/client
100755 → 100644
View file
Binary file not shown.
Binary file modified ssl-vision/bin/graphicalClient
View file
Binary file not shown.
Binary file modified ssl-vision/bin/logClient
100755 → 100644
View file
Binary file not shown.
2 changes: 1 addition & 1 deletion ssl-vision/bin/robocup-ssl.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -412,7 +412,7 @@
</Var>
<Var name="Legacy Network Output" type="list">
<Var name="Multicast Address" type="string">
224.5.23.1
224.5.23.2
</Var>
<Var name="Legacy Double-Size Field Multicast Port" type="int" minval="1" maxval="65535">
10005
Expand Down
Binary file modified ssl-vision/bin/vision
View file
Binary file not shown.
Binary file modified ssl-vision/kgp.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
40 changes: 20 additions & 20 deletions ssl-vision/robocup-ssl-cam-0-lut-yuv.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,12 +334,12 @@ AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAICAgIAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAICAgIC
AgICAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAACAgICAgICAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAgICAgICAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAgICAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
Expand Down Expand Up @@ -485,14 +485,6 @@ AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAICAgIAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAICAgICAgIAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAgICAgICAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAgICAgIC
AgIAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAACAgICAgICAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAgICAgICAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAgICAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
Expand Down Expand Up @@ -558,12 +550,20 @@ AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAgIC
AgIAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAgICAgICAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAACAgICAgICAgIAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAgICAgICAgICAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAICAgICAgIAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA
Expand Down
2 changes: 1 addition & 1 deletion ssl-vision/robocup-ssl-teams.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2526,7 +2526,7 @@
true
</Var>
<Var name="Marker Image File" type="string">
/home/ssl/fsm/src/ssl-vision/kgp.png
/home/ssl/robocup/src/ssl-vision/kgp.png
</Var>
<Var name="Marker Image Rows" type="int" minval="" maxval="">
2
Expand Down
Loading

0 comments on commit 409e4a9

Please sign in to comment.