def ballControllerCallback(data):
global gpsControllerStatus
global ballControllerStatus
global programStatus
global pub_cmd
#If ball tracker is disabled, ignore message
if gpsControllerStatus == 0:
return
#Ball tracker wants to take control
if data.data == 0:
#Stop Rover
gpsCmd = cmd()
gpsCmd.Velocity = 0.0
gpsCmd.Turn = 0.0
pub_cmd.publish(gpsCmd)
#Kill program
programStatus = 0
print("Ball tracker took control")
elif data.data == 1:
programStatus = 1
print("Control Returned")
elif data.data == 2:
print('SUCCESS: Mission accomplished.')
programStatus = 0
exit(0)
ballControllerStatus = data
def __init__(self):
rospy.on_shutdown(self.cleanup)
############################### SUBSCRIBERS #####################################
# Subscriber para leer el valor del gps del UMA, gps del goal, valor del IMU
rospy.Subscriber("fix", NavSatFix, self.gps_cb)
rospy.Subscriber("fixgoal", NavSatFix, self.gps_goal_cb)
rospy.Subscriber('imu', Imu, self.get_rotation)
#### PUBLISHERS ###
#Publica velocidad a cmd el cual recibe operator para la evasion de obstaculos
self.cmd_pub=rospy.Publisher("cmd", cmd, queue_size = 1)
### CONSTANTS ###
self.robot_vel=cmd()
self.robot_vel.Velocity=0
self.robot_vel.Turn=0
r = rospy.Rate(10) #10 Hz
self.dif_latitude=0.0
self.dif_longitude=0.0
self.latitude_robot=0.0
self.longitude_robot=0.0
self.latitude_goal=0.0
self.longitude_goal=0.0
#********** INIT NODE **********###
while not rospy.is_shutdown():
self.cmd_pub.publish(self.robot_vel) #Publicando datos a cmd_pub a una frecuencia de 10Hz
r.sleep()
def teleop():
global pub
global rate
global speed
speed = 0.4
pub = rospy.Publisher('cmd', cmd, queue_size=10)
rospy.init_node('teleop_nav2d', anonymous=True)
msg = cmd()
rate = rospy.Rate(100) # 10hz
key = ""
print("Use W,A,S,D and Space to move the robot")
while not rospy.is_shutdown():
key = "x"
key = getKey()
key = key.lower()
if (key == 'w'):
go_forward(msg)
elif (key == 's'):
go_backward(msg)
elif (key == 'a'):
turn_left(msg)
elif (key == 'd'):
turn_right(msg)
elif (key == ' '):
stop(msg)
elif (key == '\x03'):
break
rate.sleep()
def __init__(self):
rospy.on_shutdown(self.cleanup)
############################### SUBSCRIBERS #####################################
# Subscriber para leer el valor del gps del UMA, gps del goal, valor del IMU
rospy.Subscriber("fix", NavSatFix, self.gps_cb)
rospy.Subscriber('imu', Imu, self.get_rotation)
#### PUBLISHERS ###
#Publica velocidad a cmd el cual recibe operator para la evasion de obstaculos
self.cmd_pub = rospy.Publisher("cmd", cmd, queue_size=1)
### CONSTANTS ###
self.robot_vel = cmd()
self.robot_vel.Velocity = 0
self.robot_vel.Turn = 0
r = rospy.Rate(10) #10 Hz
self.dif_latitude = 0.0
self.dif_longitude = 0.0
self.latitude_robot = 0.0
self.longitude_robot = 0.0
latitude_goal = 0.0
longitude_goal = 0.0
self.option = 1
self.n_goal = 0
self.dist_to_goal = 0
#self.option = int(input("UMA empieza recorrido desde el pasillo: "))
#if self.option is 1:
#print('UMA ira al pasillo 1...')
#elif self.option is 2:
#print('UMA ira al pasillo 2...')
#elif self.option is 3:
#print('UMA ira al pasillo 3...')
#else:
#print('Esa no es una opcion, por favor elige una que este en las opciones')
#print(opciones)
#********** INIT NODE **********###
while not rospy.is_shutdown():
self.cmd_pub.publish(
self.robot_vel
) #Publicando datos a cmd_pub a una frecuencia de 10Hz
r.sleep()
def __init__(self):
rospy.on_shutdown(self.cleanup)
############################### SUBSCRIBERS #####################################
rospy.Subscriber("fix", NavSatFix, self.gps_cb)
rospy.Subscriber("fixgoal", NavSatFix, self.gps_goal_cb)
rospy.Subscriber('imu', Imu, self.get_rotation)
#### PUBLISHERS ###
self.cmd_pub=rospy.Publisher("cmd", cmd, queue_size = 1)
### CONSTANTS ###
self.robot_vel=cmd()
self.robot_vel.Velocity = 0
self.robot_vel.Turn = 0
r = rospy.Rate(10) #10 Hz
self.dif_latitude=0.0
self.dif_longitude=0.0
self.latitude_robot=0.0
self.longitude_robot=0.0
self.latitude_goal=0.0
self.longitude_goal=0.0
#********** INIT NODE **********###
while not rospy.is_shutdown():
self.cmd_pub.publish(self.robot_vel)
r.sleep()
def MoveBaseGPSGoal():
#GPS global variables
global gpsData
global target
global finalTarget
global gpsDataPoint
global gpsError
#Sys argv flags
global distanceFromTargetToTurnOnBallTracker
global gpsTopic
#Global Publisher
global pub_cmd
#Goal flag
global goal
#Global status variables
global gpsControllerStatus
global ballControllerStatus
global programStatus
#cmd publisher
pub_cmd = rospy.Publisher('cmd', cmd, queue_size=10)
#ball tracker publisher
pub_ball = rospy.Publisher('gps_controller', UInt8, queue_size=10)
#success publisher
success_ball = rospy.Publisher('servo_move', Char, queue_size=10)
#Node init
rospy.init_node('MoveBaseGPSGoal', anonymous=True)
#Suscriber with next target in list to be acheived
rospy.Subscriber('gpstarget', GPSFix, targetCallback)
#Subscriber with GPS current position (based on GpsTopic select between gpssim and GPS)
if gpsTopic == "sim":
rospy.Subscriber('gpssim', GPSFix, GPSCallback)
elif gpsTopic == "gps":
rospy.Subscriber('GPS', GPSFix, GPSCallback)
else:
print("Invalid flag name for gps topic suscriber")
exit(0)
#Topic that indicates the status regarding to the final target
rospy.Subscriber('onfinaltarget', String, msgCb)
#Suscriber with final target (leg's gps coordinate) to be achieved
rospy.Subscriber('finaltarget', GPSFix, finalTargetCallback)
#Suscriber to communicate with ball tracker algorithm
rospy.Subscriber('ball_controller', UInt8, ballControllerCallback)
gpsCmd = cmd()
r = rospy.Rate(5) #5 Hz (rate of /gpstarget)
while not rospy.is_shutdown():
if goal:
print('Final GPS coordinate reached!!!')
print('Final GPS coordinate reached!!!')
print('Final GPS coordinate reached!!!')
print('Final GPS coordinate reached!!!')
print('Final GPS coordinate reached!!!')
gpsCmd.Velocity = 0.0
gpsCmd.Turn = 0.0
pub_cmd.publish(gpsCmd)
programStatus = 0
success_ball.publish(char(0x7F))
exit(0)
if programStatus == 1 and firstGPSCb:
#Ball tracker enabler
gpsToFinalTarget = GPSVector(gpsDataPoint, finalTarget)
print("Magnitude -------> %f <= %f" %
(gpsToFinalTarget.magnitude,
distanceFromTargetToTurnOnBallTracker))
print("status --> %d" % (gpsControllerStatus.data))
if gpsToFinalTarget.magnitude <= distanceFromTargetToTurnOnBallTracker and gpsControllerStatus.data == 0:
gpsControllerStatus = UInt8(1)
pub_ball.publish(gpsControllerStatus)
elif gpsToFinalTarget.magnitude >= distanceFromTargetToTurnOnBallTracker + gpsError and gpsControllerStatus.data == 1:
gpsControllerStatus = UInt8(0)
pub_ball.publish(gpsControllerStatus)
#Calculate lineal velocity and turn
gpsToTarget = GPSVector(gpsDataPoint, target)
#lineal velocity
gpsCmd.Velocity = max(min(Kv * gpsToTarget.magnitude, maxVel),
minVel)
#angular velocity
ang_diff = gpsToTarget.orientation - gpsData.track
print(gpsDataPoint)
print(gpsToTarget)
#print(gpsToTarget.orientation)
#print("ang_diff: %.6f" % (ang_diff))
if ang_diff < 0.0:
ccw = abs(ang_diff)
cw = ang_diff + 360.0
else:
cw = ang_diff
ccw = 360.0 - ang_diff
#Choose between clockwise and couter-clockwise turn
if ccw < cw: #counter clock wise turn
turn = -min(maxTurn, radians(ccw))
else: #clock wise turn
turn = min(maxTurn, radians(cw))
#assign turn to /cmd publish message
gpsCmd.Turn = turn
#rospy.loginfo(gpsCmd)
pub_cmd.publish(gpsCmd)
r.sleep()
publisher_cmd = rospy.Publisher('cmd', cmd, queue_size=10)
rospy.init_node('balls')
#Get WEBCAM image
camera = cv2.VideoCapture(0)
#Dara to calculate distance
F = 697.67
W = 6.45
x = 0
y = 0
profundity = 0
#Variables for ROS messages
follow = cmd()
servos = Char()
area = 0
area_before = 0
x_before = 0
y_before = 0
start = 0
circularity_thresh = 0.6
#Thresholds to determine constant presence of suspect object
samples = 150 # samples for the filter
thresh1 = samples * 0.20
thresh2 = samples * 0.40
thresh3 = samples * 0.60
thresh4 = samples * 0.85
#Project ALVIN (Autonomous Logic Virtual Intelligence n' Navigation)
import rospy
import math
from nav2d_operator.msg import cmd
from std_msgs.msg import Float32
#constant limit sets
#maxVel = 1.0
#maxTurn = 0.6
#base controller constants
width_robot = 1.5
#global variables
cmdVel = cmd()
#global flag
dataChanged = False
def cmdCb(data):
global cmdVel
global dataChanged
cmdVel.Velocity = data.Velocity
cmdVel.Turn = data.Turn
dataChanged = True
def BaseController():
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('teleop_twist_keyboard')
""" ROS Parameters
"""
vel_pub = rospy.Publisher('key_vel', Twist, queue_size=10)
nav2d_cmd_pub = rospy.Publisher('cmd', cmd, queue_size=10)
x = 0
th = 0
status = 0
try:
print msg
print vels(speed, turn)
key_vel = Twist()
nav2d_cmd = cmd()
while (1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
th = moveBindings[key][1]
key_vel.linear.x = 3 * x * speed
key_vel.linear.y = 0
key_vel.linear.z = 0
key_vel.angular.x = 0
key_vel.angular.y = 0
key_vel.angular.z = th * turn
nav2d_cmd.Velocity = key_vel.linear.x
nav2d_cmd.Turn = key_vel.angular.z
vel_pub.publish(key_vel)
nav2d_cmd_pub.publish(nav2d_cmd)
def callback(data):
global pub_msg
global pub
pub_msg = cmd(data.axes[1] * maxVel, data.axes[3], 0)
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
from nav2d_operator.msg import cmd
pub_msg = cmd(0.0, 0.0, 0)
maxVel = 0.52
def callback(data):
global pub_msg
global pub
pub_msg = cmd(data.axes[1] * maxVel, data.axes[3], 0)
def node():
global pub
rospy.init_node('joyNav2d', anonymous=True)
rospy.Subscriber("joy", Joy, callback)
pub = rospy.Publisher('cmd', cmd, queue_size=10)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub.publish(pub_msg)
rate.sleep()
if __name__ == '__main__':
try:
