from numpy import * from Circumnavigation import circum from ParametersServer import staticParameters sp = staticParameters c = circum() sp.desired_radius = 1.0 #setar raio e angulo (graus) #linear = 1.95397537229 #angular = -52.9857734443 #c.beacon.setPolarAndCalcRetCoords([linear,angular]) #radius = c.getApproachRadius() #setar raio e angulo (graus) #linear = 1.95397537229 #angular = 52.9857734443 #c.beacon.setPolarAndCalcRetCoords([linear,angular]) #radius = c.getApproachRadius() #linear = 0.50990 #angular = 90 #c.beacon.setPolarAndCalcRetCoords([linear,angular]) #radius = c.getApproachRadius() #linear = 1.441 #angular = 90 #c.beacon.setPolarAndCalcRetCoords([linear,angular]) #radius = c.getApproachRadius()
def main(argv): #global robot, beacon, alien, twist quantRobots = int(sys.argv[1]) rospy.init_node('p3dx_mover') #define a frequencia de atualizacao rate = rospy.Rate(50) #original 10hz #referente ao Beacon p0_radius_to_target = rospy.Publisher("p0/radius_to_target", Float32, queue_size = 1) sp_desired_radius_to_target = rospy.Publisher("sp/desired_radius_to_target", Float32, queue_size = 1) p0_angle_to_target = rospy.Publisher("p0/angle_to_target", Float32, queue_size = 1) sp_desired_angle_to_target = rospy.Publisher("sp/desired_angle_to_target", Float32, queue_size = 1) p0_approach_radius_to_target = rospy.Publisher("p0/approach_radius_to_target", Float32, queue_size = 1) p0_performed_radius = rospy.Publisher("p0/performed_radius", Float32, queue_size = 1) #referente ao Robo (estaticos) sp_minimal_distance = rospy.Publisher("sp/minimal_distance", Float32, queue_size = 1) sp_desired_distance = rospy.Publisher("sp/desired_distance", Float32, queue_size = 1) #dinamicos p0_robot_distance = rospy.Publisher("p0/robot_distance", Float32, queue_size = 1) p0_robot_angle = rospy.Publisher("p0/robot_angle", Float32, queue_size = 1) #objetos detectados p0_hasBeacon = rospy.Publisher("p0/hasBeacon", Int8, queue_size = 1) p0_hasRobot = rospy.Publisher("p0/hasRobot", Int8, queue_size = 1) p0_hasAlien = rospy.Publisher("p0/hasAlien", Int8, queue_size = 1) #objects creation for numRobot in range(0, quantRobots): strNumRobot = 'p' + str(numRobot) velocity_publishers.append(rospy.Publisher(strNumRobot + "/cmd_vel", Twist, queue_size = 1)) #twist_subscribers.append(rospy.Subscriber(strNumRobot + "/cmd_vel", Twist, getTwist)) odom_twist_subscribers.append(rospy.Subscriber(strNumRobot + "/odom", Odometry, getRealTwist, callback_args=numRobot)) laser_subscribers.append(rospy.Subscriber(strNumRobot + "/p3dx/laser/scan", LaserScan, getLaser, callback_args=numRobot)) print("Running for robot p%2d. Please, wait ..." % (numRobot)) robots.append(LocalObject(ID_TYPES)) beacons.append(LocalObject(ID_TYPES)) aliens.append(LocalObject(ID_TYPES)) ctrlCircum.append(circum()) ctrlTwists.append(Twist()) realTwists.append(Twist()) #instancias de representacoes locais simplificadas de coordenadas/velocidades robots_coords.append(LinAng()) beacons_coords.append(LinAng()) aliens_coords.append(LinAng()) velocities.append(LinAng()) debug_msgs.append("") #publicando os dados estaticos - removidos de dentro do while sp_desired_radius_to_target.publish(sp.desired_radius) #2017 sp_desired_angle_to_target.publish(sp.desired_angle_to_beacon) sp_desired_angle_to_target.publish(sp.desired_angle_to_target) sp_minimal_distance.publish(sp.min_distance_to_robot) sp_desired_distance.publish(sp.desired_distance_to_robot) while not rospy.is_shutdown(): #prev = statusChange(prev,status) #status = getStatus() #twist.linear.x = MIN_LINEAR_VEL #print "---- While ---- " #print "----------------" for numRobot in range(0, quantRobots): #envia comando a cada robo antes do controle rate.sleep() #numRobotrint "numRobot", numRobot #print "-" * 40 #print "" #se houver beacon no cone do sensor, pega valores diferentes de 0.0 if beacons[numRobot].isBeacon(): #beacons_coords[numRobot].linear, beacons_coords[numRobot].angular = beacons[numRobot].center.getCoordPol() beacons_coords[numRobot].setAllCoords(beacons[numRobot].center.getAllCoords()) debug_msgs[numRobot] = "| Beacon |" #bea.prn() else: beacons_coords[numRobot].clear() debug_msgs[numRobot] = "| |" #se houver o robo no cone do sensor, pega valores diferentes de 0.0 if robots[numRobot].isRobot(): #robots_coords[numRobot].linear, robots_coords[numRobot].angular = robots[numRobot].center.getCoordPol() robots_coords[numRobot].setAllCoords(robots[numRobot].center.getAllCoords()) debug_msgs[numRobot] = debug_msgs[numRobot] + "| Robot |" #rob.prn() else: robots_coords[numRobot].clear() debug_msgs[numRobot] = debug_msgs[numRobot] + "| |" #se houver alien no cone do sensor, pega valores diferentes de 0.0 if aliens[numRobot].isAlien(): #aliens_coords[numRobot].setPolarCoords(aliens[numRobot].center.getCoordPol()) aliens_coords[numRobot].setAllCoords(aliens[numRobot].center.getAllCoords()) debug_msgs[numRobot] = debug_msgs[numRobot] + "| Alien |" #ali.prn() else: aliens_coords[numRobot].clear() debug_msgs[numRobot] = debug_msgs[numRobot] + "| |" #obtem a velocidade atual velocities[numRobot].setVelocities(realTwists[numRobot].linear.x, realTwists[numRobot].angular.z) #vel.prn() print debug_msgs[numRobot] #obtem o horario atual now = rospy.get_rostime() #processa a circunavegacao ctrlTwists[numRobot].linear.x, ctrlTwists[numRobot].angular.z = ctrlCircum[numRobot].getVelocities(numRobot, velocities[numRobot], beacons_coords[numRobot], robots_coords[numRobot], aliens_coords[numRobot], now).getVelocities() #twist.linear.x = 0.25 #envie as informacoes de P0 para plotagem if(numRobot == 0): p0_radius_to_target.publish(ctrlCircum[numRobot].obtainedRadiusToBeacon) p0_angle_to_target.publish(ctrlCircum[numRobot].obtainedAngleToBeacon) p0_robot_angle.publish(ctrlCircum[numRobot].obtainedAngleToRobot) p0_robot_distance.publish(ctrlCircum[numRobot].obtainedDistanceToRobot) p0_approach_radius_to_target.publish(ctrlCircum[numRobot].approachRadius) p0_hasRobot.publish(ctrlCircum[numRobot].hasRobot) p0_hasBeacon.publish(ctrlCircum[numRobot].hasBeacon) p0_hasAlien.publish(ctrlCircum[numRobot].hasAlien) pRadius = NaN if velocities[numRobot].angular != 0.0: pRadius = velocities[numRobot].linear/ velocities[numRobot].angular #print "Real Velocity : vL =", velocities[numRobot].linear, " vA =", velocities[numRobot].angular, "pRadius =", pRadius p0_performed_radius.publish(pRadius) #envie ao robo os valores das velocidades velocity_publishers[numRobot].publish(ctrlTwists[numRobot]) #for numRobot in range(0, quantRobots): #print "P", numRobot, "vl: ", twists[numRobot].linear.x, ", va: ", twists[numRobot].angular.z #estabelece uma pausa para manter a frequencia definida rate.sleep() print "ending control.py..." #twist.angular.z = 0.0 #twist.linear.x = 0.0 #p1_pub.publish(twist) exit()