multi bot path planner invocation

PlaySelector.py

@@ -1,3 +1,4 @@
+
 import sys, os
 import rospy
 from krssg_ssl_msgs.msg import BeliefState
@@ -24,6 +25,8 @@
 from plays import pCordinatedPass
 from skills import skills_union
 from utils import tactics_union
+from velocity.getVel import GetVelocity
+
 ref_play_id = 0
 cur_play = None
 start_time = 0
@@ -38,6 +41,7 @@
 isteamyellow = False
 ref_command = False
 
+gv = None 
 
 def select_play(state):
     global start_time,pub
@@ -143,33 +147,39 @@ def debug_subscriber(state):
     params.PositionP.x = state.ballPos.x
     params.PositionP.y = state.ballPos.y
     cur_tactic = TPosition.TPosition(attacker_id+1,state,params)
-    cur_tactic.execute(state,pub)
+    cur_tactic.execute(state,gv,pub)
 
 def debug_subscriber2(state):
     # print("New Call Back")
     global pub
     attacker_id = 0
     params = tactics_union.Param()
-    params.PositionP.x = 3000
-    params.PositionP.y = 0
+    params.PositionP.x = state.homePos[1].x -200
+    params.PositionP.y = state.homePos[1].y + 200
     cur_tactic = TPosition.TPosition(attacker_id,state,params)
-    cur_tactic.execute(state,pub)
+    cur_tactic.execute(state,gv,pub)
 
 
     # cur_tactic = TTestIt.TTestIt(attacker_id,state)
     # cur_tactic.execute(state,pub)
 def main():
-    global pub
+    global pub, gv
     print "Initializing the node "
     # rospy.init_node('play_py_node',anonymous=False)
     start_time = rospy.Time.now()
     start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
 
     for i in xrange(6):
         os.environ['bot'+str(i)]=str(start_time)
+        os.environ['fc'+str(i)]='1'
     for i in xrange(6):
         print os.environ.get('bot'+str(i))
 
+    gv = []
+    for i in xrange(6):
+        start_time = float(os.environ.get('bot'+str(i)))
+        gv.append(GetVelocity(start_time = start_time,kubs_id = i))
+
     pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
     # rospy.Subscriber("/ref_data", Referee, referee_callback, queue_size=1000)
     # rospy.Subscriber('/belief_state', BeliefState, bs_callback, queue_size=1000)

kgpkubs_launch/scripts/ssl.sh

@@ -2,7 +2,8 @@
 # Start SSL functionality in screen sessions
 killall -15 screen
 #killall screen
-
+# /home/ssl/robocup_stp_ws/src/vision_comm/src/vision_node.cpp
+# change port from 9020 to 10020 in ^^
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 function launcher {

ompl_planner/src/listener.cpp

@@ -160,12 +160,12 @@ int main(int argc, char **argv)
   ros::NodeHandle n;
   ros::Subscriber sub = n.subscribe("/belief_state", 1000, Callback);
   ros::Subscriber sub1 = n.subscribe("/gui_params", 1000, Callback_gui);
-  ros::ServiceServer service0 = n.advertiseService("planner0", path);
-  ros::ServiceServer service1 = n.advertiseService("planner1", path);
-  ros::ServiceServer service2 = n.advertiseService("planner2", path);
-  ros::ServiceServer service3 = n.advertiseService("planner3", path);
-  ros::ServiceServer service4 = n.advertiseService("planner4", path);
-  ros::ServiceServer service5 = n.advertiseService("planner5", path);
+  ros::ServiceServer service0 = n.advertiseService("planner", path);
+  // ros::ServiceServer service1 = n.advertiseService("planner1", path);
+  // ros::ServiceServer service2 = n.advertiseService("planner2", path);
+  // ros::ServiceServer service3 = n.advertiseService("planner3", path);
+  // ros::ServiceServer service4 = n.advertiseService("planner4", path);
+  // ros::ServiceServer service5 = n.advertiseService("planner5", path);
   pub = n.advertise<krssg_ssl_msgs::planner_path>("/path_planner_ompl", 1000);
   ros::spin();
   return 0;