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