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