base.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 
00004 # ROS stuff
00005 import roslib; roslib.load_manifest('clearpath_base')
00006 import rospy
00007 
00008 from clearpath_base.msg import ClearpathRobot
00009 from geometry_msgs.msg import Twist
00010 from std_msgs.msg import String, Float32
00011 
00012 # Required Clearpath Modules
00013 from clearpath.horizon import Horizon 
00014 from clearpath.horizon import transports 
00015 from clearpath import utils  
00016 import data
00017 
00018 # Standard 
00019 import sys
00020 import logging
00021 from threading import Event
00022 
00023 
00024 CMD_TIME_MAX = 0.12
00025 CMD_TIME_MIN = 0.02
00026 RECONNECT_TIMEOUT = 1.0
00027 DATA_TIMEOUT = 1.1
00028 
00029 
00030 class Clearpath:
00031     def __init__(self):
00032         rospy.init_node('clearpath_base')
00033         self.port = rospy.get_param('~port', '')
00034         self.cmd_fill = rospy.get_param('~cmd_fill', True)
00035 
00036         if self.port != '':
00037             # Serial port 
00038             rospy.loginfo("Using port %s", self.port)
00039             self.horizon = Horizon(transport = transports.Serial, 
00040                                    transport_args = { 'port': self.port })
00041         else:
00042             # Not specified. Autodetect.
00043             rospy.loginfo("Using port autodetection")
00044             self.horizon = Horizon(transport = transports.Serial.autodetect)
00045 
00046         announce_pub = rospy.Publisher('/clearpath/announce/robots', String, latch=True)
00047         announce_pub.publish(rospy.get_namespace());
00048 
00049         self.freq_pub = rospy.Publisher('cmd_freq', Float32, latch=True)
00050         self.cmd_sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_handler)
00051 
00052         self.cmd_msg = Twist()
00053         self.cmd_time = rospy.Time.now()
00054         self.cmd_event = Event()
00055 
00056 
00057     def run(self):
00058         previous_error = False
00059         cmd_timeout = False
00060 
00061         # Reconnection loop.
00062         while not rospy.is_shutdown():
00063             if previous_error:
00064                 rospy.sleep(RECONNECT_TIMEOUT)
00065                 
00066             try:
00067                 if self.horizon.opened:
00068                     self.horizon.close()
00069                 self.horizon.open()
00070                 self.unsub_all()
00071                 rospy.loginfo("Connection successful on %s", self.horizon)
00072                 previous_error = False
00073                  
00074                 # Fetch and publish robot information.
00075                 self.horizon.acks(True);
00076                 platform_name = self.horizon.request_platform_name(subscription=0)
00077                 platform_info = self.horizon.request_platform_info(subscription=0)
00078                 firmware_info = self.horizon.request_firmware_info(subscription=0)
00079                 robot_msg = data.pkg_robot_info(platform_name, platform_info, firmware_info)
00080                 robot_pub = rospy.Publisher('robot', ClearpathRobot, latch=True)
00081                 robot_pub.publish(robot_msg);
00082 
00083                 self.do_subscriptions()
00084                 self.horizon.acks(False);
00085 
00086                 while not rospy.is_shutdown():
00087                     self.cmd_event.wait(CMD_TIME_MAX)
00088 
00089                     if (rospy.Time.now() - self.cmd_time).to_sec() > CMD_TIME_MAX:
00090                         # Timed out waiting on command message. Send zeros instead to
00091                         # keep connection alive.
00092                         self.cmd_msg = Twist()
00093                         if not cmd_timeout:
00094                             rospy.loginfo("User commands timed out.")
00095                             cmd_timeout = True
00096                     else:
00097                         if cmd_timeout:
00098                             rospy.loginfo("Receiving user commands.")
00099                             cmd_timeout = False
00100 
00101                     self.cmd_event.clear()
00102                     
00103                     try:
00104                         if not cmd_timeout or self.cmd_fill:
00105                             self.cmd_vel(self.cmd_msg.linear.x, self.cmd_msg.angular.z)
00106                         rospy.sleep(CMD_TIME_MIN)
00107                     except: 
00108                         rospy.logerr("Problem issuing command to platform. Attempting reconnection.")
00109                         break
00110 
00111                     if (rospy.Time.now() - self.data_time).to_sec() > DATA_TIMEOUT:
00112                         rospy.logerr("Problem receiving data from platform. Attempting reconnection.")
00113                         break
00114 
00115 
00116             except utils.TransportError:
00117                 if not previous_error:
00118                     rospy.logerr("Connection error on %s. Will retry every second.", self.port if self.port != '' else '/dev/ttyUSB* or /dev/ttyS*')
00119                     previous_error = True
00120 
00121         self.horizon.close()
00122 
00123 
00124     def cmd_vel_handler(self, msg):
00125         """ Received command from ROS. Signal the main thread to send it to robot. """
00126         last_cmd_time = self.cmd_time
00127 
00128         self.cmd_msg = msg
00129         self.cmd_time = rospy.Time.now()
00130         self.cmd_event.set()
00131 
00132         self.freq_pub.publish((last_cmd_time - self.cmd_time).to_sec())
00133 
00134 
00135     def cmd_vel(self, linear_velocity, angular_velocity):
00136         rospy.logerror('Base class contains no command model. Use kinematic.py or raw.py.')
00137         raise NotImplementedError()
00138 
00139 
00140     def unsub_all(self):
00141         for topic, cls in data.msgs.items():
00142             subscribe_func = getattr(self.horizon, 'request_' + topic)
00143             try:
00144                 subscribe_func(0xffff)
00145             except:
00146                 pass
00147 
00148 
00149     def do_subscriptions(self):
00150         self.publishers = {}
00151         for topic, frequency in rospy.get_param('~data', {}).items():
00152             if not topic in self.publishers:
00153                 self.publishers[topic] = rospy.Publisher('data/' + topic, 
00154                                                          data.msgs[topic],
00155                                                          latch=True)
00156             subscribe_func = getattr(self.horizon, 'request_' + topic)
00157             subscribe_func(frequency)
00158             rospy.loginfo("Successfully returning data: request_%s", topic)
00159 
00160         self.horizon.add_handler(self._receive)
00161         self.data_time = rospy.Time.now()
00162 
00163 
00164     def _receive(self, name, payload, timestamp):
00165         self.data_time = rospy.Time.now()
00166 
00167         try:
00168             pkg_func = getattr(data, 'pkg_' + name)
00169         except AttributeError:
00170             rospy.loginfo("Unhandled Clearpath message of type: %s", name)
00171         else:
00172             msg = pkg_func(payload)
00173             msg.header.stamp = rospy.Time.now()
00174             if name in self.publishers:
00175                 self.publishers[name].publish(msg)
00176             else:
00177                 rospy.loginfo("Unsubscribed Clearpath message of type: %s", name)


clearpath_base
Author(s): Mike Purvis
autogenerated on Sun Oct 5 2014 22:52:08