def pub():
posi_pub = rospy.Publisher('posi_pub', posi, queue_size=5)
rospy.init_node('posi_publisher', anonymous=False)
tr = trans()
rb_s = rb()
my_log = log()
my_log.loginfo("Posi_publisher: Posi Publisher Initialized!")
rate = rospy.Rate(pub_rate)
while not rospy.is_shutdown():
connected = bool(rospy.get_param("connected"))
if connected:
try:
msg = tr.get_msg()
p = tr.get_pose(msg)
except (rospy.exceptions.ROSException, tf.LookupException,
tf.ConnectivityException, tf.ExtrapolationException) as e:
my_log.logerr(
"Posi_publisher: Cannot get the position!\nDetails: %s", e)
my_log.loginfo("Posi_publisher: Wait 10 seconds")
rospy.sleep(10)
continue
posi_pub.publish(msg.header.stamp.to_sec(), p[0], p[1], p[2], p[3],
p[4])
rate.sleep()
def __init__(self):
rospy.init_node('navi_node', anonymous=False)
self.map_loaded = False
self.g = None
self.rb_s = rb()
self.exp_s = exp()
self.trans = trans()
self.my_log = log()
self.p_user = self.get_user_posi()
self.move_cmd_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.move_cmd = Twist()
self.path_pub = rospy.Publisher('path', path_now, queue_size=10)
self.res_to_ctrl_pub = rospy.Publisher('response_to_ctrl',
enum_type,
queue_size=10)
self.response_pub = rospy.Publisher('response',
response,
queue_size=10)
rospy.Subscriber('virtual_map', vmap, self.load_map)
rospy.Subscriber('path_ori', path_ori, self.receieve_path)
rospy.Subscriber('dst', point_2d, self.receieve_dst)
rospy.Subscriber('voice_cmd', voice_cmd, self.receive_voice)
self.my_log.loginfo("Navi: Navigation Node Initialized!")
return
def init():
rospy.init_node('voice_pub', anonymous = False)
global voice_pub
voice_pub = rospy.Publisher('voice_cmd', voice_cmd, queue_size = 1)
global my_log
my_log = log(if_pub = False)
rospy.Subscriber('xfspeech', String, receive_voice)
my_log.loginfo("Voice_pub: Voice Publisher Initialized!")
def __init__(self):
rospy.init_node('vmap_broadcaster', anonymous=False)
self.response_pub = rospy.Publisher('response',
response,
queue_size=10)
rospy.Subscriber('mark_vmap', String, self.mark_vmap)
self.t = geometry_msgs.msg.TransformStamped()
self.marked = False
self.my_log = log()
return
def __init__(self):
self.res = Queue()
rospy.init_node('test', anonymous=False)
self.rb_s = status.rb()
self.exp_s = status.exp()
self.tr = trans()
self.my_log = log()
self.voice_pub = rospy.Publisher('xfspeech', String, queue_size=10)
self.path_pub = rospy.Publisher('path', path, queue_size=10)
self.path_ori_pub = rospy.Publisher('path_ori',
path_ori,
queue_size=10)
self.map_pub = rospy.Publisher('virtual_map', vmap, queue_size=5)
self.dst_pub = rospy.Publisher('dst', point_2d, queue_size=5)
self.mark_pub = rospy.Publisher('mark_vmap', String, queue_size=5)
self.stop_pub = rospy.Publisher('stop_exp', stop, queue_size=2)
rospy.Subscriber('response', response, self.get_response)
self.m = vmap()
self.m.w = 300
self.m.h = 400
self.m.obj.append(map_object(0, 13, 114, 30, 110))
self.m.obj.append(map_object(0, 163, 350, 35, 30))
self.m.obj.append(map_object(0, 253, 360, 40, 40))
self.m.obj.append(map_object(0, 273, 190, 15, 120))
self.m.obj.append(map_object(0, 50, 290, 30, 130))
self.m.obj.append(map_object(1, 142, 253, 33, 50))
self.m.obj.append(map_object(1, 130, 113, 50, 50))
self.m.obj.append(map_object(1, 250, 50, 23, 50))
self.m_cplx = vmap()
self.m_cplx.w = 300
self.m_cplx.h = 400
self.m_cplx.obj.append(map_object(0, 13, 114, 30, 110))
self.m_cplx.obj.append(map_object(0, 163, 350, 35, 30))
self.m_cplx.obj.append(map_object(0, 253, 360, 40, 40))
self.m_cplx.obj.append(map_object(0, 273, 190, 15, 120))
self.m_cplx.obj.append(map_object(0, 50, 290, 30, 130))
self.m_cplx.obj.append(map_object(0, 50, 310, 120, 40))
self.m_cplx.obj.append(map_object(1, 142, 253, 33, 50))
self.m_cplx.obj.append(map_object(1, 130, 113, 50, 50))
self.m_cplx.obj.append(map_object(1, 250, 50, 23, 50))
self.m_cplx.obj.append(map_object(1, 150, 390, 20, 50))
self.my_log.loginfo("Test: Test Node Initialized!")
return
def __init__(self):
rospy.init_node('communicater', anonymous = False)
# Members
self.rb_s = rb()
self.exp_s = exp()
self.my_log = log()
#Publisher and Subscriber
self.map_pub = rospy.Publisher('virtual_map', vmap, queue_size = 5)
self.drive_pub = rospy.Publisher('drive_cmd', enum_type, queue_size = 10)
self.stop_pub = rospy.Publisher('stop_exp', stop, queue_size = 2)
# TEST_MODE and set the IP and Port of other port
global TEST_MODE
TEST_MODE = bool(rospy.get_param("TEST_MODE"))
if not TEST_MODE:
try:
self.set_IP()
except Exception as e:
self.my_log.logerr("Cannot set the IP and Port of other ports!\nDetails: %s", e)
rospy.Subscriber('posi_pub', posi, self.send_rb_posi)
rospy.Subscriber('path_ori', path_ori, self.send_rb_path_ori)
rospy.Subscriber('path', path, self.send_rb_path)
rospy.Subscriber('voice_cmd', voice_cmd, self.send_rb_voice_cmd)
rospy.Subscriber('response_to_ctrl', enum_type, self.send_response_to_ctrl)
rospy.Subscriber('log_msg', String, self.send_log_msg)
# Start the msg server
receive_srv = msg_server.RobotServicer(self.receive_map, self.receive_command, self.receive_voice, self.receive_drive_command)
try:
thread.start_new_thread(msg_server.serve, (receive_srv, ))
self.my_log.loginfo("Communicater: Receiving service Initialized!")
except:
self.my_log.logerr("Communicater: Unable to start the receiving service thread!")
self.init_ok = False
return
self.my_log.loginfo("Communicater: Communicate Node Initialized!")
self.init_ok = True
return
def __init__(self):
rospy.init_node('drive', anonymous=False)
self.rb_s = rb()
self.exp_s = exp()
self.my_log = log()
self.move_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.response_pub = rospy.Publisher('response',
response,
queue_size=10)
self.move_cmd = Twist()
rospy.Subscriber('voice_cmd', voice_cmd, self.receive_voice)
rospy.Subscriber('drive_cmd', enum_type, self.receive_drive_cmd)
rospy.Subscriber('stop_exp', stop, self.stop_exp)
self.is_spin = False
self.my_log.loginfo("Drive: Drive Node Initialized!")
return
def __init__(self):
rospy.init_node('gesture', anonymous=False)
self.my_log = log()
self.exp = exp()
self.is_color_cemera_set = False
self.is_depth_cemera_set = False
self.color_image = None
self.depth_image = None
self.depth_point_cloud = None
rospy.Subscriber('voice_cmd', voice_cmd, self.receive_voice)
self.dst_pub = rospy.Publisher('dst', point_2d, queue_size=1)
self.response_pub = rospy.Publisher('response',
response,
queue_size=10)
global TEST_MODE
TEST_MODE = bool(rospy.get_param("TEST_MODE"))
if TEST_MODE:
path = rospy.get_param("pkg_path") + "/data/test"
self.save_data = recognition_data(path)
(depth_camera, color_camera) = self.save_data.read_camera_info()
self.receive_depth_camera_info(depth_camera)
self.receive_color_camera_info(color_camera)
(depth_image, color_image,
frame) = self.save_data.read_recognition_data()
self.receive_depth_image(depth_image)
self.receive_color_image(color_image)
self.test_frame = frame
else:
if rospy.has_param("pkg_path"):
path = rospy.get_param("pkg_path")
else:
path = os.path.dirname(os.path.realpath(__file__))
path += "/data"
if not os.path.exists(path):
os.makedirs(path)
path += "/" + rospy.get_param("start_time")
if not os.path.exists(path):
os.makedirs(path)
self.save_data = recognition_data(path)
self.gesture_recognizer = gesture_recognize(self.my_log)
self.color_image_pub = rospy.Publisher(
'/camera/color/image_raw/my_pub', Image, queue_size=1)
self.depth_image_pub = rospy.Publisher(
'/camera/depth/image_raw/my_pub', Image, queue_size=1)
rospy.Subscriber('/camera/color/image_raw', Image,
self.receive_color_image)
rospy.Subscriber('/camera/depth/image_raw', Image,
self.receive_depth_image)
self.dp_info_sub = rospy.Subscriber('/camera/depth/camera_info',
CameraInfo,
self.receive_depth_camera_info)
self.cl_info_sub = rospy.Subscriber('/camera/color/camera_info',
CameraInfo,
self.receive_color_camera_info)
rospy.Subscriber('/frame', Frame, self.receive_frame)
self.my_log.loginfo("Gesture: Gesture Node Initialized!")