Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 __author__ = "mferguson@willowgarage.com (Michael Ferguson)"
00037
00038 import rospy
00039 from rosserial_python import SerialClient, RosSerialServer
00040 import multiprocessing
00041
00042 import sys
00043
00044 if __name__=="__main__":
00045
00046 rospy.init_node("serial_node")
00047 rospy.loginfo("ROS Serial Python Node")
00048
00049 port_name = rospy.get_param('~port','/dev/ttyUSB0')
00050 baud = int(rospy.get_param('~baud','57600'))
00051
00052
00053 tcp_portnum = int(rospy.get_param('/rosserial_embeddedlinux/tcp_port', '11411'))
00054 fork_server = rospy.get_param('/rosserial_embeddedlinux/fork_server', False)
00055
00056
00057 sys.argv = rospy.myargv(argv=sys.argv)
00058 if len(sys.argv) == 2 :
00059 port_name = sys.argv[1]
00060 if len(sys.argv) == 3 :
00061 tcp_portnum = int(sys.argv[2])
00062
00063 if port_name == "tcp" :
00064 server = RosSerialServer(tcp_portnum, fork_server)
00065 rospy.loginfo("Waiting for socket connections on port %d" % tcp_portnum)
00066 try:
00067 server.listen()
00068 except KeyboardInterrupt:
00069 rospy.loginfo("got keyboard interrupt")
00070 finally:
00071 rospy.loginfo("Shutting down")
00072 for process in multiprocessing.active_children():
00073 rospy.loginfo("Shutting down process %r", process)
00074 process.terminate()
00075 process.join()
00076 rospy.loginfo("All done")
00077
00078 else :
00079 rospy.loginfo("Connecting to %s at %d baud" % (port_name,baud) )
00080 client = SerialClient(port_name, baud)
00081 try:
00082 client.run()
00083 except KeyboardInterrupt:
00084 pass
00085