wheeled_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2010, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib; roslib.load_manifest('wheeled_robin_node')
00035 
00036 import os
00037 import sys
00038 import time
00039 import select
00040 from math import sin, cos
00041 
00042 import rospkg
00043 import rospy
00044 import tf
00045 
00046 from std_msgs.msg import String
00047 from geometry_msgs.msg import Point, Pose, Pose2D, PoseWithCovariance, Quaternion, Twist, TwistWithCovariance, Vector3
00048 from nav_msgs.msg import Odometry
00049 from sensor_msgs.msg import JointState
00050 
00051 from wheeled_robin_driver import WheeledRobin
00052 from wheeled_robin_node.msg import WheeledRobinSensorState
00053 from wheeled_robin_node.srv import SetWheeledRobinMode,SetWheeledRobinModeResponse, SetDigitalOutputs, SetDigitalOutputsResponse
00054 from wheeled_robin_node.diagnostics import WheeledRobinDiagnostics
00055 from wheeled_robin_node.wheeled_robin_sensor_handler import WheeledRobinSensorHandler;
00056 from create_driver import DriverError
00057 from create_node.covariances import \
00058      ODOM_POSE_COVARIANCE, ODOM_POSE_COVARIANCE2, ODOM_TWIST_COVARIANCE, ODOM_TWIST_COVARIANCE2
00059 
00060 #dynamic reconfigure
00061 import dynamic_reconfigure.server
00062 from wheeled_robin_node.cfg import WheeledRobinConfig
00063 
00064 class WheeledRobin_Node(object):
00065     _SENSOR_READ_RETRY_COUNT = 5
00066 
00067     def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=100.0):
00068 
00069         """
00070         @param default_port: default tty port to use for establishing
00071         connection to WheeledRobin. This will be overriden by ~port ROS
00072         param if available.
00073         """
00074         self.default_port = default_port
00075         self.default_update_rate = default_update_rate
00076 
00077         self.robot = WheeledRobin()
00078         self.sensor_handler = None
00079         self.sensor_state = WheeledRobinSensorState()
00080         self.req_cmd_vel = None
00081 
00082         rospy.init_node('wheeled_robin')
00083         self._init_params()
00084         self._init_pubsub()
00085         
00086         self._pos2d = Pose2D() # 2D pose for odometry
00087         self._diagnostics = WheeledRobinDiagnostics()
00088         if self.has_gyro:
00089             #from create_node.gyro import TurtlebotGyro
00090             #self._gyro = TurtlebotGyro()
00091             self._gyro = None
00092         else:
00093             self._gyro = None
00094             
00095         dynamic_reconfigure.server.Server(WheeledRobinConfig, self.reconfigure)
00096 
00097     def _init_params(self):
00098         self.port = rospy.get_param('~port', self.default_port)
00099         self.update_rate = rospy.get_param('~update_rate', self.default_update_rate)
00100         self.drive_mode = rospy.get_param('~drive_mode', 'drive')
00101         self.has_gyro = rospy.get_param('~has_gyro', False)
00102         self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
00103         self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
00104         self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.6))
00105         self.stop_motors_on_bump = rospy.get_param('~stop_motors_on_bump', True)
00106         self.min_abs_yaw_vel = rospy.get_param('~min_abs_yaw_vel', None)
00107         self.max_abs_yaw_vel = rospy.get_param('~max_abs_yaw_vel', None)
00108         self.publish_tf = rospy.get_param('~publish_tf', False)
00109         self.odom_frame = rospy.get_param('~odom_frame', 'odom')
00110         self.base_footprint_frame = rospy.get_param('~base_footprint_frame', 'base_footprint')
00111         self.base_link_frame = rospy.get_param('~base_link_frame', 'base_link')
00112         self.operate_mode = rospy.get_param('~operation_mode', 3)
00113 
00114         rospy.loginfo("serial port: %s"%(self.port))
00115         rospy.loginfo("update rate: %s"%(self.update_rate))
00116         rospy.loginfo("drive mode: %s"%(self.drive_mode))
00117         rospy.loginfo("has_gyro: %s"%(self.has_gyro))
00118 
00119     def _init_pubsub(self):
00120         self.joint_states_pub = rospy.Publisher('joint_states', JointState)
00121         self.odom_pub = rospy.Publisher('odom', Odometry)
00122 
00123         self.sensor_state_pub = rospy.Publisher('~sensor_state', WheeledRobinSensorState)
00124         self.operating_mode_srv = rospy.Service('~set_operation_mode', SetWheeledRobinMode, self.set_operation_mode)
00125         self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs)
00126 
00127         if self.drive_mode == 'direct_drive':
00128             self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel)
00129             self.drive_cmd = self.robot.direct_drive
00130         elif self.drive_mode == 'drive':
00131             self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel)
00132             self.drive_cmd = self.robot.drive
00133         else:
00134             rospy.logerr("unknown drive mode :%s"%(self.drive_mode))
00135 
00136         self.transform_broadcaster = None
00137         if self.publish_tf:
00138             self.transform_broadcaster = tf.TransformBroadcaster()
00139 
00140     def reconfigure(self, config, level):
00141         self.update_rate = config['update_rate']
00142         self.drive_mode = config['drive_mode']
00143         self.has_gyro = config['has_gyro']
00144         if self.has_gyro:
00145             self._gyro.reconfigure(config, level)
00146         self.odom_angular_scale_correction = config['odom_angular_scale_correction']
00147         self.odom_linear_scale_correction = config['odom_linear_scale_correction']
00148         self.cmd_vel_timeout = rospy.Duration(config['cmd_vel_timeout'])
00149         self.stop_motors_on_bump = config['stop_motors_on_bump']
00150         self.min_abs_yaw_vel = config['min_abs_yaw_vel']
00151         self.max_abs_yaw_vel = config['max_abs_yaw_vel']
00152         return config
00153 
00154     def start(self):
00155         log_once = True
00156         while not rospy.is_shutdown():
00157             try:
00158                 self.robot.start(self.port)
00159                 break
00160             except serial.serialutil.SerialException as ex:
00161                 msg = "Failed to open port %s. Please make sure the Wheeled_Robin cable is plugged into the computer. \n"%(self.port)
00162                 self._diagnostics.node_status(msg,"error")
00163                 if log_once:
00164                     log_once = False
00165                     rospy.logerr(msg)
00166                 else:
00167                     sys.stderr.write(msg)
00168                 time.sleep(1.0)
00169 
00170         self.sensor_handler = WheeledRobinSensorHandler(self.robot);
00171         self.robot.safe = True
00172 
00173         self.robot.control()
00174 
00175         # Startup readings from WheeledRobin can be incorrect, discard first values
00176         #s = WheeledRobinSensorState()
00177         s = self.sensor_state
00178         try:
00179             self.sense(s)
00180         except Exception:
00181             # packet read can get interrupted, restart loop to
00182             # check for exit conditions
00183             pass
00184 
00185     def spin(self):
00186 
00187         # state
00188         s = self.sensor_state
00189         odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_footprint_frame)
00190         js = JointState(name = ["left_wheel_joint", "right_wheel_joint"], position=[0,0], velocity=[0,0], effort=[0,0])
00191 
00192         r = rospy.Rate(self.update_rate)
00193         last_cmd_vel = 0, 0
00194         last_cmd_vel_time = rospy.get_rostime()
00195         last_js_time = rospy.Time(0)
00196         # We set the retry count to 0 initially to make sure that only
00197         # if we received at least one sensor package, we are robust
00198         # agains a few sensor read failures. For some strange reason,
00199         # sensor read failures can occur when switching to full mode
00200         # on the Roomba.
00201         sensor_read_retry_count = 0
00202 
00203 
00204         while not rospy.is_shutdown():
00205             last_time = s.header.stamp
00206             curr_time = rospy.get_rostime()
00207 
00208             # SENSE/COMPUTE STATE
00209             try:
00210                 self.sense(s)
00211                 transformFootprintBase = self.compute_odom(s, last_time, odom)
00212                 # Future-date the joint states so that we don't have
00213                 # to publish as frequently.
00214                 js.header.stamp = curr_time + rospy.Duration(1)
00215             except select.error:
00216                 # packet read can get interrupted, restart loop to
00217                 # check for exit conditions
00218                 continue
00219 
00220             except DriverError:
00221                 if sensor_read_retry_count > 0:
00222                     rospy.logwarn('Failed to read sensor package. %d retries left.' % sensor_read_retry_count)
00223                     sensor_read_retry_count -= 1
00224                     continue
00225                 else:
00226                     raise
00227             sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT
00228 
00229             # PUBLISH STATE
00230             self.sensor_state_pub.publish(s)
00231             self.odom_pub.publish(odom)
00232             if self.publish_tf:
00233                 self.publish_odometry_transform(odom)
00234                 self.transform_broadcaster.sendTransform((0,0,0.055), transformFootprintBase, odom.header.stamp, self.base_link_frame, self.base_footprint_frame)
00235                 
00236             # publish future-dated joint state at 1Hz
00237             if curr_time > last_js_time + rospy.Duration(1):
00238                 self.joint_states_pub.publish(js)
00239                 last_js_time = curr_time
00240             self._diagnostics.publish(s, self._gyro)
00241             #if self._gyro:
00242             #    self._gyro.publish(s, last_time)
00243 
00244             # WRITE DESIRED VELOCITY
00245             if self.req_cmd_vel is not None:
00246                 # check for velocity command and set the robot into full mode if necessary
00247                 if s.mode != self.operate_mode:
00248                     if self.operate_mode == 2:
00249                         self._robot_run_safe()
00250                     else:
00251                         self._robot_run_full()
00252 
00253                 # do some checks on the velocity : TODO
00254                 req_cmd_vel = self.req_cmd_vel
00255 
00256                 # Set to None so we know it's a new command
00257                 self.req_cmd_vel = None
00258                 # reset time for timeout
00259                 last_cmd_vel_time = last_time
00260 
00261             else:
00262                 #zero commands on timeout
00263                 if last_time - last_cmd_vel_time > self.cmd_vel_timeout:
00264                     last_cmd_vel = 0,0
00265                     #rospy.loginfo("A Timout in cmd_vel occured")
00266 
00267                 # do some checks on the velocity : TODO  --> check bumpers
00268                 req_cmd_vel = last_cmd_vel
00269 
00270             # send command
00271             self.drive_cmd(*req_cmd_vel)
00272             # record command
00273             last_cmd_vel = req_cmd_vel
00274 
00275             r.sleep()
00276 
00277     def _robot_run_passive(self):
00278         """
00279         Set WheeldRobin into passive run mode
00280         """
00281         rospy.loginfo("Setting WheeldRobin to passive mode.")
00282         #setting all the digital outputs to 0
00283         #self._set_digital_outputs([0, 0, 0, 0, 0, 0, 0, 0])
00284         self.robot.passive()
00285 
00286     def _robot_run_safe(self):
00287         """
00288         Set WheeldRobin into safe run mode
00289         """
00290         rospy.loginfo("Setting WheeldRobin to safe mode.")
00291         self.robot.safe = True
00292         self.robot.control()
00293         b1 = (self.sensor_state.digital_inputs & 2)/2
00294         b2 = (self.sensor_state.digital_inputs & 4)/4
00295         #self._set_digital_outputs([1, b1, b2, 0, 0, 0, 0, 0])
00296 
00297     def _robot_run_full(self):
00298         """
00299         Set WheeldRobin into full run mode
00300         """
00301         rospy.loginfo("Setting WheeldRobin to full mode.")
00302         self.robot.safe = False
00303         self.robot.control()
00304         b1 = (self.sensor_state.digital_inputs & 2)/2
00305         b2 = (self.sensor_state.digital_inputs & 4)/4
00306         #self._set_digital_outputs([1, b1, b2, 0, 0, 0, 0, 0])
00307 
00308     def _set_digital_outputs(self, outputs):
00309         assert len(outputs) == 8, 'Expecting 8 output states.'
00310         byte = 0
00311         for output, state in enumerate(outputs):
00312             byte += (2 ** output) * int(state)
00313         self.robot.set_digital_outputs(byte)
00314         #self.sensor_state.digital_outputs = byte
00315 
00316     def set_digital_outputs(self,req):
00317         if not self.robot.sci:
00318             rospy.logwarn("WheeledRobin: robot not connected yet!")
00319             return SetDigitalOutputsResponse(False)
00320             
00321         outputs = [req.digital_out_0,req.digital_out_1, req.digital_out_2, req.digital_out_3, req.digital_out_4, req.digital_out_5, req.digital_out_6, req.digital_out_7]
00322         self._set_digital_outputs(outputs)
00323         return SetDigitalOutputsResponse(True)
00324         
00325     def set_operation_mode(self,req):
00326         if not self.robot.sci:
00327             rospy.logwarn("WheeledRobin: robot not connected yet!")
00328             return SetTurtlebotModeResponse(False)
00329 
00330         self.operate_mode = req.mode
00331 
00332         if req.mode == 1: #passive
00333             self._robot_run_passive()
00334         elif req.mode == 2: #safe
00335             self._robot_run_safe()
00336         elif req.mode == 3: #full
00337             self._robot_run_full()
00338         else:
00339             rospy.logerr("Requested an invalid mode.")
00340             return SetTurtlebotModeResponse(False)
00341         return SetTurtlebotModeResponse(True)
00342 
00343     def sense(self, sensor_state):
00344         #sensor_state.mode = 3
00345         self.sensor_handler.get_all(sensor_state)
00346         #if self._gyro:
00347         #    self._gyro.update_calibration(sensor_state)
00348         pass
00349 
00350     def cmd_vel(self, msg):
00351         # Clamp to min abs yaw velocity, to avoid trying to rotate at low speeds, which doesn't work well.
00352         if self.min_abs_yaw_vel is not None and msg.angular.z != 0.0 and abs(msg.angular.z) < self.min_abs_yaw_vel:
00353             msg.angular.z = self.min_abs_yaw_vel if msg.angular.z > 0.0 else -self.min_abs_yaw_vel
00354         # Limit maximum yaw
00355         if self.max_abs_yaw_vel is not None and self.max_abs_yaw_vel > 0.0 and msg.angular.z != 0.0 and abs(msg.angular.z) > self.max_abs_yaw_vel:
00356             msg.angular.z = self.max_abs_yaw_vel if msg.angular.z > 0.0 else -self.max_abs_yaw_vel
00357 
00358         if self.drive_mode == 'direct_drive':
00359             # TODO 
00360             # convert twist to direct_drive args
00361             #ts = msg.linear.x * 1000 # m -> mm
00362             #tw = msg.angular.z * (robot_types.ROBOT_TYPES[self.robot_type].wheel_separation / 2) * 1000
00363             # Prevent saturation at max wheel speed when a compound command is sent.
00364             #if ts > 0:
00365             #    ts = min(ts, MAX_WHEEL_SPEED - abs(tw))
00366             #else:
00367             #    ts = max(ts, -(MAX_WHEEL_SPEED - abs(tw)))
00368             #self.req_cmd_vel = int(ts - tw), int(ts + tw)
00369             self.req_cmd_vel = msg.linear.x * 1000, msg.angular.z * 1000
00370         elif self.drive_mode == 'drive':
00371             # convert twist to drive args, m->mm (velocity_x, velocity_g)
00372             self.req_cmd_vel = msg.linear.x * 1000, msg.angular.z * 1000
00373 
00374     def compute_odom(self, sensor_state, last_time, odom):
00375         """
00376         Compute current odometry.  Updates odom instance and returns tf
00377         transform. compute_odom() does not set frame ids or covariances in
00378         Odometry instance.  It will only set stamp, pose, and twist.
00379 
00380         @param sensor_state: Current sensor reading
00381         @type  sensor_state: WheeledRobinSensorState
00382         @param last_time: time of last sensor reading
00383         @type  last_time: rospy.Time
00384         @param odom: Odometry instance to update.
00385         @type  odom: nav_msgs.msg.Odometry
00386 
00387         @return: transformFootprintBase
00388         @rtype: (float, float, float, float)
00389         """
00390 
00391         current_time = sensor_state.header.stamp
00392         dt = (current_time - last_time).to_sec()
00393 
00394         delta_x = sensor_state.linear_velocity * cos(self._pos2d.theta) * dt;
00395         delta_y = sensor_state.linear_velocity * sin(self._pos2d.theta) * dt;
00396         delta_th = sensor_state.angular_velocity  * dt;
00397         
00398         self._pos2d.x += delta_x
00399         self._pos2d.y += delta_y
00400         self._pos2d.theta += delta_th
00401 
00402         # Turtlebot quaternion from yaw. simplified version of tf.transformations.quaternion_about_axis
00403         odom_quat = (0., 0., sin(self._pos2d.theta/2.), cos(self._pos2d.theta/2.))
00404 
00405         # construct the transform
00406         # transform = (self._pos2d.x, self._pos2d.y, 0.), odom_quat
00407 
00408         # update the odometry state
00409         odom.header.stamp = current_time
00410         odom.pose.pose   = Pose(Point(self._pos2d.x, self._pos2d.y, 0.), Quaternion(*odom_quat))
00411         odom.twist.twist = Twist(Vector3(sensor_state.linear_velocity, 0, 0), Vector3(0, 0, sensor_state.angular_velocity))
00412         #if sensor_state.requested_right_velocity == 0 and \
00413         #       sensor_state.requested_left_velocity == 0 and \
00414         #       sensor_state.distance == 0:
00415         #    odom.pose.covariance = ODOM_POSE_COVARIANCE2
00416         #odom.twist.covariance = ODOM_TWIST_COVARIANCE2
00417         #else:
00418         #odom.pose.covariance = ODOM_POSE_COVARIANCE
00419         #    odom.twist.covariance = ODOM_TWIST_COVARIANCE
00420 
00421         odom.pose.covariance = ODOM_POSE_COVARIANCE
00422         odom.twist.covariance = ODOM_TWIST_COVARIANCE
00423         
00424         # construct the transform from footprint to base
00425         base_quat = (0., sin(sensor_state.pitch/2.), 0., cos(sensor_state.pitch/2.))
00426         transformFootprintBase = base_quat
00427 
00428         return transformFootprintBase
00429 
00430     def publish_odometry_transform(self, odometry):
00431         self.transform_broadcaster.sendTransform(
00432             (odometry.pose.pose.position.x, odometry.pose.pose.position.y, odometry.pose.pose.position.z),
00433             (odometry.pose.pose.orientation.x, odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z, odometry.pose.pose.orientation.w),
00434              odometry.header.stamp, odometry.child_frame_id, odometry.header.frame_id)
00435 
00436 def wheeled_robin_main(argv):
00437     node = WheeledRobin_Node()
00438     while not rospy.is_shutdown():
00439         try:
00440             # This sleep throttles reconnecting of the driver. It
00441             # appears that pyserial does not properly release the file
00442             # descriptor for the USB port in the event that the Create is
00443             # unplugged from the laptop. This file desecriptor prevents
00444             # the create from reassociating with the same USB port when it
00445             # is plugged back in. The solution, for now, is to quickly
00446             # exit the driver and let roslaunch respawn the driver until
00447             # reconnection occurs. However, it order to not do bad things
00448             # to the Create bootloader, and also to keep relaunching at a
00449             # minimum, we have a 3-second sleep.
00450             time.sleep(1.0)
00451             
00452             node.start()
00453             node.spin()
00454 
00455         except Exception as ex:
00456             msg = "Failed to contact device with error: [%s]. Please check that the Wheeled_Robin is powered on and that the connector is plugged into the Wheeled_Robin."%(ex)
00457             node._diagnostics.node_status(msg,"error")
00458             rospy.logerr(msg)
00459 
00460         finally:
00461             try: pass
00462             except Exception: pass
00463 
00464 
00465 if __name__ == '__main__':
00466     wheeled_robin_main(sys.argv)


wheeled_robin_node
Author(s): Johannes Mayr , Klemens Springer
autogenerated on Fri Aug 28 2015 13:39:00