def listener():
global state
arm.cinematicaInversa(state)
rospy.Subscriber('/pra_vale/arm_tilt', Float32, arm_tilt)
rospy.Subscriber("/pra_vale/estados", Int32, state_callback)
rospy.Subscriber("/pra_vale/imu", Float32MultiArray, arm_imu)
rospy.Subscriber('/pra_vale/arm_move', Int32MultiArray, arm_move)
rospy.Subscriber("/ur5/forceTorqueSensorOutput", TwistStamped,
torque_callback)
rospy.Subscriber("/pra_vale/hokuyo_distance", Int32MultiArray,
hokuyo_distance_callback)
rospy.Subscriber("/ur5/jointsPositionCurrentState", ManipulatorJoints,
arm_current_position)
rospy.spin()
def arm_imu(data):
global state
global arm_publisher
if (state & (1 << defs.ARM_CHANGING_POSE | 1 << defs.CLIMB_STAIR)):
return
temp_y = 0.0
angles = data.data
if (state & (1 << defs.ROBOT_CLOCKWISE)):
arm.tilt_x = angles[0]
temp_y = -angles[1] #+ 0.065
arm.tilt_z = angles[2]
else:
arm.tilt_x = -angles[0]
temp_y = -angles[1] - 0.065
arm.tilt_z = angles[2]
if (arm.tilt_z > arm._pi):
arm.tilt_z = arm.tilt_z - 2 * arm._pi
if arm.get_tilt_y_from_imu:
arm.tilt_y = temp_y
else:
arm.tilt_y = arm.camera_tilt
if (not (state & (1 << defs.IN_STAIR))):
if arm.tilt_y > temp_y + arm.IMU_TILT_ERROR:
arm.tilt_y = temp_y + arm.IMU_TILT_ERROR
elif arm.tilt_y < temp_y - arm.IMU_TILT_ERROR:
arm.tilt_y = temp_y - arm.IMU_TILT_ERROR
pos, sucess = arm.cinematicaInversa(state)
#publish joints angles
if (sucess):
arm_publisher.publish(joint_variable=pos)
def listener():
rospy.init_node('arm_joy', anonymous=True)
#publish joint values to ur5 arm
arm_publisher = rospy.Publisher('/ur5/jointsPosTargetCommand', ManipulatorJoints, queue_size=10)
#step (in mm) wich the arm position is incremented
step = 1
pos = [0,0,0]
state = 1 << defs.ROBOT_CLOCKWISE
x = 320
y = 71
z = 925
node_sleep_rate = rospy.Rate(10)
while not rospy.is_shutdown():
#read a key and check if it means a increment
#w,s increments/decrements at x axis
#a,d increments/decrements at y axis
#e,q increments/decrements at z axis
key = getkey()
if key in ('wsadqe'):
print ('Key:' + key)
if key == 'w':
x += step
if key == 's':
x += -step
if key == 'a':
y += step
if key == 'd':
y += -step
if key == 'e':
z += step
if key == 'q':
z += -step
#'.' key finishes the program
if key == '.':
print ("finished")
break
#publishes the colected key
arm.x = x
arm.y = y
arm.z = z
print([x,y,z])
pos, sucess = arm.cinematicaInversa(state)
if(sucess):
arm_publisher.publish(joint_variable = pos)
node_sleep_rate.sleep()
def arm_pos(data):
global state
if (state & 1 << defs.ARM_CHANGING_POSE):
return
global arm_publisher
#print(data) #debug
arm.x = data.data[0]
arm.y = data.data[1]
arm.z = data.data[2]
#get the joints angles
pos, sucess = arm.cinematicaInversa(state)
#publish joints angles
if (sucess):
arm_publisher.publish(joint_variable=pos)
def arm_move(data):
#globals
global state
global fire_found
global narrow_path_counter
global arm_publisher
global state_publisher
global rosi_speed_publisher
global leaving_fire_cont
global arm_move_sucess_counter
#if the arm is changing position or the robot is rotating, the arm it's not supose to change its position
if ((state & (1 << defs.ARM_CHANGING_POSE | 1 << defs.ROBOT_ROTATION
| 1 << defs.CLIMB_STAIR))):
return
#print(data) #debug
x_move = data.data[0]
y_move = data.data[1]
z_move = data.data[2]
#if the robot it's on right side of the track, the x and y axis are swaped
if (not state & (1 << defs.ROBOT_CLOCKWISE)):
temp = x_move
x_move = y_move
y_move = temp
#to debug only
print("xmove : " + str(x_move) + " y_move: " + str(y_move))
#if the arm is detecting a fire, but didn't touch it yet
if (not state & (1 << defs.LEAVING_FIRE)):
#if the arm just detected the fire, change it's position
if (not fire_found):
fire_found = True
state_publisher.publish(data=defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.SETTING_UP_HOKUYO)
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
rosi_speed_publisher.publish(data=[0, 0, 0, 0, 0, 0])
if (not state & (1 << defs.IN_STAIR)):
arm.x = arm.FIRE_FOUND_X_VALUE
arm.y = arm.FIRE_FOUND_Y_VALUE
arm.z = arm.FIRE_FOUND_Z_VALUE
if (state & (1 << defs.ROBOT_ANTICLOCKWISE)):
arm.z -= 10
arm.x += 100
#if the fire was dected on previously frames, try to get closer to it
else:
#the camera sends the distance in the x axis from the arm to the fire
#if the arm isn't already touching it, then move the arm to the position indicated by the camera
if (not state &
(1 << defs.FOUND_FIRE_FRONT | 1 << defs.FOUND_FIRE_TOUCH
| 1 << defs.IN_STAIR)):
arm.x += x_move
arm.y += y_move
arm.z += z_move
#print("move x : " + str(x))
#try to centralize the fire to te arm to find the fire in the hokuyo reading
if (state & (1 << defs.SETTING_UP_HOKUYO)):
#if the arm is centralized with the fire, start hokuyo reading
if (x_move == 0 and y_move == 0):
if (state & (1 << defs.IN_STAIR)):
arm.z = arm.z - 60
arm.x = arm.x + 40
state |= (1 << defs.HOKUYO_READING
| 1 << defs.ARM_CHANGING_POSE)
rosi_speed_publisher.publish(data=([0, 0, 0, 0]))
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
state_publisher.publish(data=defs.HOKUYO_READING)
#if the fire is in the back of the robot, make the robot go reverse
elif ((x_move < 0 and x_move > -10)
or (y_move > 0 and y_move < 10)):
state_publisher.publish(data=-defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.SETTING_UP_HOKUYO)
rosi_speed_publisher.publish(
data=([-0.2, -0.2, -0.2, -0.2]))
#if the fire is in the front of the robot, make the robot go foward
elif (abs(x_move) < 10 and abs(y_move) < 10):
state_publisher.publish(data=-defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.SETTING_UP_HOKUYO)
#if the robot is it in the stair, the give a little extra speed in the wheels
if (state & (1 << defs.IN_STAIR)):
rosi_speed_publisher.publish(
data=([0.5, 0.5, 0.5, 0.5]))
else:
rosi_speed_publisher.publish(
data=([0.2, 0.2, 0.2, 0.2]))
elif (x_move == -10 or y_move == 10):
state_publisher.publish(data=-defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.SETTING_UP_HOKUYO)
rosi_speed_publisher.publish(
data=([-0.7, -0.7, -0.7, -0.7]))
elif (arm.x < 300 and (state & (1 << defs.FOUND_FIRE_RIGHT))):
arm.z = arm.FIRE_FOUND_Z_VALUE
rosi_speed_publisher.publish(data=[0, 0, 0, 0])
state_publisher.publish(data=-defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.ROBOT_ROTATION)
#get the joints angles
pos, sucess = arm.cinematicaInversa(state)
#publish joints angles
if (sucess):
arm_publisher.publish(joint_variable=pos)
arm_move_sucess_counter = 20
def state_callback(data):
global state
global in_stairs
global narrow_path_counter
global arm_publisher
global state_publisher
global rosi_speed_publisher
global robot_last_orientation
state = data.data
if (state & (1 << defs.INITIAL_SETUP)):
global arm_publisher
global state_publisher
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
speed = [0, 0, 0, 0]
rosi_speed_publisher.publish(data=speed)
#if the arm is it in
if ((state & (1 << defs.NARROW_PATH | 1 << defs.IN_STAIR))
and not narrow_path_counter):
narrow_path_counter = 40
arm.x = arm.NARROW_PATH_X_VALUE
arm.y = arm.NARROW_PATH_Y_VALUE
arm.z = arm.NARROW_PATH_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
elif (not state & (1 << defs.NARROW_PATH | 1 << defs.IN_STAIR)
and narrow_path_counter):
narrow_path_counter -= 1
#print("narrow path counter = " + str(narrow_path_counter))
if (narrow_path_counter == 0):
arm.x = arm.FIRE_NOT_FOUND_X_VALUE
arm.y = arm.FIRE_NOT_FOUND_Y_VALUE
arm.z = arm.FIRE_NOT_FOUND_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
if (state & (1 << defs.CLIMB_STAIR)):
arm_publisher.publish(joint_variable=[0, 0, 0, 0, 0, 0])
if (not state & (1 << robot_last_orientation)):
if (robot_last_orientation == defs.ROBOT_CLOCKWISE):
robot_last_orientation = defs.ROBOT_ANTICLOCKWISE
else:
robot_last_orientation = defs.ROBOT_CLOCKWISE
arm.x = arm.FIRE_NOT_FOUND_X_VALUE
arm.y = arm.FIRE_NOT_FOUND_Y_VALUE
arm.z = arm.FIRE_NOT_FOUND_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
rosi_speed_publisher.publish(data=[0, 0, 0, 0, 0, 0])
if (state & (1 << defs.IN_STAIR) and not in_stairs):
in_stairs = True
arm.x = arm.IN_STAIRS_X_VALUE
arm.y = arm.IN_STAIRS_Y_VALUE
arm.z = arm.IN_STAIRS_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
state_publisher.publish(data=defs.FORCE_VELODYME)
if (((state & (1 << defs.ARM_CHANGING_POSE))
and (not (state & (1 << defs.FORCE_VELODYME))))):
rosi_speed_publisher.publish(data=[0, 0, 0, 0, 0, 0])
elif (((state & (1 << defs.FORCE_VELODYME))
and (not (state & (1 << defs.ARM_CHANGING_POSE))))):
state_publisher.publish(data=-defs.FORCE_VELODYME)
def hokuyo_distance_callback(data):
global state
global fire_found
global torque_value
global state_publisher
global hokuyo_distance
global leaving_fire_cont
global arm_move_sucess_counter
#get the data from the hokuyo reading
hokuyo_distance = data.data[0]
#if the arm is moving, is better not process it, because it will change a lot
if (state & (1 << defs.ARM_CHANGING_POSE)):
return
#const used to move the arm
x_temp = 0
y_temp = 0
x_move = 0
y_move = 0
if (state & 1 << defs.ROBOT_CLOCKWISE):
x_temp = arm.x
y_temp = arm.y
else:
x_temp = arm.y
y_temp = arm.x
#the hokuyo gives how much distance the arm is in the x an y axis
#if the arm is getting closer to the fire it's better that the x axis distance is equal to 0
if (state & (1 << defs.FOUND_FIRE_RIGHT | 1 << defs.FOUND_FIRE_TOUCH
| 1 << defs.FOUND_FIRE_FRONT)):
x_dist = data.data[1]
if (x_dist > 10):
x_dist = 10
elif (x_dist < -10):
x_dist = -10
#print("getting distance by hokuyo. x_dist: " + str(x_dist) + " plus displacement: " + str(x_dist + _HOKUYO_DISPLACEMENT) + " x: " + str(arm.x))
x_move += (int)(x_dist) + _HOKUYO_DISPLACEMENT
#debug
#print ("\nhokuyo distance: " + str (hokuyo_distance))
#change from FOUND_FIRE_RIGHT to FOUND_FIRE_FRONT
if (not state & (1 << defs.ENABLE_VELODYME | 1 << defs.ROBOT_ROTATION)
and state & (1 << defs.FOUND_FIRE_RIGHT)):
state_publisher.publish(data=-defs.FOUND_FIRE_RIGHT)
state_publisher.publish(data=defs.FOUND_FIRE_FRONT)
#if it's the first reading from the hokuyo, the SETTING_UP_HOKUYO is still up
elif (state & (1 << defs.SETTING_UP_HOKUYO)):
if (hokuyo_distance > _TOUCH_FIRE_FRONT_DISTANCE):
state_publisher.publish(data=-defs.SETTING_UP_HOKUYO)
state_publisher.publish(data=defs.ENABLE_VELODYME)
state_publisher.publish(data=-defs.FOUND_FIRE_RIGHT)
state_publisher.publish(data=-defs.HOKUYO_READING)
else:
state_publisher.publish(data=-defs.SETTING_UP_HOKUYO)
state_publisher.publish(data=-defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.FOUND_FIRE_TOUCH)
state_publisher.publish(data=defs.HOKUYO_READING)
arm.camera_tilt = 0
arm.tilt_y = 0
#if the robot is facing the fire, make it go foward while is distante from the fire
elif (state & (1 << defs.FOUND_FIRE_FRONT)
and hokuyo_distance > _TOUCH_FIRE_FRONT_DISTANCE):
rosi_speed_publisher.publish(data=[2, 2, 2, 2])
#if the robot is facing the fire and it's closer to it, rotate the robot to be able to touch the fire
elif (hokuyo_distance <= _TOUCH_FIRE_FRONT_DISTANCE
and state & (1 << defs.FOUND_FIRE_FRONT)):
rosi_speed_publisher.publish(data=[0, 0, 0, 0])
state_publisher.publish(data=-defs.FOUND_FIRE_FRONT)
state_publisher.publish(data=defs.ROBOT_ROTATION)
state_publisher.publish(data=defs.FOUND_FIRE_TOUCH)
#if the robot is close to the fire...
elif (state & (1 << defs.FOUND_FIRE_TOUCH)
and not state & (1 << defs.ROBOT_ROTATION)):
#if the distance in the x axis is still big, the make it go foward or reverse
if (state & (1 << defs.ROBOT_CLOCKWISE)
and (x_temp > 400 or x_temp < 300)):
if (x_temp > 400):
rosi_speed_publisher.publish(data=([0.5, 0.5, 0.5, 0.5]))
elif (x_temp < 300):
rosi_speed_publisher.publish(data=([-0.5, -0.5, -0.5, -0.5]))
#if the distance in the x axis is still big, the make it go foward or reverse
elif (not (state & (1 << defs.ROBOT_CLOCKWISE))
and (x_temp < -50 or x_temp > 0)):
if (x_temp < -50):
rosi_speed_publisher.publish(data=([0.5, 0.5, 0.5, 0.5]))
elif (x_temp > 0):
rosi_speed_publisher.publish(data=([-0.5, -0.5, -0.5, -0.5]))
#if the distance in the x axis is little, the make the arm go in the direction of the fire
elif (torque_value < 0.13 and not state & (1 << defs.LEAVING_FIRE)):
y_move -= 4
if (state & (1 << defs.IN_STAIR)):
if (arm.z > arm.FIRE_TOUCH_Z_VALUE_IN_STAIRS):
arm.z -= 2
else:
if (arm.z > arm.FIRE_TOUCH_Z_VALUE):
arm.z -= 2
if (state & (1 << defs.BEAM_FIND)):
arm.z -= 2
rosi_speed_publisher.publish(data=[0, 0, 0, 0])
#if the arm alrady touch the fire, but it's still close to it. Make it go backwards, but no really fast
elif (torque_value >= 0.1 and not state & (1 << defs.LEAVING_FIRE)):
leaving_fire_cont = 0
state_publisher.publish(data=defs.LEAVING_FIRE)
fire_found = False
#if the arm alrady touch the fire and is distant from it. make it go backward and fast
elif (state & (1 << defs.LEAVING_FIRE) and hokuyo_distance < 25):
if hokuyo_distance > 15:
y_move += 10
else:
y_move += 5
#if the arm alrady touch the fire and is really distant from it. return to the original position
elif (state & (1 << defs.LEAVING_FIRE) and hokuyo_distance >= 25):
if (narrow_path_counter):
arm.x = arm.NARROW_PATH_X_VALUE
arm.y = arm.NARROW_PATH_Y_VALUE
arm.z = arm.NARROW_PATH_Z_VALUE
else:
arm.x = arm.FIRE_NOT_FOUND_X_VALUE
arm.y = arm.FIRE_NOT_FOUND_Y_VALUE
arm.z = arm.FIRE_NOT_FOUND_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
state_publisher.publish(data=-defs.FOUND_FIRE_TOUCH)
state_publisher.publish(data=-defs.HOKUYO_READING)
state_publisher.publish(data=defs.ENABLE_VELODYME)
state_publisher.publish(data=defs.ARM_CHANGING_POSE)
return
#move the arm
if (state & 1 << defs.ROBOT_CLOCKWISE):
arm.x = x_temp + x_move
arm.y = y_temp + y_move
else:
arm.x = y_temp - y_move
arm.y = x_temp + x_move
pos, sucess = arm.cinematicaInversa(state)
if (sucess):
arm_publisher.publish(joint_variable=pos)
arm_move_sucess_counter = 20
else:
arm_move_sucess_counter -= 1
if (arm_move_sucess_counter == 0):
state_publisher.publish(data=defs.LEAVING_FIRE)
state_publisher.publish(data=defs.ENABLE_VELODYME)
state_publisher.publish(data=-defs.HOKUYO_READING)
state_publisher.publish(data=-defs.FOUND_FIRE_FRONT)
state_publisher.publish(data=-defs.FOUND_FIRE_TOUCH)
state_publisher.publish(data=-defs.FOUND_FIRE_FRONT)
state_publisher.publish(data=-defs.SETTING_UP_HOKUYO)
arm.x = arm.FIRE_NOT_FOUND_X_VALUE
arm.y = arm.FIRE_NOT_FOUND_Y_VALUE
arm.z = arm.FIRE_NOT_FOUND_Z_VALUE
pos, sucess = arm.cinematicaInversa(state)
arm_publisher.publish(joint_variable=pos)
arm_move_sucess_counter = 20