def test_char_msg():
msg = Char()
msg.data = 105
parsed_msg = json_to_msg(msg_to_json(msg), Char())
assert parsed_msg.data == msg.data
import base64 #Librery numpy import numpy as np #Library OpenCVvideo_msg=String() import cv2 from cv_bridge import CvBridge, CvBridgeError ##SUBSCRIPTOR VARIABLES #OMNI NUMBER - NODERED-TRADUCTOR omni_n = Int8() omni_n.data = 1 #OMNI MOVE - NODERED-OMNI movimiento_net = Char() movimiento_net.data = 75 #OMNI VELOCITY - NODERED-OMNI vel_net = Float32MultiArray() vel_net.data = [0, 0, 0, 0] ###PUBLISH FUNCTIONS ##ENVIROMENT CAMERA BROKER-NODERED cam1 = String() cam1.data = " " #OMNI POSITION - BROKER-NODERED pos_net = Float32MultiArray() pos_net.data = [0, 0, 0, 0, 0, 0] #ACTION WEB MESSAGE BROKER-NODERED / CONDITIONAL BROKER-LECTOR
puntos = Float32MultiArray()
puntos.data = [0, 0, 0, 0, 0, 0]
#Datos Imu
imu_d = Float32MultiArray()
imu_d.data = [0, 0, 0, 0, 0, 0]
#Datos encoder
encoder_ticks = Int32MultiArray()
encoder_ticks.data = [0, 0, 0, 0]
#######Publiciones con subscriptor#######
#Velocidad de carro
Setpoint_omni = Float32MultiArray()
Setpoint_omni.data = [0, 0, 0, 0]
#Tipo de movimiento
movimiento = Char()
movimiento.data = ord('K')
#Estado de rasberry al moverse de punto A -> B
estado = Int8()
estado.data = 0
#######FUNCIONES DE CALCULO#######
def Calculo_pts():
global puntos
x_0 = puntos.data[2] - puntos.data[0]
y_0 = puntos.data[3] - puntos.data[1]
x_p = puntos.data[4] - puntos.data[0]
y_p = puntos.data[5] - puntos.data[1]
global distancia
#Librery numpy
import numpy as np
import cv2
#from cv_bridge import CvBridge, CvBridgeError
import sys
#global num_omni.data
num_omni = Int8()
num_omni.data = 0
_opc_omni = Int8()
_opc_omni.data = 0
_mov_omni = Char()
_mov_omni.data = ord('K')
_setpoint_omni = Float32MultiArray()
_setpoint_omni.data = [0, 0, 0, 0]
###PUBLISH FUNCTIONS
##ENVIROMENT CAMERA BROKER-NODERED
cam1 = String()
cam1.data = " "
#OMNI POSITION - BROKER-NODERED
pos_net = Float32MultiArray()
pos_net.data = [0, 0, 0, 0, 0, 0]
#VELOCITY OF OMNIS LECTOR-NODERED
rpm_net = Float32MultiArray()
return key
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('ctibot_key')
pub = rospy.Publisher('/cmd_vel_key', Char, queue_size=5)
try:
while (1):
key = getKey()
if key:
print key
twist = Char()
twist.data = ord(key)
pub.publish(twist)
#twist.linear.z = 0
if key == 'q':
exit()
except Exception, e:
print e
finally:
pass #twist = Twist()
#twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
#twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
#pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def controller():
pub1 = rospy.Publisher("/dreamer_controller/neck/goal_position", Float64MultiArray, queue_size=10)
pub2 = rospy.Publisher("/dreamer_controller/neck/eye_goal_position", Float64MultiArray, queue_size=10)
pub3 = rospy.Publisher("fail", Char, queue_size = 10)
rospy.init_node('robot_head_controller', anonymous=True)
#rospy.init_node("scan_dir_listener", anonymous = True)
rate = rospy.Rate(10) # 10hz
tau = 3.1415926535798932 * 2
global scan_mode
global scan_dir
while not rospy.is_shutdown():
rospy.Subscriber("chatter", String, stringBack)
rospy.Subscriber("chatter2", Bool, boolBack)
#print "Testing if publishing a char works"
print "Waiting for facial recognition to start."
while scan_mode != "scan" and not rospy.is_shutdown():
#print scan_mode
rate.sleep()
#rospy.spinOnce()
print "Scanning mode initiated."
startTime = time.time()
eye_yaw = 0
neck_yaw = 0
while scan_mode == "scan" and not rospy.is_shutdown():
#while not rospy.is_shutdown():
curtime = time.time() - startTime
xValue = curtime / 20 * tau
eye_yaw = math.sin(xValue) * 0.59
neck_yaw = math.sin(xValue) * 1.4
neckArray = Float64MultiArray();
eyeArray = Float64MultiArray();
neckArray.data.append(0)
neckArray.data.append(neck_yaw)
neckArray.data.append(0)
neckArray.data.append(0)
eyeArray.data.append(0)
eyeArray.data.append(eye_yaw)
eyeArray.data.append(eye_yaw)
pub1.publish(neckArray)
pub2.publish(eyeArray)
print str(neck_yaw) + ' ' + str(eye_yaw)
rate.sleep()
eyeArray = Float64MultiArray();
eyeArray.data.append(0)
eyeArray.data.append(0)
eyeArray.data.append(0)
pub2.publish(eyeArray)
print "Scanning Horizontally."
fail_test = Char()
fail_test.data = 'g'
pub3.publish(fail_test)
while scan_mode == "focus_h" and fail_test != 'f' and not rospy.is_shutdown():
if scan_dir:
neck_yaw -= 0.005
else:
neck_yaw += 0.005
if abs(neck_yaw) > 1.4:
fail_test.data = 'f'
else:
neckArray = Float64MultiArray();
neckArray.data.append(0)
neckArray.data.append(neck_yaw)
neckArray.data.append(0)
neckArray.data.append(0)
pub1.publish(neckArray)
pub3.publish(fail_test)
rate.sleep()
print "Scanning Vertically."
neck_pitch = 0;
while scan_mode == "focus_v" and fail_test != 'f' and not rospy.is_shutdown():
if scan_dir:
neck_pitch -= 0.005
else:
neck_pitch += 0.005
if neck_pitch < -0.11 or neck_pitch > 0.64:
fail_test.data = 'f'
else:
neckArray = Float64MultiArray();
neckArray.data.append(0)
neckArray.data.append(neck_yaw)
neckArray.data.append(0)
neckArray.data.append(neck_pitch)
pub1.publish(neckArray)
pub3.publish(fail_test)
rate.sleep()
import rospy from std_msgs.msg import Int16 from std_msgs.msg import Char from std_msgs.msg import Float32MultiArray from std_msgs.msg import Int8 _mov_omni_1 = Char() _mov_omni_1.data = 75 _mov_omni_2 = Int16() _mov_omni_2.data = 75 _mov_omni_3 = Int16() _mov_omni_3.data = 75 _mov_omni_4 = Int16() _mov_omni_4.data = 75 _mov_omni_5 = Int16() _mov_omni_5.data = 75 _rpm_omni_1 = Float32MultiArray() _rpm_omni_1.data = [0, 0, 0, 0] _rpm_omni_2 = Int16() _rpm_omni_2.data = 0 _rpm_omni_3 = Int16() _rpm_omni_3.data = 0 _rpm_omni_4 = Int16() _rpm_omni_4.data = 0 _rpm_omni_5 = Int16() _rpm_omni_5.data = 0 _opc_omni_1 = Int8() _opc_omni_1.data = 0
#!/usr/bin/env python
import roslib
import rospy
import math
import time
import rospy
import numpy as np
import time
import serial
from std_msgs.msg import Char
msg = Char()
msg.data = 0x00
def callback(data):
global msg
msg = data
def setup():
#Ros susbscriber B ES EL ROJOOOOOO
rospy.init_node('serial_com')
rospy.Subscriber("servo_move", Char, callback)
if __name__ == "__main__":
aux = 0x02
setup()
