working shammin

KRSSG · Nov 9, 2018 · 8d502c4 · 8d502c4
1 parent db9a357
commit 8d502c4
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 6 deletions.
diff --git a/kgpkubs_launch/scripts/ssl.sh b/kgpkubs_launch/scripts/ssl.sh
@@ -28,7 +28,7 @@ fi
 
 launcher    "core"         "roscore"
 pr_launcher "vision"    "vision.launch"
-# pr_launcher "belief_state"      "belief_state.launch"
+pr_launcher "belief_state"      "belief_state.launch"
 pr_launcher "grsim_comm"    "grsim_comm.launch"
 
 

diff --git a/kubs/kubs.py b/kubs/kubs.py
@@ -14,7 +14,7 @@ class kubs:
     ## @param      kubs_id  The kubs identifier
     ##
 
-    def __init__(self, kubs_id, pub):
+    def __init__(self, kubs_id, BState, pub):
         self.kubs_id = kubs_id
         self.vx = 0
         self.vy = 0

diff --git a/ompl_planner/CMakeLists.txt b/ompl_planner/CMakeLists.txt
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   krssg_ssl_msgs
 )
-find_package(OMPL REQUIRED)
+find_package(ompl)
 find_package(OpenCV REQUIRED)
 
 catkin_package()

diff --git a/test_GoToPoint.py b/test_GoToPoint.py
@@ -37,15 +37,19 @@ def GUI_Callback(data):
 	kub = kubs.kubs(BOT_ID, BState, pub)
 
 def BS_callback(data):
+	print('BS_callback running...')
 	global homePos, REPLANNED
 	global awayPos, start_time, BState, kub
 	BState = data
 	homePos = data.homePos
 	awayPos = data.awayPos
+	print("homePos = ",homePos)
+	print("awayPos = ",awayPos)
 	t = rospy.Time.now()
 	t = t.secs + 1.0*t.nsecs/pow(10,9)
 	print(" t - start = ",t-start_time)
-	[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, BOT_ID, data.ballPos, homePos, awayPos)	#vx, vy, vw, replanned
+	[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, BOT_ID, data.ballPos, homePos, awayPos)
+		#vx, vy, vw, replanned
 	print("-------------------REPLANNED = ",REPLANNED)
 	if(REPLANNED):
 		reset()
@@ -65,7 +69,8 @@ def BS_callback(data):
 	rospy.init_node('node_new',anonymous=False)
 	start_time = rospy.Time.now()
 	start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
-	pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)	
+	pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
+	print('testing...')
 	rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000)
 	rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000)
 	rospy.spin()

diff --git a/velocity/run.py b/velocity/run.py
@@ -31,7 +31,7 @@ def distance_(a, b):
     return sqrt(dx*dx+dy*dy)
 
 def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
-    # print("Get_Vel:     t - start = ",t-start)       
+    print("Get_Vel:     t - start = ",t-start)       
     global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target
     REPLAN = 0
     homePos = homePos_