def goalsender(): argv = rospy.myargv(argv=sys.argv) while (Ballbot_x == -10) and not (rospy.is_shutdown()): rospy.sleep(1) if (len(argv) == 3): d_goal = float(argv[1]) * 100 th_goal = float(argv[2]) th_goal = Ballbot_theta - math.radians( th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] th_goal = th_goal % (2 * math.pi) x2 = Ballbot_x * 100.0 + d_goal * math.cos(th_goal) y2 = Ballbot_y * 100.0 + d_goal * math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2, y2, th2) pub_goal.publish(goal_msg) print "pose", (Ballbot_x, Ballbot_y, Ballbot_theta) print "sent goal", (x2, y2, th2) else: rospy.loginfo( "Usage: rosrun lattice_planner goalsender.py d_dist[m] d_theta[deg]" ) rospy.signal_shutdown("Exiting")
def goalsender(): argv = rospy.myargv(argv=sys.argv) while (Ballbot_x == -10) and not (rospy.is_shutdown()): rospy.sleep(1) if(len(argv) == 3): d_goal = float(argv[1])*100 th_goal = float(argv[2]) th_goal = Ballbot_theta - math.radians(th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] th_goal = th_goal%(2*math.pi) x2 = Ballbot_x*100.0 + d_goal*math.cos(th_goal) y2 = Ballbot_y*100.0 + d_goal*math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2,y2,th2) pub_goal.publish(goal_msg) print "pose",(Ballbot_x,Ballbot_y,Ballbot_theta) print "sent goal",(x2,y2,th2) else: rospy.loginfo("Usage: rosrun lattice_planner goalsender.py d_dist[m] d_theta[deg]") rospy.signal_shutdown("Exiting")
def state_IDLE(): global Goal_current,Ball_x,Ball_y,Ball_theta if (Ball_x >= 0) and (Ball_y >= 0): print "new ball seen. Hit a key to pickup",Ball_x,Ball_y,Ball_theta raw_input() goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(Ball_x*100.0,Ball_y*100.0,Ball_theta) pub_goal.publish(goal_msg) Goal_current = goal_msg return "newballseen" else: return "noball"
def state_BALLPICKUP(): global Goal_current,Ballbot_status while((Ballbot_status != "goalreached") and not(rospy.is_shutdown())): if(Ball_x >= 0) and (Ball_y >= 0): # if ball is visible dist = math.sqrt(math.pow((Goal_current.pose.x/100.0 - Ball_x),2) + math.pow((Goal_current.pose.y/100.0 - Ball_y),2)) goal_msg = Goal() goal_msg.pose = Pose(Ball_x*100.0,Ball_y*100.0,Ball_theta) if (dist >= 0.10) and (dist <= 0.5): goal_msg.goaltype = String("updategoal") pub_goal.publish(goal_msg) Goal_current = goal_msg #print (Ball_x,Ball_y),goal_msg.goaltype elif(dist >= 0.5): goal_msg.goaltype = String("newball") pub_goal.publish(goal_msg) Goal_current = goal_msg #print (Ball_x,Ball_y),goal_msg.goaltype rospy.sleep(1.0) # update goal position every 1 s for now Ballbot_status = None #-------------------------------------- # activate ball pickup for 10 seconds ballpickup_msg = BallPickup() ballpickup_msg.direction = -1 pub_ballpickup.publish(ballpickup_msg) ballpickup_msg = BallPickup() ballpickup_msg.direction = 0 pub_ballpickup.publish(ballpickup_msg) rospy.sleep(10) # deactivate ball pickup ballpick_msg = BallPickup() ballpickup_msg.direction = 1 pub_ballpickup.publish(ballpickup_msg) #---------------------------------------- goal_msg = Goal() goal_msg.pose = Pose(10.0 * 100.0,10.0 * 100.0,0.0) goal_msg.goaltype = String("gotopose") print "Go to (10.0 10.0 0); hit any key to drive" raw_input() pub_goal.publish(goal_msg) Goal_current = goal_msg return "pickedup"
def startPlanner(d_goal=0.0, th_goal=0.0, x_goal=0.0, y_goal=0.0, th_goal_abs=0.0, goaltype="None"): """ Send a goal message to topic 'goal' """ """ Draw court """ global Goal_x, Goal_y graphics.canvas.delete(ALL) graphics.draw_court() if (goaltype == "newball"): th_goal = Ballbot_theta - math.radians( th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] x2 = Ballbot_x * 100.0 + d_goal * math.cos(th_goal) y2 = Ballbot_y * 100.0 + d_goal * math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2, y2, th2) graphics.draw_point(x2, y2, th2, color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 elif (goaltype == "gotopose"): x2 = x_goal y2 = y_goal th2 = math.radians(th_goal_abs) # publish goal goal_msg = Goal() goal_msg.goaltype = String("gotopose") goal_msg.pose = Pose(x2, y2, th2) graphics.draw_point(x2, y2, th2, color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 """
def startPlanner(d_goal=0.0,th_goal=0.0,x_goal=0.0,y_goal=0.0,th_goal_abs=0.0,goaltype = "None"): """ Send a goal message to topic 'goal' """ """ Draw court """ global Goal_x,Goal_y graphics.canvas.delete(ALL) graphics.draw_court() if(goaltype == "newball"): th_goal = Ballbot_theta - math.radians(th_goal) # angle to goal in global coord # = angle of car in global coord - angle to goal from car (which is in [-90deg,90deg] x2 = Ballbot_x*100.0 + d_goal*math.cos(th_goal) y2 = Ballbot_y*100.0 + d_goal*math.sin(th_goal) th2 = 0 # doesn't matter for goal test # publish goal goal_msg = Goal() goal_msg.goaltype = String("newball") goal_msg.pose = Pose(x2,y2,th2) graphics.draw_point(x2,y2,th2,color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 elif(goaltype == "gotopose"): x2 = x_goal y2 = y_goal th2 = math.radians(th_goal_abs) # publish goal goal_msg = Goal() goal_msg.goaltype = String("gotopose") goal_msg.pose = Pose(x2,y2,th2) graphics.draw_point(x2,y2,th2,color='red') graphics.canvas.update_idletasks() pub_goal.publish(goal_msg) print "sent goal" Goal_x = x2 Goal_y = y2 """