def test_byte_msg():
msg = Byte()
msg.data = b'\x10'
parsed_msg = json_to_msg(msg_to_json(msg), Byte())
assert parsed_msg.data == msg.data
def callback_enc_dx(channel):
global dir_dx
global tick_dx_pub
global v_input_dx
global v_input_sx
if ((v_input_dx != 0) or (v_input_sx != 0)):
enc_msg = Byte()
enc_msg.data = dir_dx
tick_dx_pub.publish(enc_msg)
def pause_cb(self, msg):
if msg.data == 1 and not self.pause_status:
self.ena_pub.publish(Byte(0))
self.pause_time = rospy.get_time()
self.pause_status = True
elif msg.data == 0 and self.pause_status:
self.ena_pub.publish(Byte(1))
self.pause_duration = self.pause_duration + rospy.get_time(
) - self.pause_time
self.pause_status = False
def run(self):
r = rospy.Rate(4)
for i in range(3):
self.pub.publish(Byte(data=65))
r.sleep()
for i in range(3):
self.pub.publish(Byte(data=90))
r.sleep()
return 'Completed_Successfully'
def main():
buttondown_pub = rospy.Publisher('/rfreceiver/buttondown',
Byte,
queue_size=1)
rospy.init_node(NODE_NAME)
device_path = rospy.get_param('~device_path')
baud_rate = rospy.get_param('~baud_rate', 9600)
retry_grace_time = rospy.get_param('~retry_grace_time', 30)
ready = False
i = 0
while not ready:
try:
receiver = serial.Serial(port=device_path, baudrate=baud_rate)
ready = True
except SerialException:
rospy.logerr("Could not open device_path: %s" % (device_path))
rospy.logerr("I'm going to sleep %s seconds between attempts" %
(i))
time.sleep(i)
if i < retry_grace_time:
i += 3
buf = ''
while not rospy.is_shutdown():
try:
button = int(receiver.readline(10).strip())
except serial.SerialException as e:
print e
break
buttondown_pub.publish(Byte(button))
def test_dictionary_with_byte(self):
from std_msgs.msg import Byte
expected_message = Byte(data = 3)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/Byte', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def test_ros_message_with_byte(self):
from std_msgs.msg import Byte
expected_dictionary = { 'data': 5 }
message = Byte(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def start(self):
self.launch.start()
time.sleep(10)
rospy.init_node('admin')
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_cb)
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=3)
while not self.pose_pub.get_num_connections():
pass
#init_pose = [ 0.0453101396561, 0.00364243984222, 0.0, 0.0, 0.0, -0.032842760778, 0.999460531019]
init_pose = self.traj[self.waypt_idx]
print(init_pose)
# initialize position in the map.
ipose = PoseWithCovarianceStamped()
ipose.header.frame_id = "map"
ipose.pose.pose.position.x = init_pose[0]
ipose.pose.pose.position.y = init_pose[1]
ipose.pose.pose.orientation.z = init_pose[5]
ipose.pose.pose.orientation.w = init_pose[6]
self.pose_pub.publish(ipose)
while not payload.isReady():
pass
self.pose_pub.publish(ipose)
time.sleep(1)
self.run_thread = multiprocessing.Process(target=self.run, args=())
self.run_thread.daemon = True
self.run_thread.start()
#def payload_mon(self):
print('In main process')
#print(payload.isReady())
pause_pub = rospy.Publisher('/pauseAction', Byte, queue_size=1)
while payload.isReady():
#print(payload.isPaused())
if payload.isPaused():
pause_pub.publish(Byte(1))
else:
pause_pub.publish(Byte(0))
time.sleep(0.1)
def saveData(self):
try:
rospy.wait_for_service('to_csv')
to_csv = rospy.ServiceProxy('to_csv', ToCsv)
number_msg = Byte(int(self.lineEdit.text()))
resp = to_csv(number_msg)
except (rospy.ServiceException, rospy.ROSException) as e:
print("Service call failed: %s" % e)
def __init__(self):
self.v_lim = [0.5, 0.5, 0.5]
rospy.init_node('teleop_twist_keyboard')
rospy.Subscriber('joy', Joy, self.joy_cb)
self.vel_pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
Twist,
queue_size=1)
self.ena_pub = rospy.Publisher('/mobile_base_controller/control_mode',
Byte,
queue_size=1)
while not self.ena_pub.get_num_connections():
pass
self.ena_pub.publish(
Byte(1)) # enable the robot and enters velocity mode
def main():
buttondown_pub = rospy.Publisher('/rfreceiver/buttondown',
Byte,
queue_size=1)
rospy.init_node('rfreceiver')
device_path = rospy.get_param('~device_path')
baud_rate = rospy.get_param('~baud_rate', 9600)
retry_grace_time = rospy.get_param('~retry_grace_time', 5)
device_timeout = rospy.get_param('~serial_timeout', 10)
retry_attempts = rospy.get_param('~retry_attempts', 20)
ready = False
# Try `retry_attempts` times with `retry_grace_time` grace period between attempts to open the device
while not ready and retry_attempts >= 0:
try:
receiver = serial.Serial(port=device_path,
baudrate=baud_rate,
timeout=device_timeout)
ready = True
except SerialException:
rospy.logerr("Could not open device_path: %s in %s seconds" %
(device_path, device_timeout))
rospy.logerr(
"I'm going to try %s times more and will sleep %s seconds between attempts"
% (retry_attempts, retry_grace_time))
time.sleep(retry_grace_time)
retry_attempts -= 1
buf = ''
if not ready:
rospy.logerr("Giving up - couldn't connect to the device %s" %
device_path)
while not rospy.is_shutdown():
pass
else:
while not rospy.is_shutdown():
try:
button = int(receiver.readline(10).strip())
except serial.SerialException as e:
print e
break
buttondown_pub.publish(Byte(button))
def get_twist(
x, y, theta,
xy_g): # arguments are the robot's pose in the global reference frame
global i
#print("i",i)
#print(len(xy_g))
if i == 0:
i = len(xy_g)
print("i", i)
#print (len(xy_g))
#print (xy_g)
if i > 0:
flage = 0
if (i % 2 == 0):
i -= 1
print("i", i)
#x_g = xy_g[i-1]
#for map 1
x_g = ((xy_g[i]) - 1) * 17.5
#for map 2
#x_g = ((xy_g[i])-1)*25
print(x)
print('x', x_g)
#y_g = xy_g[i]
#for map 1
y_g = ((xy_g[i - 1]) - 1) * 17.5
#for map 2
#y_g = ((xy_g[i-1])-1)*25
print(y)
print('y', y_g)
#print("in while1")
if (flage == 0):
# get the error in the global reference frame
error_x = x_g - x
#print("ex",error_x)
error_y = y_g - y
#print("ey",error_y)
if ((error_x >= -3) and (error_x <= 3)):
error_x = 0
if ((error_y >= -3) and (error_y <= 3)):
error_y = 0
#print("error_x",error_x)
#print("error_y",error_y)
# get the error in the robot's reference frame
# @Xreference//Ycam
gr_y = error_x * math.cos(theta) + error_y * math.sin(
theta) #takes rad
gr_x = -error_x * math.sin(theta) + error_y * math.cos(theta)
# @ Xreference//Xcam == - eqn elmo3eed
#gr_x = -(error_x*math.cos(theta) + error_y*math.sin(theta)) #takes rad
#gr_y = -error_x*math.sin(theta) + error_y*math.cos(theta)
#print("gr_y",gr_y)
#print("gr_x",gr_x)
# calculate rho and alfa
rho = math.sqrt(gr_x**2 + gr_y**2) #in cm
alfa = math.atan2(gr_y, gr_x) #in rad
# define controller gains
K_RHO = 1.4 # 0.6 doesn't work fine with me
#(robot becomes unstable a bit after arriving to the desired pose)
K_ALPHA = 0.5
#K_THETA = 0.5
#calculate control commands
v = K_RHO * rho # in cm/sec # v = linear velocity command
omega = K_ALPHA * alfa # in rad/sec # omega = angular velocity command
# return th control commands
twist = Twist()
gtg_flag = Byte()
gtg_flag = 0
if ((omega <= 0.2) and (omega >= -0.2)):
omega = 0
if (omega != 0):
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = omega
elif (omega == 0):
twist.linear.x = v
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
#print("going to publish")
#print("publishing1")
cmd_vel_publisher1.publish(twist)
#cmd_vel_publisher2.publish(gtg_flag)
if (gr_x == 0 and gr_y == 0):
print("flage")
flage = 1
if (flage == 1):
#print("increment i")
i -= 2
if i < 0:
twist = Twist()
gtg_flag = Byte()
gtg_flag = 1
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
print("i", i)
cmd_vel_publisher1.publish(twist)
cmd_vel_publisher2.publish(gtg_flag)
def map_follow(self):
# initializations
rospy.init_node('map_follow', anonymous=True)
pub = rospy.Publisher('waypts', Marker, queue_size=100)
cmd_pub = rospy.Publisher("/mobile_base_controller/cmd_vel",
Twist,
queue_size=1)
mode_pub = rospy.Publisher('/mobile_base_controller/control_mode',
Byte,
queue_size=1)
# Set control mode 1- vel cont
while not mode_pub.get_num_connections(
): # hold there until the subsecribers are ready
pass
mode_pub.publish(Byte(data=1))
#rospy.Subscriber("scan",LaserScan,self.scanCallback)
rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped,
self.amcl_callback)
#rospy.Subscriber("map",OccupancyGrid,self.map_callback)
rospy.Subscriber("/ov3/button", Empty, self.button_cb)
rospy.Subscriber("control_params", ctrlParams, self.ctrlprm_update)
rt = rospy.Rate(self.pd_rate)
# Run controller param
run_flag = rospy.get_param('runPD_q')
print('Run Flag is', run_flag)
self.parse_waypts()
self.waypt_buf = self.trajs
self.waypt_i = 0
self.running = True
# for showing waypts on rviz
wpt_marker = Marker()
wpt_marker.header.frame_id = "/map"
wpt_marker.type = wpt_marker.ARROW
wpt_marker.action = wpt_marker.ADD
wpt_marker.pose.orientation.x = 0
wpt_marker.pose.orientation.y = 0
wpt_marker.pose.orientation.z = 0
wpt_marker.pose.orientation.w = 1.0
wpt_marker.pose.position.x = 0
wpt_marker.pose.position.y = 0
wpt_marker.pose.position.z = 0
wpt_marker.scale.x = 1
wpt_marker.scale.y = 0.1
wpt_marker.scale.z = 0.1
wpt_marker.color.a = 0.8
wpt_marker.color.r = 0.2
wpt_marker.color.g = 1.0
wpt_marker.color.b = 0.2
rospy.loginfo('************** Initialization complete ************')
while not rospy.is_shutdown() and (run_flag == 1):
# compute distance between present pos estimate and latest waypt update
self.dis_err = np.sqrt(
math.pow((self.car_ax - self.waypt_xd), 2) +
math.pow((self.car_ay - self.waypt_yd), 2))
#rospy.loginfo(self.dis_err)
# waypoint updates
if(rospy.get_time()-self.waypt_ptime > 0.1 and\
self.dis_err < self.waypt_lim):
self.waypt_ptime = rospy.get_time()
self.waypt_xd = self.waypt_buf[self.waypt_i][0]
self.waypt_yd = self.waypt_buf[self.waypt_i][1]
#rospy.loginfo([self.waypt_xd,self.waypt_yd, 'wpt'])
print(self.waypt_i, len(self.waypt_buf))
if (self.waypt_i < (len(self.waypt_buf) - 1)):
self.waypt_i += 1
else:
print('End')
self.running = False
cmd_pub.publish(Twist())
# waypt publishing
wpt_marker.pose.position.x = self.waypt_xd
wpt_marker.pose.position.y = self.waypt_yd
#rospy.loginfo(wpt_marker)
pub.publish(wpt_marker)
prev_time = self.current_time #compute the time for the loop
###########################
# The pd control
###### Steer pd
alpha = math.atan2(self.waypt_yd - self.car_ay,
self.waypt_xd - self.car_ax)
self.steer_err = alpha - self.car_ayaw
if (self.steer_err > math.pi):
self.steer_err = -2 * math.pi + self.steer_err
elif (self.steer_err < -math.pi):
self.steer_err = 2 * math.pi + self.steer_err
# controller
self.steer = self.kp_steer*self.steer_err \
+ self.kd_steer*(self.steer_err-self.psteer_err)
######## gas pd
self.gas_err = self.dis_err
# Controller
self.gasvel = self.kp_gas*self.gas_err \
+ self.kd_gas*(self.gas_err-self.pgas_err)
# Update previous errors
self.pgas_err = self.gas_err
self.psteer_err = self.steer_err
# Scale the values and write
# for steer we limit the scale to:
# steer_limit = +/- max_dist_frm_wall*kp_steer = 0.5*5 = 2.5 + 0.5(for kd term)
# This angle max is assumed to 30deg equivalent to 3(steer_limit)
# self.steer = self.map_vel(self.steer, -math.pi/3, math.pi/3, self.steer_max, self.steer_min)
# steer_wrt = int(self.steer) # Steer to be written
# self.gasvel = self.map_vel(self.gasvel, 0, 2048, 0, 2048) # change when controlling gas
# gasvel_wrt = int(self.gasvel)*self.vlim # gas velocity to be written
# scale gasvel inversely to steer
# gasvel_wrt = 0.08*(2048/(abs(steer_wrt)+10))*gasvel_wrt + 190
# for low velocities needed set 0 every 6 cycles
# if(abs(steer_wrt) > 1800):
# self.gas_cnt = self.gas_cnt+1
# if(self.gas_cnt == 6):
# self.gas_cnt = 0
# gasvel_wrt = 0
# else:
# gasvel_wrt = 400 * self.vlim
# if(gasvel_wrt > 440):
# gasvel_wrt = 440 * self.vlim
# Setup write string and write to serial port
if self.car_state == 0:
self.ser_write = Twist()
else:
self.ser_write.linear.x = self.gasvel
self.ser_write.angular.z = self.steer
#print(self.vlim)
#print(self.ser_write)
if self.running:
#print('Running')
cmd_pub.publish(self.ser_write)
#print(self.ser_write)
else:
cmd_pub.publish(Twist())
rt.sleep()
run_flag = rospy.get_param('runPD_q')
if rospy.is_shutdown() or (run_flag == 0):
print('Sutting down')
try:
print('Setting velocities to zero')
cmd_pub.publish(Twist())
mode_pub.publish(Byte(data=0))
except:
print('Error closing serial port, do it manually')
if (run_flag == 0):
rospy.set_param('runPD_q', 1)
def main(self):
# Publish initial pose
#ret = input('Is robot in position? (1-Yes, 0-NO)')
#if ret==0:
# rospy.signal_shutdown('Exit')
#ipose = PoseWithCovarianceStamped()
#ipose.pose.pose.position.x = self.init_pose[0]
#ipose.pose.pose.position.y = self.init_pose[1]
#ipose.pose.pose.orientation.z = self.init_pose[5]
#ipose.pose.pose.orientation.w = self.init_pose[6]
#self.pose_pub.publish(ipose)
#time.sleep(1)
# Start publishing goals from trajectory
#msg = PoseStamped()
#self.gpub.publish(msg)
#self.gpub.publish(msg)
while not self.gpub.get_num_connections():
pass
for i in range(self.traj.shape[0]):
msg = PoseStamped()
# Create header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = "map"
msg.header = h
# Add position
msg.pose.position.x = self.traj[i, 0]
msg.pose.position.y = self.traj[i, 1]
# Add orientation
msg.pose.orientation.x = self.traj[i, 3]
msg.pose.orientation.y = self.traj[i, 4]
msg.pose.orientation.z = self.traj[i, 5]
msg.pose.orientation.w = self.traj[i, 6]
self.status = 0
self.gpub.publish(msg)
time.sleep(0.5)
if i == 0:
self.gpub.publish(msg)
time.sleep(0.5)
print(msg)
cond = True
itim = rospy.get_time()
while cond and not rospy.is_shutdown():
dt = rospy.get_time() - itim
cond = self.status != 3 and dt < 30
continue
if (self.status == 3 and self.traj[i, 7] > 0):
# do disinfection stuff here.
loc = self.last_loc
#payload.turnOnUVC()
if (self.traj[i, 7] > 10):
self.ena_pub.publish(Byte(0))
time.sleep(4)
time.sleep(self.traj[i, 7] - 8)
self.ena_pub.publish(Byte(1))
time.sleep(4)
#payload.turnOffUVC()
else:
time.sleep(self.traj[i, 7])
#loc.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
self.pose_pub.publish(loc)
time.sleep(1)
if self.status != 3:
rospy.logerr('Did not succeed, status code: ' +
str(self.status))
sys.exit(self.status - 3)
#if res.status.status != 3:
# break
if rospy.is_shutdown():
#payload.turnOffUVC()
break
return 0
def base_enable(self):
self.ena_pub.publish(Byte(1))
def base_disable(self):
self.ena_pub.publish(Byte(0))
value = 0
try:
print(msg)
while (1):
key = getKey()
if key in pinBindings.keys():
value = pinBindings[key]
else:
value = 0
if (key == '\x03'):
break
packet = Byte()
packet.data = value
pub.publish(packet)
packet.data = 0
pub.publish(packet)
print(Pots.pot1)
except Exception as e:
print(e)
finally:
packet = Byte()
value = 0
packet.data = value
pub.publish(packet)
def map_follow(self):
# initializations
rospy.init_node('map_follow', anonymous=True)
pubUI = rospy.Publisher('waypts', Marker, queue_size=100)
# cmd_pub = rospy.Publisher("/mobile_base_controller/cmd_vel", Twist, queue_size = 1)
# Set control mode 1- vel cont
while not self.mode_pub.get_num_connections(
): # hold there until the subsecribers are ready
pass
self.mode_pub.publish(Byte(data=1))
rospy.Subscriber("scan", LaserScan, self.scanCallback)
#rospy.Subscriber("amcl_pose",PoseWithCovarianceStamped,self.amcl_callback)
#rospy.Subscriber("map",OccupancyGrid,self.map_callback)
rospy.Subscriber("/ov3/button", Empty, self.button_cb)
#rospy.Subscriber("control_params", ctrlParams, self.ctrlprm_update)
rate = rospy.Rate(self.cmd_rate)
# Run controller param
run_flag = 1
#run_flag = rospy.get_param('runPD_q')
#print('Run Flag is', run_flag)
self.parse_waypts()
# for showing waypts on rviz
wpt_marker = Marker()
wpt_marker.header.frame_id = "/map"
wpt_marker.type = wpt_marker.ARROW
wpt_marker.action = wpt_marker.ADD
wpt_marker.pose.orientation.x = 0
wpt_marker.pose.orientation.y = 0
wpt_marker.pose.orientation.z = 0
wpt_marker.pose.orientation.w = 1.0
wpt_marker.pose.position.x = 0
wpt_marker.pose.position.y = 0
wpt_marker.pose.position.z = 0
wpt_marker.scale.x = 1
wpt_marker.scale.y = 0.1
wpt_marker.scale.z = 0.1
wpt_marker.color.a = 0.8
wpt_marker.color.r = 0.2
wpt_marker.color.g = 1.0
wpt_marker.color.b = 0.2
rospy.loginfo('************** Initialization complete ************')
while not rospy.is_shutdown() and (run_flag == 1):
# compute distance between present pos estimate and latest waypt update
#self.dis_err = np.sqrt(math.pow((self.car_ax-self.waypt_xd),2)+math.pow((self.car_ay-self.waypt_yd),2))
#rospy.loginfo(self.dis_err)
# waypoint updates
if (rospy.get_time() - self.waypt_ptime > 0.1):
self.waypt_ptime = rospy.get_time()
self.waypt_xd = self.waypt_buf[self.waypt_i, 0]
self.waypt_yd = self.waypt_buf[self.waypt_i, 1]
#rospy.loginfo([self.waypt_xd,self.waypt_yd, 'wpt'])
if (self.waypt_i < (len(self.waypt_buf) - 1)):
self.waypt_i += 1
else:
self.running = False
cmd_pub.publish(Twist())
# waypt publishing
wpt_marker.pose.position.x = self.waypt_xd
wpt_marker.pose.position.y = self.waypt_yd
#rospy.loginfo(wpt_marker)
pubUI.publish(wpt_marker)
# TODO: make LED on button go off instead...
#if self.running:
# print('Running')
# cmd_pub.publish(self.ser_write)
#else:
# cmd_pub.publish(Twist())
rt.sleep()
run_flag = rospy.get_param('runPD_q')
if rospy.is_shutdown() or (run_flag == 0):
print('Sutting down')
try:
print('Setting velocities to zero')
cmd_pub.publish(Twist())
self.mode_pub.publish(Byte(data=0))
except:
print('Error closing serial port, do it manually')
if (run_flag == 0):
rospy.set_param('runPD_q', 1)
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
Twist,
queue_size=1)
rospy.init_node('teleop_twist_keyboard')
pub2 = rospy.Publisher('/mobile_base_controller/control_mode',
Byte,
queue_size=1)
#mode = Byte(1)
# hold there until the subsecribers are ready
while not pub2.get_num_connections():
pass
pub2.publish(Byte(data=1))
#os.system('rostopic pub --once /mobile_base_controller/control_mode std_msgs/Byte 1')
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print(vels(speed, turn))
while (1):
key = getKey()
def createParticipant(self):
try:
rospy.wait_for_service('create_participant', timeout=2.0)
create_participant = rospy.ServiceProxy('create_participant',
CreateParticipant)
number = int(self.lineEdit.text())
number_msg = Byte(number)
if self.radioButton.isChecked():
gender = 1
else:
gender = 0
gender_msg = Bool(gender)
age = int(self.lineEdit_3.text())
age_msg = Byte(age)
field_of_study = self.lineEdit_4.text()
field_of_study_msg = String(field_of_study)
if self.radioButton_3.isChecked():
teleop_experience = 0
elif self.radioButton_4.isChecked():
teleop_experience = 1
elif self.radioButton_5.isChecked():
teleop_experience = 2
elif self.radioButton_6.isChecked():
teleop_experience = 3
elif self.radioButton_7.isChecked():
teleop_experience = 4
elif self.radioButton_8.isChecked():
teleop_experience = 5
teleop_experience_msg = Byte(teleop_experience)
if self.radioButton_15.isChecked():
keyboard_experience = 0
elif self.radioButton_16.isChecked():
keyboard_experience = 1
elif self.radioButton_17.isChecked():
keyboard_experience = 2
elif self.radioButton_18.isChecked():
keyboard_experience = 3
elif self.radioButton_19.isChecked():
keyboard_experience = 4
elif self.radioButton_20.isChecked():
keyboard_experience = 5
keyboard_experience_msg = Byte(keyboard_experience)
if self.radioButton_13.isChecked():
left_right_handed = 0
elif self.radioButton_14.isChecked():
left_right_handed = 1
left_right_handed_msg = Bool(left_right_handed)
resp = create_participant(number_msg, gender_msg, age_msg,
field_of_study_msg,
teleop_experience_msg,
keyboard_experience_msg,
left_right_handed_msg)
except (rospy.ServiceException, rospy.ROSException) as e:
print("Service call failed: %s" % e)
motor_cmd_scale = int((motor_volts * controller_cmd_mag) / 8.4)
motor_rate_sub = rospy.Subscriber('/mip_rover/motors/rates', MotorRate,
motor_rate_handler)
motor_cmd_pub = rospy.Publisher('/mip_rover/motors/cmd',
MotorCmd,
queue_size=10)
green_led_pub = rospy.Publisher('/mip_rover/leds/green',
Byte,
queue_size=10)
rate = rospy.Rate(4)
motor_cmd = MotorCmd()
motor_cmd.left_motor = 0
motor_cmd.right_motor = 0
green_led_cmd = Byte()
green_led_cmd.data = 0
while not rospy.is_shutdown():
print('{:d} {:d} {:f} {:f}'.format(motor_cmd.left_motor,
motor_cmd.right_motor,
current_left_motor_rate,
current_right_motor_rate))
green_led_pub.publish(green_led_cmd)
motor_cmd.header.stamp = rospy.Time.now()
motor_cmd_pub.publish(motor_cmd)
motor_cmd.left_motor += 10
motor_cmd.right_motor += 10
green_led_cmd.data = green_led_cmd.data == 0
if motor_cmd.left_motor > motor_cmd_scale or motor_cmd.right_motor > motor_cmd_scale:
break
rate.sleep()
leds = LEDs(pi)
selecter = Selecter(pi)
tirette = Tirette(pi)
bumpers = Bumpers(pi)
########################################################
def LEDsCallback(data):
leds.set(data.r, data.g, data.b)
selecterPub = rospy.Publisher('/hat/selecter', Bool, queue_size=2)
tirettePub = rospy.Publisher('/hat/tirette', Bool, queue_size=2)
bumpersPub = rospy.Publisher('/hat/bumpers', Byte, queue_size=2)
rospy.init_node('hat_driver', anonymous=False)
rospy.Subscriber('/hat/rgb', ColorRGBA, LEDsCallback)
threading.Thread(target=rospy.spin).start()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
selecterPub.publish(Bool(selecter.get()))
tirettePub.publish(Bool(tirette.get()))
bumpersPub.publish(Byte(bumpers.get()))
rate.sleep()
pi.stop()
#!/usr/bin/env python
import rospy
from std_msgs.msg import Byte
import pigpio
pub = rospy.Publisher('/ball_count', Byte, queue_size=10)
msg1 = Byte(98)
rate = rospy.Rate(30)
pi = pigpio.pi()
"""
Sensors are inversed
"""
lift_bas = 7
lift_haut = 8
lanceur = 9
c1_state = 1
c2_state = 1
count = 0
while not rospy.is_shutdown():
if pi.read(lift_bas) == 0:
c1_state = 0
elif c1_state == 0 and pi.read(lift_bas) == 1:
count += 1
c1_state = 1
def main(self):
while not rospy.is_shutdown():
rospy.spin()
self.ena_pub.publish(Byte(0))
def test_byte_msg_from_json():
msg_json = '{ "data": "\u0010" }'
parsed_msg = json_to_msg(msg_json, Byte())
assert parsed_msg.data == b'\x10'
def send_byte(self, data):
msg = Byte()
msg.data = data
self.publisher_.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg.data)
def sendTrigger(self):
b = Byte(1)
self.pub_trigger.publish(b)
def onStartTrainingClick(self):
participant_number_msg = Byte(int(self.lineEdit.text()))
self._operator_gui_interaction_pub.publish(participant_number_msg)