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
def test_dictionary_with_char(self):
from std_msgs.msg import Char
expected_message = Char(data = 99)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/Char', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def test_ros_message_with_char(self):
from std_msgs.msg import Char
expected_dictionary = { 'data': 99 }
message = Char(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def __init__(self,parent=None):
# GUI
super(Test, self).__init__(parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# ROS,pubの設定。
self.control_command = Char()
self.pub_cmd = rospy.Publisher('/DirCommand',Char,queue_size=10)
def __init__(self):
rospy.init_node("armControlLogic", anonymous=False)
self.motors_lock = True
self.index = 1
self.gripperPositionLock = 0
self.lock = [0] * 2
self.command = Char()
self.pwm = [Int16()] * 5
self.motor_selection = UInt8()
self.pwm0_pub = rospy.Publisher(name="manipulator/axisZeroPwm",
data_class=Int16,
queue_size=1)
self.pwm1_pub = rospy.Publisher(name="manipulator/axisOnePwm",
data_class=Int16,
queue_size=1)
self.pwm2_pub = rospy.Publisher(name="manipulator/axisTwoPwm",
data_class=Int16,
queue_size=1)
self.pwm3_pub = rospy.Publisher(name="manipulator/axisThreePwm",
data_class=Int16,
queue_size=1)
self.pwm4_pub = rospy.Publisher(name="manipulator/axisFourPwm",
data_class=Int16,
queue_size=1)
self.pwm_pub = [
self.pwm0_pub, self.pwm1_pub, self.pwm2_pub, self.pwm3_pub,
self.pwm4_pub
]
self.motor_selection_pub = rospy.Publisher(
name="manipulator/motorSelection", data_class=UInt8, queue_size=1)
self.command_pub = rospy.Publisher(name="gripper/command",
data_class=Char,
queue_size=1)
self.joy_sub = rospy.Subscriber(name="spacenav/joy",
data_class=Joy,
callback=self.joyCallback,
queue_size=10)
self.motorsLockSub = rospy.Subscriber(
name="safety/motorsLock",
data_class=Bool,
callback=self.motors_lock_callback,
queue_size=1)
def gripper_open(self):
print("Gripper_Open")
gripper_pose = Char()
gripper_pose = 0
pub.publish(gripper_pose)
def gripper_close(self):
print("Gripper_Close")
gripper_pose = Char()
gripper_pose = 255
pub.publish(gripper_pose)
from sensor_msgs.msg import Image 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]
#Puntoa P0, P1, P3 para movimiento a P3
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]
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
#!/usr/bin/env python
import roslib
import rospy
import math
import time
import rospy
import numpy as np
import time
import serial
import os
from std_msgs.msg import Char
from std_msgs.msg import Int8
msg=Char()
pos=Int8()
past_pos=1
pos_char=0
publisher_pos = rospy.Publisher('servo_pos', Int8, queue_size=1)
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)
pos.data=0
msg.data=0x00
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
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
def test_char_msg_from_json():
msg_json = '{ "data": "105" }'
parsed_msg = json_to_msg(msg_json, Char())
assert parsed_msg.data == 105
from config import globales
#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
defs_obs=[('/training/image', Image)],
func_compile_obs=compile_obs,
defs_reward=[
('/rl/has_obstacle_nearby', Bool),
('/rl/distance_to_longestpath', Float32),
('/rl/car_velocity', Float32),
('/rl/last_on_opposite_path', Int16)],
func_compile_reward=compile_reward,
defs_action=[('/autoDrive_KeyboardMode', Char)],
rate_action=1e-10,
window_sizes={'obs': 1, 'reward': 1},
buffer_sizes={'obs': 5, 'reward': 5},
step_delay_target=1.0,
is_dummy_action=True
)
ACTIONS = [(Char(ord(mode)),) for mode in ['s', 'd', 'a', '!']]
# Agent
agent = RelayAgent(queue_len=5)
n_interactive = 0
try:
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
sess = tf.Session(config=config)
sess.run(tf.global_variables_initializer())
agent.set_session(sess)
while True:
cum_reward = 0.0
n_steps = 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()