def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=120.0): """ @param default_port: default tty port to use for establishing connection to Eddiebot. This will be overriden by ~port ROS param if available. """ self.default_port = default_port self.default_update_rate = default_update_rate self.robot = Eddiebot() self.sensor_handler = None self.sensor_state = EddiebotSensorState() self.req_cmd_vel = None rospy.init_node('eddiebot') self._init_params() self._init_pubsub() self._pos2d = Pose2D() # 2D pose for odometry self._diagnostics = EddiebotDiagnostics() if self.has_gyro: self._gyro = EddiebotGyro() else: self._gyro = None dynamic_reconfigure.server.Server(EddieBotConfig, self.reconfigure) self.last_pan_degree = 96 # Default is facing center
def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=120.0): """ @param default_port: default tty port to use for establishing connection to Eddiebot. This will be overriden by ~port ROS param if available. """ self.default_port = default_port self.default_update_rate = default_update_rate self.robot = Eddiebot() self.sensor_handler = None self.sensor_state = EddiebotSensorState() self.req_cmd_vel = None rospy.init_node('eddiebot') self._init_params() self._init_pubsub() self._pos2d = Pose2D() # 2D pose for odometry self._diagnostics = EddiebotDiagnostics() if self.has_gyro: self._gyro = EddiebotGyro() else: self._gyro = None dynamic_reconfigure.server.Server(EddieBotConfig, self.reconfigure) self.last_pan_degree = 96 # Default is facing center self.last_angle = self.UNDEFINED self.last_dist = self.UNDEFINED self.emergency_activated = False self.last_time = 0 self.cmd_log = open( '/home/eddie/logs/cmd_log_' + str(int(time.time())), 'w') self.cmd_log.write( 'cur_time,msg.linear.x,msg.angular.z,linear_speed_ticks,angular_speed_ticks,left_spd,right_spd\n' )
class EddiebotNode(object): _SENSOR_READ_RETRY_COUNT = 5 def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=120.0): """ @param default_port: default tty port to use for establishing connection to Eddiebot. This will be overriden by ~port ROS param if available. """ self.default_port = default_port self.default_update_rate = default_update_rate self.robot = Eddiebot() self.sensor_handler = None self.sensor_state = EddiebotSensorState() self.req_cmd_vel = None rospy.init_node('eddiebot') self._init_params() self._init_pubsub() self._pos2d = Pose2D() # 2D pose for odometry self._diagnostics = EddiebotDiagnostics() if self.has_gyro: self._gyro = EddiebotGyro() else: self._gyro = None dynamic_reconfigure.server.Server(EddieBotConfig, self.reconfigure) self.last_pan_degree = 96 # Default is facing center def start(self): log_once = True while not rospy.is_shutdown(): try: self.robot.start(self.port, robot_types.ROBOT_TYPES[self.robot_type].baudrate) break except serial.serialutil.SerialException as ex: msg = "Failed to open port %s. Please make sure the Create cable is plugged into the computer. \n"%(self.port) self._diagnostics.node_status(msg,"error") if log_once: log_once = False rospy.logerr(msg) else: sys.stderr.write(msg) # time.sleep(3.0) self.sensor_handler = robot_types.ROBOT_TYPES[self.robot_type].sensor_handler(self.robot) self.robot.safe = True # if rospy.get_param('~bonus', False): # bonus(self.robot) self.robot.control() # Write driver state to disk with open(connected_file(), 'w') as f: f.write("1") # Disable all sensors Startup readings from Create can be incorrect, discard first values s = EddiebotSensorState() try: self.sense(s) except Exception: # packet read can get interrupted, restart loop to # check for exit conditions pass def _init_params(self): self.port = rospy.get_param('~port', self.default_port) self.robot_type = rospy.get_param('~robot_type', 'eddie') #self.baudrate = rospy.get_param('~baudrate', self.default_baudrate) self.update_rate = rospy.get_param('~update_rate', self.default_update_rate) self.drive_mode = rospy.get_param('~drive_mode', 'twist') self.has_gyro = rospy.get_param('~has_gyro', False) self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.2)) self.stop_motors_on_bump = rospy.get_param('~stop_motors_on_bump', True) self.min_abs_yaw_vel = rospy.get_param('~min_abs_yaw_vel', None) self.max_abs_yaw_vel = rospy.get_param('~max_abs_yaw_vel', None) self.publish_tf = rospy.get_param('~publish_tf', False) self.odom_frame = rospy.get_param('~odom_frame', 'odom') self.base_frame = rospy.get_param('~base_frame', 'base_footprint') self.operate_mode = rospy.get_param('~operation_mode', 3) rospy.loginfo("serial port: %s"%(self.port)) rospy.loginfo("update_rate: %s"%(self.update_rate)) rospy.loginfo("drive mode: %s"%(self.drive_mode)) rospy.loginfo("has gyro: %s"%(self.has_gyro)) def _init_pubsub(self): self.joint_states_pub = rospy.Publisher('joint_states', JointState) self.odom_pub = rospy.Publisher('odom', Odometry) self.sensor_state_pub = rospy.Publisher('~sensor_state', EddiebotSensorState) self.operating_mode_srv = rospy.Service('~set_operation_mode', SetEddiebotMode, self.set_operation_mode) self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs) if self.drive_mode == 'twist': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel) self.drive_cmd = self.robot.direct_drive self.cmd_joints_sub = rospy.Subscriber('cmd_joints', JointState, self.process_cmd_joints) elif self.drive_mode == 'drive': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel) self.drive_cmd = self.robot.direct_drive elif self.drive_mode == 'eddie': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Eddie, self.cmd_vel) self.drive_cmd = self.robot.direct_drive else: rospy.logerr("unknown drive mode :%s"%(self.drive_mode)) self.transform_broadcaster = None if self.publish_tf: self.transform_broadcaster = tf.TransformBroadcaster() def reconfigure(self, config, level): self.update_rate = config['update_rate'] self.drive_mode = config['drive_mode'] self.has_gyro = config['has_gyro'] # if self.has_gyro: # self._gyro.reconfigure(config, level) self.odom_angular_scale_correction = config['odom_angular_scale_correction'] self.odom_linear_scale_correction = config['odom_linear_scale_correction'] self.cmd_vel_timeout = rospy.Duration(config['cmd_vel_timeout']) self.stop_motors_on_bump = config['stop_motors_on_bump'] self.min_abs_yaw_vel = config['min_abs_yaw_vel'] self.max_abs_yaw_vel = config['max_abs_yaw_vel'] return config def process_cmd_joints(self, msg): if(msg.position[0] > 0): pan_degree = self.last_pan_degree - (msg.velocity[0] / 3) * 10 else: pan_degree = self.last_pan_degree + (msg.velocity[0] / 3) * 10 print pan_degree # if(msg.position[1] > 0): # tilt_degree = 97 - (msg.velocity[1] / 3) * 97 # else: # tilt_degree = 97 + (msg.velocity[1] / 3) * 97 tilt_degree = 0.0 # if(abs(self.last_pan_degree - pan_degree) > 10): # Smooth the servo and cancer minor movement if(pan_degree > 180): pan_degree = 180 if(pan_degree < 0): pan_degree = 0 self.joints_cmd = self.robot.command_joints(pan_degree, tilt_degree) self.last_pan_degree = pan_degree # self.tilt_degree = msg. # selft.pan_degree = msg. def cmd_vel(self, msg): # Clamp to min abs yaw velocity, to avoid trying to rotate at low # speeds, which doesn't work well. 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: msg.angular.z = self.min_abs_yaw_vel if msg.angular.z > 0.0 else -self.min_abs_yaw_vel # Limit maximum yaw to avoid saturating the gyro 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: msg.angular.z = self.max_abs_yaw_vel if msg.angular.z > 0.0 else -self.max_abs_yaw_vel if self.drive_mode == 'twist': print "msg.linear.x " + str(msg.linear.x) print "\nmsg.angular.z " + str(msg.angular.z) print "\nbuf " + str(msg.linear.z) print "\nangle " + str(msg.linear.y) # convert twist to direct_drive args ts = msg.linear.x * 1000 # m -> mm tw = msg.angular.z * (robot_types.ROBOT_TYPES[self.robot_type].wheel_separation / 2) * 1000 # Prevent saturation at max wheel speed when a compound command is sent. if ts > 0: ts = min(ts, MAX_WHEEL_SPEED - abs(tw)) else: ts = max(ts, -(MAX_WHEEL_SPEED - abs(tw))) self.req_cmd_vel = int(ts + tw), int(ts - tw) elif self.drive_mode == 'eddie': # convert to direct_drive args ts = msg.linear * 1000 # m -> mm tw = msg.angular * (robot_types.ROBOT_TYPES[self.robot_type].wheel_separation / 2) * 1000 self.req_cmd_vel = int(ts - tw), int(ts + tw) elif self.drive_mode == 'drive': # convert twist to drive args, m->mm (velocity, radius) self.req_cmd_vel = msg.velocity * 1000, msg.radius * 1000 def set_operation_mode(self,req): if not self.robot.sci: raise Exception("Robot not connected, SCI not available") self.operate_mode = req.mode if req.mode == 1: #passive self._robot_run_passive() elif req.mode == 2: #safe self._robot_run_safe() elif req.mode == 3: #full self._robot_run_full() else: rospy.logerr("Requested an invalid mode.") return SetEddiebotModeResponse(False) return SetEddiebotModeResponse(True) def _robot_run_passive(self): """ Set robot into passive run mode """ rospy.loginfo("Setting eddiebot to passive mode.") #setting all the digital outputs to 0 self._set_digital_outputs([0, 0, 0]) self.robot.passive() def _robot_reboot(self): """ Perform a soft-reset of the Create """ msg ="Soft-rebooting eddiebot to passive mode." # rospy.logdebug(msg) # self._diagnostics.node_status(msg,"warn") # self._set_digital_outputs([0, 0, 0]) # self.robot.soft_reset() # time.sleep(2.0) def _robot_run_safe(self): """ Set robot into safe run mode """ rospy.loginfo("Setting eddiebot to safe mode.") self.robot.safe = True self.robot.control() b1 = (self.sensor_state.user_digital_inputs & 2)/2 b2 = (self.sensor_state.user_digital_inputs & 4)/4 self._set_digital_outputs([1, b1, b2]) def _robot_run_full(self): """ Set robot into full run mode """ # rospy.loginfo("Setting eddiebot to full mode.") self.robot.safe = False # self.robot.control() # b1 = (self.sensor_state.user_digital_inputs & 2)/2 # b2 = (self.sensor_state.user_digital_inputs & 4)/4 # self._set_digital_outputs([1, b1, b2]) def _set_digital_outputs(self, outputs): assert len(outputs) == 3, 'Expecting 3 output states.' byte = 0 for output, state in enumerate(outputs): byte += (2 ** output) * int(state) self.robot.set_digital_outputs(byte) self.sensor_state.user_digital_outputs = byte def set_digital_outputs(self,req): if not self.robot.sci: raise Exception("Robot not connected, SCI not available") outputs = [req.digital_out_0,req.digital_out_1, req.digital_out_2] self._set_digital_outputs(outputs) return SetDigitalOutputsResponse(True) def sense(self, sensor_state): self.sensor_handler.get_all(sensor_state) # if self._gyro: # self._gyro.update_calibration(sensor_state) def spin(self): # state s = self.sensor_state odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_frame) js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0]) r = rospy.Rate(self.update_rate) last_cmd_vel = 0, 0 last_cmd_vel_time = rospy.get_rostime() last_js_time = rospy.Time(0) # We set the retry count to 0 initially to make sure that only # if we received at least one sensor package, we are robust # agains a few sensor read failures. For some strange reason, # sensor read failures can occur when switching to full mode # on the Roomba. sensor_read_retry_count = 0 while not rospy.is_shutdown(): last_time = s.header.stamp curr_time = rospy.get_rostime() # SENSE/COMPUTE STATE try: self.sense(s) # transform = self.compute_odom(s, last_time, odom) # Future-date the joint states so that we don't have # to publish as frequently. js.header.stamp = curr_time + rospy.Duration(1) except select.error: # packet read can get interrupted, restart loop to # check for exit conditions continue except DriverError: if sensor_read_retry_count > 0: rospy.logwarn('Failed to read sensor package. %d retries left.' % sensor_read_retry_count) sensor_read_retry_count -= 1 continue else: raise sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT # Reboot Create if we detect that charging is necessary. # if s.charging_sources_available > 0 and \ # s.oi_mode == 1 and \ # s.charging_state in [0, 5] and \ # s.charge < 0.93*s.capacity: # rospy.loginfo("going into soft-reboot and exiting driver") # self._robot_reboot() # rospy.loginfo("exiting driver") # break # Reboot Create if we detect that battery is at critical level switch to passive mode. # if s.charging_sources_available > 0 and \ # s.oi_mode == 3 and \ # s.charging_state in [0, 5] and \ # s.charge < 0.15*s.capacity: # rospy.loginfo("going into soft-reboot and exiting driver") # self._robot_reboot() # rospy.loginfo("exiting driver") # break # PUBLISH STATE # self.sensor_state_pub.publish(s) # self.odom_pub.publish(odom) # if self.publish_tf: # self.publish_odometry_transform(odom) # 1hz, future-dated joint state # if curr_time > last_js_time + rospy.Duration(1): # self.joint_states_pub.publish(js) # last_js_time = curr_time # self._diagnostics.publish(s, self._gyro) # if self._gyro: # self._gyro.publish(s, last_time) # ACT if self.req_cmd_vel is not None: # check for velocity command and set the robot into full mode if not plugged in # if s.oi_mode != self.operate_mode and s.charging_sources_available != 1: # if self.operate_mode == 2: # self._robot_run_safe() # else: # self._robot_run_full() # check for bumper contact and limit drive command req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel) # Set to None so we know it's a new command self.req_cmd_vel = None # reset time for timeout last_cmd_vel_time = last_time else: #zero commands on timeout # if last_time - last_cmd_vel_time > self.cmd_vel_timeout: # last_cmd_vel = 0,0 # double check bumpers req_cmd_vel = self.check_bumpers(s, last_cmd_vel) # if(req_cmd_vel[0] != last_cmd_vel[0] or req_cmd_vel[1] != last_cmd_vel[1]): # Added if only drive command are different then issue new command # send command self.drive_cmd(*req_cmd_vel) # record command last_cmd_vel = req_cmd_vel r.sleep() def check_bumpers(self, s, cmd_vel): # Safety: disallow forward motion if bumpers or wheeldrops # are activated. # TODO: check bumps_wheeldrops flags more thoroughly, and disable # all motion (not just forward motion) when wheeldrops are activated # forward = (cmd_vel[0] + cmd_vel[1]) > 0 # if self.stop_motors_on_bump and s.bumps_wheeldrops and forward: # print "Bump-------------------------!!!" # return (0,0) # else: # return cmd_vel return cmd_vel def compute_odom(self, sensor_state, last_time, odom): """ Compute current odometry. Updates odom instance and returns tf transform. compute_odom() does not set frame ids or covariances in Odometry instance. It will only set stamp, pose, and twist. @param sensor_state: Current sensor reading @type sensor_state: EddiebotSensorState @param last_time: time of last sensor reading @type last_time: rospy.Time @param odom: Odometry instance to update. @type odom: nav_msgs.msg.Odometry @return: transform @rtype: ( (float, float, float), (float, float, float, float) ) """ # based on otl_roomba by OTL <*****@*****.**> current_time = sensor_state.header.stamp dt = (current_time - last_time).to_sec() # On startup, Create can report junk readings if abs(sensor_state.distance) > 1.0 or abs(sensor_state.angle) > 1.0: raise Exception("Distance, angle displacement too big, invalid readings from robot. Distance: %.2f, Angle: %.2f" % (sensor_state.distance, sensor_state.angle)) # this is really delta_distance, delta_angle d = sensor_state.distance * self.odom_linear_scale_correction #correction factor from calibration angle = sensor_state.angle * self.odom_angular_scale_correction #correction factor from calibration x = cos(angle) * d y = -sin(angle) * d last_angle = self._pos2d.theta self._pos2d.x += cos(last_angle)*x - sin(last_angle)*y self._pos2d.y += sin(last_angle)*x + cos(last_angle)*y self._pos2d.theta += angle # Eddiebot quaternion from yaw. simplified version of tf.transformations.quaternion_about_axis odom_quat = (0., 0., sin(self._pos2d.theta/2.), cos(self._pos2d.theta/2.)) # construct the transform transform = (self._pos2d.x, self._pos2d.y, 0.), odom_quat # update the odometry state odom.header.stamp = current_time odom.pose.pose = Pose(Point(self._pos2d.x, self._pos2d.y, 0.), Quaternion(*odom_quat)) odom.twist.twist = Twist(Vector3(d/dt, 0, 0), Vector3(0, 0, angle/dt)) if sensor_state.requested_right_velocity == 0 and \ sensor_state.requested_left_velocity == 0 and \ sensor_state.distance == 0: odom.pose.covariance = ODOM_POSE_COVARIANCE2 odom.twist.covariance = ODOM_TWIST_COVARIANCE2 else: odom.pose.covariance = ODOM_POSE_COVARIANCE odom.twist.covariance = ODOM_TWIST_COVARIANCE # return the transform return transform def publish_odometry_transform(self, odometry): self.transform_broadcaster.sendTransform( (odometry.pose.pose.position.x, odometry.pose.pose.position.y, odometry.pose.pose.position.z), (odometry.pose.pose.orientation.x, odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z, odometry.pose.pose.orientation.w), odometry.header.stamp, odometry.child_frame_id, odometry.header.frame_id)
logging.basicConfig(filename='test_eddiebot_driver.log', level=logging.INFO) # configure the serial connections (the parameters differs on the device you are connecting to) ser = serial.Serial( port='/dev/ttyUSB0', baudrate=115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS ) ser.open() ser.isOpen() print 'Enter your commands below.\r\nInsert "exit" to leave the application.' eddiebot = Eddiebot() input=1 while 1 : # get keyboard input command = raw_input("command: ") if command == "": # Stop the Eddie robot # send the character to the device # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) ser.write('STOP' + ' 0' + chr(13)) out = '' # let's wait one second before reading output (let's give device time to answer) time.sleep(1) while ser.inWaiting() > 0: out += ser.read(1)
from decimal import * logging.basicConfig(filename='test_eddiebot_driver.log', level=logging.INFO) # configure the serial connections (the parameters differs on the device you are connecting to) ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS) ser.open() ser.isOpen() print 'Enter your commands below.\r\nInsert "exit" to leave the application.' eddiebot = Eddiebot() input = 1 while 1: # get keyboard input command = raw_input("command: ") if command == "": # Stop the Eddie robot # send the character to the device # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) ser.write('STOP' + ' 0' + chr(13)) out = '' # let's wait one second before reading output (let's give device time to answer) time.sleep(1) while ser.inWaiting() > 0: out += ser.read(1)
class EddiebotNode(object): UNDEFINED = -1000 _SENSOR_READ_RETRY_COUNT = 5 ENCODER_TICKS = 144 WHEEL_BASE = 39.0525 # cm WHEEL_RADIUS = 7.62 # cm MILLIVOLTS_PER_VOLT = 3.918457 # magic constant CENTIMETERS_PER_TICK = 0.332485223 # 2 * PI * WHEEL_RADIUS / ENCODER_TICKS METERS_PER_TICK = 0.00332485223 DEGREES_PER_TICK = 0.975609758 # CENTIMETERS_PER_TICK / (PI * WHEEL_BASE) * 360 RADIANS_PER_TICK = 0.017027602 def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=120.0): """ @param default_port: default tty port to use for establishing connection to Eddiebot. This will be overriden by ~port ROS param if available. """ self.default_port = default_port self.default_update_rate = default_update_rate self.robot = Eddiebot() self.sensor_handler = None self.sensor_state = EddiebotSensorState() self.req_cmd_vel = None rospy.init_node('eddiebot') self._init_params() self._init_pubsub() self._pos2d = Pose2D() # 2D pose for odometry self._diagnostics = EddiebotDiagnostics() if self.has_gyro: self._gyro = EddiebotGyro() else: self._gyro = None dynamic_reconfigure.server.Server(EddieBotConfig, self.reconfigure) self.last_pan_degree = 96 # Default is facing center self.last_angle = self.UNDEFINED self.last_dist = self.UNDEFINED self.emergency_activated = False self.linear_speed = 0 self.angular_speed = 0 def start(self): log_once = True while not rospy.is_shutdown(): try: self.robot.start(self.port, robot_types.ROBOT_TYPES[self.robot_type].baudrate) break except serial.serialutil.SerialException as ex: msg = "Failed to open port %s. Please make sure the Create cable is plugged into the computer. \n"%(self.port) self._diagnostics.node_status(msg,"error") if log_once: log_once = False rospy.logerr(msg) else: sys.stderr.write(msg) os.system("fuser -k /dev/ttyUSB0") # time.sleep(3.0) #self.robot.sci.start_time = time.time() #self.robot.sci.com_log = open('/home/eddie/logs/com_log_' + str(int(self.robot.sci.start_time)), 'w') #self.robot.sci.com_log.write('cur_time,command\n') self.sensor_handler = robot_types.ROBOT_TYPES[self.robot_type].sensor_handler(self.robot) self.robot.safe = True # if rospy.get_param('~bonus', False): # bonus(self.robot) #self.robot.control() # Write driver state to disk with open(connected_file(), 'w') as f: f.write("1") # Disable all sensors Startup readings from Create can be incorrect, discard first values s = EddiebotSensorState() try: self.sense(s) except Exception: # packet read can get interrupted, restart loop to # check for exit conditions pass def _init_params(self): self.port = rospy.get_param('~port', self.default_port) self.robot_type = rospy.get_param('~robot_type', 'eddie') #self.baudrate = rospy.get_param('~baudrate', self.default_baudrate) self.update_rate = rospy.get_param('~update_rate', self.default_update_rate) self.drive_mode = rospy.get_param('~drive_mode', 'twist') self.has_gyro = rospy.get_param('~has_gyro', False) self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.2)) self.stop_motors_on_bump = rospy.get_param('~stop_motors_on_bump', True) self.min_abs_yaw_vel = rospy.get_param('~min_abs_yaw_vel', None) self.max_abs_yaw_vel = rospy.get_param('~max_abs_yaw_vel', None) self.publish_tf = rospy.get_param('~publish_tf', True) self.odom_frame = rospy.get_param('~odom_frame', 'odom') self.base_frame = rospy.get_param('~base_frame', 'base_footprint') self.operate_mode = rospy.get_param('~operation_mode', 3) rospy.loginfo("serial port: %s"%(self.port)) rospy.loginfo("update_rate: %s"%(self.update_rate)) rospy.loginfo("drive mode: %s"%(self.drive_mode)) rospy.loginfo("has gyro: %s"%(self.has_gyro)) self.max_linear_speed = rospy.get_param('~max_linear_speed_m_per_sec', 1) self.max_slow_speed = rospy.get_param('~max_slow_speed_m_per_sec', 0.3) self.max_angular_speed = rospy.get_param('~max_angular_speed_deg_per_sec', 40) self.max_acc = int(rospy.get_param('~max_acceleration_cm_per_sec_square', 20) / self.CENTIMETERS_PER_TICK) self.slow_speed_distance = rospy.get_param('~slow_speed_distance_cm', 150) self.stop_distance = rospy.get_param('~stop_distance_cm', 30) self.check_distance = rospy.get_param('~check_distance', True) def _init_pubsub(self): self.joint_states_pub = rospy.Publisher('joint_states', JointState) self.odom_pub = rospy.Publisher('odom', Odometry) self.sensor_state_pub = rospy.Publisher('~sensor_state', EddiebotSensorState) self.operating_mode_srv = rospy.Service('~set_operation_mode', SetEddiebotMode, self.set_operation_mode) self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs) if self.drive_mode == 'twist': #self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel) self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.web_and_joy_cmd_vel) self.drive_cmd = self.robot.direct_drive self.cmd_joints_sub = rospy.Subscriber('cmd_joints', JointState, self.process_cmd_joints) #self.cmd_navigation_vel_sub = rospy.Subscriber('navigation_velocity_smoother/raw_cmd_vel', Twist, self.cmd_navigation_vel) self.cmd_navigation_vel_sub = rospy.Subscriber('navigation_velocity_smoother/raw_cmd_vel', Twist, self.nav_cmd_vel) elif self.drive_mode == 'drive': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel) self.drive_cmd = self.robot.direct_drive elif self.drive_mode == 'eddie': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Eddie, self.cmd_vel) self.drive_cmd = self.robot.direct_drive else: rospy.logerr("unknown drive mode :%s"%(self.drive_mode)) self.transform_broadcaster = None if self.publish_tf: self.transform_broadcaster = tf.TransformBroadcaster() self.emergency_stop_sub = rospy.Subscriber('emergency_stop', Bool, self.emergency_stop) def reconfigure(self, config, level): self.update_rate = config['update_rate'] self.drive_mode = config['drive_mode'] self.has_gyro = config['has_gyro'] # if self.has_gyro: # self._gyro.reconfigure(config, level) self.odom_angular_scale_correction = config['odom_angular_scale_correction'] self.odom_linear_scale_correction = config['odom_linear_scale_correction'] self.cmd_vel_timeout = rospy.Duration(config['cmd_vel_timeout']) self.stop_motors_on_bump = config['stop_motors_on_bump'] self.min_abs_yaw_vel = config['min_abs_yaw_vel'] self.max_abs_yaw_vel = config['max_abs_yaw_vel'] return config def process_cmd_joints(self, msg): if(msg.position[0] > 0): pan_degree = self.last_pan_degree - (msg.velocity[0] / 3) * 10 else: pan_degree = self.last_pan_degree + (msg.velocity[0] / 3) * 10 print pan_degree # if(msg.position[1] > 0): # tilt_degree = 97 - (msg.velocity[1] / 3) * 97 # else: # tilt_degree = 97 + (msg.velocity[1] / 3) * 97 tilt_degree = 0.0 # if(abs(self.last_pan_degree - pan_degree) > 10): # Smooth the servo and cancer minor movement if(pan_degree > 180): pan_degree = 180 if(pan_degree < 0): pan_degree = 0 self.joints_cmd = self.robot.command_joints(pan_degree, tilt_degree) self.last_pan_degree = pan_degree # self.tilt_degree = msg. # selft.pan_degree = msg. def emergency_stop(self, msg): if msg.data == True: print ("Software emergency stop activated") else: print ("Software emergency stop deactivated") self.emergency_activated = msg.data def limit_speed(self): #if self.check_distance: if self.linear_speed > 0: if self.sensor_state.cliff_front_right_signal < self.stop_distance: self.linear_speed = 0 elif self.sensor_state.wall_signal < self.slow_speed_distance or self.sensor_state.cliff_left_signal < self.slow_speed_distance: self.linear_speed = min(self.max_slow_speed, self.linear_speed) def web_and_joy_cmd_vel(self, msg): self.linear_speed = self.max_linear_speed * msg.linear.x self.angular_speed = self.max_angular_speed * msg.angular.z #self.limit_speed() if self.angular_speed > 15: self.angular_speed = 15 if self.angular_speed < -15: self.angular_speed = -15 linear_speed_ticks = self.linear_speed / self.METERS_PER_TICK angular_speed_ticks = self.angular_speed / self.DEGREES_PER_TICK s1 = linear_speed_ticks + angular_speed_ticks s2 = linear_speed_ticks - angular_speed_ticks self.req_cmd_vel = int(s1), int(s2) print self.req_cmd_vel def nav_cmd_vel(self, msg): if msg.angular.z > 0.26: msg.angular.z = 0.26 if msg.angular.z < -0.26: msg.angular.z = -0.26 if msg.linear.x > 0.1: msg.linear.x = 0.1 if msg.linear.x < -0.1: msg.linear.x = -0.1 if msg.linear.x < 0.001: if msg.angular.z > 0 and msg.angular.z < 0.3: msg.angular.z = 0.3 if msg.angular.z < 0 and msg.angular.z > -0.3: msg.angular.z = -0.3 linear_speed_ticks = msg.linear.x / self.METERS_PER_TICK angular_speed_ticks = msg.angular.z / self.RADIANS_PER_TICK #if linear_speed_ticks < 11: # linear_speed_ticks = 0 s1 = linear_speed_ticks + angular_speed_ticks s2 = linear_speed_ticks - angular_speed_ticks #while linear_speed_ticks > 11 and (s1 < 11 or s2 < 11): # s1 = s1 + 3 # s2 = s2 + 3 print s1, s2 self.req_cmd_vel = int(s1), int(s2) def set_operation_mode(self,req): if not self.robot.sci: raise Exception("Robot not connected, SCI not available") self.operate_mode = req.mode if req.mode == 1: #passive self._robot_run_passive() elif req.mode == 2: #safe self._robot_run_safe() elif req.mode == 3: #full self._robot_run_full() else: rospy.logerr("Requested an invalid mode.") return SetEddiebotModeResponse(False) return SetEddiebotModeResponse(True) def _robot_run_passive(self): """ Set robot into passive run mode """ rospy.loginfo("Setting eddiebot to passive mode.") #setting all the digital outputs to 0 self._set_digital_outputs([0, 0, 0]) self.robot.passive() def _robot_reboot(self): """ Perform a soft-reset of the Create """ msg ="Soft-rebooting eddiebot to passive mode." # rospy.logdebug(msg) # self._diagnostics.node_status(msg,"warn") # self._set_digital_outputs([0, 0, 0]) # self.robot.soft_reset() # time.sleep(2.0) def _robot_run_safe(self): """ Set robot into safe run mode """ rospy.loginfo("Setting eddiebot to safe mode.") self.robot.safe = True #self.robot.control() b1 = (self.sensor_state.user_digital_inputs & 2)/2 b2 = (self.sensor_state.user_digital_inputs & 4)/4 self._set_digital_outputs([1, b1, b2]) def _robot_run_full(self): """ Set robot into full run mode """ # rospy.loginfo("Setting eddiebot to full mode.") self.robot.safe = False # self.robot.control() # b1 = (self.sensor_state.user_digital_inputs & 2)/2 # b2 = (self.sensor_state.user_digital_inputs & 4)/4 # self._set_digital_outputs([1, b1, b2]) def _set_digital_outputs(self, outputs): assert len(outputs) == 3, 'Expecting 3 output states.' byte = 0 for output, state in enumerate(outputs): byte += (2 ** output) * int(state) self.robot.set_digital_outputs(byte) self.sensor_state.user_digital_outputs = byte def set_digital_outputs(self,req): if not self.robot.sci: raise Exception("Robot not connected, SCI not available") outputs = [req.digital_out_0,req.digital_out_1, req.digital_out_2] self._set_digital_outputs(outputs) return SetDigitalOutputsResponse(True) def sense(self, sensor_state): self.sensor_handler.get_all(sensor_state) # if self._gyro: # self._gyro.update_calibration(sensor_state) def spin(self): # state s = self.sensor_state odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_frame) js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0]) r = rospy.Rate(self.update_rate) last_cmd_vel = 0, 0 last_cmd_vel_time = rospy.get_rostime() last_js_time = rospy.Time(0) # We set the retry count to 0 initially to make sure that only # if we received at least one sensor package, we are robust # agains a few sensor read failures. For some strange reason, # sensor read failures can occur when switching to full mode # on the Roomba. sensor_read_retry_count = 0 while not rospy.is_shutdown(): last_time = s.header.stamp curr_time = rospy.get_rostime() # SENSE/COMPUTE STATE try: self.sense(s) transform = self.compute_odom(s, last_time, odom) # Future-date the joint states so that we don't have # to publish as frequently. js.header.stamp = curr_time + rospy.Duration(1) except select.error: # packet read can get interrupted, restart loop to # check for exit conditions continue except DriverError: if sensor_read_retry_count > 0: rospy.logwarn('Failed to read sensor package. %d retries left.' % sensor_read_retry_count) sensor_read_retry_count -= 1 continue else: raise sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT # Reboot Create if we detect that charging is necessary. # if s.charging_sources_available > 0 and \ # s.oi_mode == 1 and \ # s.charging_state in [0, 5] and \ # s.charge < 0.93*s.capacity: # rospy.loginfo("going into soft-reboot and exiting driver") # self._robot_reboot() # rospy.loginfo("exiting driver") # break # Reboot Create if we detect that battery is at critical level switch to passive mode. # if s.charging_sources_available > 0 and \ # s.oi_mode == 3 and \ # s.charging_state in [0, 5] and \ # s.charge < 0.15*s.capacity: # rospy.loginfo("going into soft-reboot and exiting driver") # self._robot_reboot() # rospy.loginfo("exiting driver") # break # PUBLISH STATE if transform is not None: self.sensor_state_pub.publish(s) self.odom_pub.publish(odom) if self.publish_tf: self.publish_odometry_transform(odom) # 1hz, future-dated joint state # if curr_time > last_js_time + rospy.Duration(1): # self.joint_states_pub.publish(js) # last_js_time = curr_time # self._diagnostics.publish(s, self._gyro) # if self._gyro: # self._gyro.publish(s, last_time) # ACT if self.req_cmd_vel is not None: # check for velocity command and set the robot into full mode if not plugged in # if s.oi_mode != self.operate_mode and s.charging_sources_available != 1: # if self.operate_mode == 2: # self._robot_run_safe() # else: # self._robot_run_full() # check for bumper contact and limit drive command req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel) # Set to None so we know it's a new command self.req_cmd_vel = None # reset time for timeout last_cmd_vel_time = last_time else: #zero commands on timeout # if last_time - last_cmd_vel_time > self.cmd_vel_timeout: # last_cmd_vel = 0,0 # double check bumpers req_cmd_vel = self.check_bumpers(s, last_cmd_vel) # if(req_cmd_vel[0] != last_cmd_vel[0] or req_cmd_vel[1] != last_cmd_vel[1]): # Added if only drive command are different then issue new command # send command if self.emergency_activated: #self.drive_cmd(0, 0) self.robot.sci.getSer().write('STOP 0' + chr(13)) else: self.drive_cmd(*req_cmd_vel) # record command last_cmd_vel = req_cmd_vel r.sleep() def check_bumpers(self, s, cmd_vel): # Safety: disallow forward motion if bumpers or wheeldrops # are activated. # TODO: check bumps_wheeldrops flags more thoroughly, and disable # all motion (not just forward motion) when wheeldrops are activated #forward = (cmd_vel[0] + cmd_vel[1]) > 0 if self.stop_motors_on_bump and s.bumps_wheeldrops:# and forward: #return (0,0) return cmd_vel else: return cmd_vel # return cmd_vel def compute_odom(self, sensor_state, last_time, odom): """ Compute current odometry. Updates odom instance and returns tf transform. compute_odom() does not set frame ids or covariances in Odometry instance. It will only set stamp, pose, and twist. @param sensor_state: Current sensor reading @type sensor_state: EddiebotSensorState @param last_time: time of last sensor reading @type last_time: rospy.Time @param odom: Odometry instance to update. @type odom: nav_msgs.msg.Odometry @return: transform @rtype: ( (float, float, float), (float, float, float, float) ) """ # based on otl_roomba by OTL <*****@*****.**> current_time = sensor_state.header.stamp dt = (current_time - last_time).to_sec() # On startup, Create can report junk readings #if abs(sensor_state.distance) > 1.0 or abs(sensor_state.angle) > 1.0: # raise Exception("Distance, angle displacement too big, invalid readings from robot. Distance: %.2f, Angle: %.2f" % (sensor_state.distance, sensor_state.angle)) angle_rad = math.radians(-sensor_state.angle) #sensor_state.distance = self.dist_cmd #sensor_state.angle = self.angle_cmd if self.last_angle == self.UNDEFINED: self.last_angle = angle_rad self.last_dist = sensor_state.distance #with open('/home/robco/ros/pos2d_x', 'r') as f: # self._pos2d.x = float(f.read()) #with open('/home/robco/ros/pos2d_y', 'r') as f: # self._pos2d.y = float(f.read()) #with open('/home/robco/ros/pos2d_theta', 'r') as f: # self._pos2d.theta = float(f.read()) return None # this is really delta_distance, delta_angle d = (sensor_state.distance - self.last_dist) #* self.odom_linear_scale_correction #correction factor from calibration angle = (angle_rad - self.last_angle) #* self.odom_angular_scale_correction #correction factor from calibration self.last_angle = angle_rad self.last_dist = sensor_state.distance x = cos(angle) * d y = -sin(angle) * d #print(sensor_state.distance) last_angle = self._pos2d.theta self._pos2d.x += cos(last_angle)*x - sin(last_angle)*y self._pos2d.y += sin(last_angle)*x + cos(last_angle)*y self._pos2d.theta += angle #with open('/home/robco/ros/pos2d_x', 'w') as f: # f.write(str(self._pos2d.x)) #with open('/home/robco/ros/pos2d_y', 'w') as f: # f.write(str(self._pos2d.y)) #with open('/home/robco/ros/pos2d_theta', 'w') as f: # f.write(str(self._pos2d.theta)) # Eddiebot quaternion from yaw. simplified version of tf.transformations.quaternion_about_axis odom_quat = (0., 0., sin(self._pos2d.theta/2.), cos(self._pos2d.theta/2.)) # construct the transform transform = (self._pos2d.x, self._pos2d.y, 0.), odom_quat # update the odometry state odom.header.stamp = current_time odom.pose.pose = Pose(Point(self._pos2d.x, self._pos2d.y, 0.), Quaternion(*odom_quat)) odom.twist.twist = Twist(Vector3(d/dt, 0, 0), Vector3(0, 0, angle/dt)) if sensor_state.requested_right_velocity == 0 and \ sensor_state.requested_left_velocity == 0 and \ sensor_state.distance == 0: odom.pose.covariance = ODOM_POSE_COVARIANCE2 odom.twist.covariance = ODOM_TWIST_COVARIANCE2 else: odom.pose.covariance = ODOM_POSE_COVARIANCE odom.twist.covariance = ODOM_TWIST_COVARIANCE # return the transform return transform def publish_odometry_transform(self, odometry): self.transform_broadcaster.sendTransform( (odometry.pose.pose.position.x, odometry.pose.pose.position.y, odometry.pose.pose.position.z), (odometry.pose.pose.orientation.x, odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z, odometry.pose.pose.orientation.w), odometry.header.stamp, odometry.child_frame_id, odometry.header.frame_id)