def rmp_thread():
"""
Create and response and command queue. The responses will be in the form of
a dictionary containing the vaiable name as the key and a converted value
the names are defined in the feedback_X_bitmap_menu_items dictionaries if a particular
variable is of interest
"""
rsp_queue = Queue.Queue()
cmd_queue = Queue.Queue()
in_flags = Queue.Queue()
out_flags = Queue.Queue()
"""
Create the thread to run RMP
"""
my_thread = threading.Thread(target=RMP,
args=(rmp_addr, rsp_queue, cmd_queue,
in_flags, out_flags, UPDATE_DELAY_SEC,
LOG_DATA))
my_thread.daemon = True
my_thread.start()
"""
Initialize my event handler class
"""
EventHandler = RMPEventHandlers(cmd_queue, rsp_queue, in_flags)
"""
-------------------------------------------------------------------------------
User loop starts here modify to make it do what you want.
You can pipe std_in from another application to the command queue and the response to std out or
let the event handlers define everything. That is up to the user. In this example we transition modes,
send motion commands (zeroed), play audio songs, and print the response dictionary. The application
terminates the thread and exits when all the songs have been played. It is just an example of how to
spawn a RMP thread, handle events, and send/receive data
-------------------------------------------------------------------------------
"""
"""
Generate a goto tractor event
"""
EventHandler.GotoTractor()
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
while (True == EventHandler._continue):
while not out_flags.empty():
EventHandler.handle_event[out_flags.get()]()
print("stopped")
#alive=threading.active_count()
#rospy.loginfo("alive thread num: %d"%alive) //when init the node alive thread is 4
#setup communication thread
rsp_queue = multiprocessing.Queue()
cmd_queue = multiprocessing.Queue()
in_flags = multiprocessing.Queue()
out_flags = multiprocessing.Queue()
my_thread = threading.Thread(target=RMP,
args=(rmp_addr, rsp_queue, cmd_queue,
in_flags, out_flags, UPDATE_DELAY_SEC,
LOG_DATA))
my_thread.setDaemon(True)
my_thread.start()
#svreate event handler and set to BALANCE MODE
EventHandler = RMPEventHandlers(cmd_queue, rsp_queue, in_flags)
#EventHandler.GotoTractor()
#go to BALANCE
EventHandler.GotoBalance()
rospy.loginfo("finish initialize!")
# Create the NodeName object
node = rmp_base()
while my_thread.isAlive():
pass
# Setup proper shutdown behavior
rospy.on_shutdown(node.on_shutdown)
#rospy.loginfo("alive thread num: %d"%alive) //when init the node alive thread is 4
#setup communication thread
rsp_queue = multiprocessing.Queue()
cmd_queue = multiprocessing.Queue()
in_flags = multiprocessing.Queue()
out_flags = multiprocessing.Queue()
vel = TwistStamped()
my_thread = threading.Thread(target=RMP,
args=(rmp_addr, rsp_queue, cmd_queue,
in_flags, out_flags, UPDATE_DELAY_SEC,
LOG_DATA))
my_thread.setDaemon(True)
my_thread.start()
#svreate event handler and set to BALANCE MODE
EventHandler = RMPEventHandlers(cmd_queue, rsp_queue, in_flags)
#EventHandler.GotoTractor()
#go to BALANCE
EventHandler.GotoBalance()
rospy.loginfo("finish initialize!")
# Create the NodeName object
node = rmp_base(vel)
vel.twist.linear.x = 0.0
vel.twist.angular.z = 0.0
while my_thread.isAlive():
EventHandler.Send_MotionCmd(vel.twist.linear.x, vel.twist.angular.z)
#rospy.loginfo("Send velocity(%.2f, %.2f)"% (vel.twist.linear.x, vel.twist.angular.z))