www.gusucode.com > robotexamples 工具箱 matlab源码程序 > robotexamples/ros/helpers/exampleHelperTurtleBotTrackingTimer.m
function exampleHelperTurtleBotTrackingTimer(~, event, handles) %exampleHelperTurtleBotTrackingTimer - Timer update function called in TurtlebotObjectTracking % exampleHelperTurtleBotTrackingTimer(~, event, handles) updates the state of a % Turtlebot using ROS publishers and subscribers passed into the % function through the handles struct. The controller here generates % object tracking behavior by using the Kinect sensor. % The first argument is the timer object and can be ignored. % % See also TurtleBotObjectTrackingExample % Copyright 2014-2015 The MathWorks, Inc. persistent state persistent angVFilt persistent linVFilt persistent initialTime persistent cliffFlag persistent bumpArray maxSpinTime = 3.5; maxBumpTime = 2; maxCliffTime = 2; betaLin = 0.7; betaAng = 0.7; [findObject, imageControl] = initControl(); currentTime = datetime(event.Data.time); % Initialize state and persistent variables if isempty(state) state = ExampleHelperTurtleBotStates.Seek; angVFilt = 0; linVFilt = 0; setSound(handles.soundPub, 'cleaning end'); initialTime = currentTime; cliffFlag = 0; handles.cliffSub.NewMessageFcn = @cliffCallback; bumpArray = []; handles.bumpSub.NewMessageFcn = @bumpCallback; end % Check bumper status and start timer if active if ~isempty(bumpArray) bumpArray = []; state = ExampleHelperTurtleBotStates.Bumper; setSound(handles.soundPub, 'error sound'); initialTime = currentTime; end % Check cliff sensor status and start timer if active if cliffFlag cliffFlag = 0; state = ExampleHelperTurtleBotStates.Cliff; setSound(handles.soundPub, 'error sound'); initialTime = currentTime; end switch state case ExampleHelperTurtleBotStates.Seek % Object-finding state latestImg = readImage(handles.colorImgSub.LatestMessage); [center, scale] = findObject(latestImg,handles.params); % Wander if no circle is found, target the circle if it exists if isempty(center) [linearV, angularV] = exampleHelperTurtleBotWanderController(); else [linearV, angularV] = imageControl(center, scale, size(latestImg),handles.gains); setSound(handles.soundPub, 'recharge start'); end state = ExampleHelperTurtleBotStates.Seek; case ExampleHelperTurtleBotStates.Bumper % Bumper contact state - reverse [linearV, angularV] = exampleHelperTurtleBotReverseController(); % Check for bumper state time expiration if seconds(currentTime - initialTime) > maxBumpTime if isempty(bumpArray) state = ExampleHelperTurtleBotStates.Spin; initialTime = currentTime; end end case ExampleHelperTurtleBotStates.Spin % Spin state [linearV, angularV] = exampleHelperTurtleBotSpinController(); % Check for spin state time expiration if seconds(currentTime - initialTime) > maxSpinTime setSound(handles.soundPub, 'turn off'); state = ExampleHelperTurtleBotStates.Seek; end case ExampleHelperTurtleBotStates.Cliff % Cliff avoidance [linearV, angularV] = exampleHelperTurtleBotReverseController(); % Check for cliff state time expiration if seconds(currentTime - initialTime) > maxCliffTime cliffFlag = 0; state = ExampleHelperTurtleBotStates.Spin; initialTime = currentTime; end end % Filter the velocities linVFilt = betaLin*linearV + (1-betaLin)*linVFilt; angVFilt = betaAng*angularV + (1-betaAng)*angVFilt; % Publish velocities to the robot exampleHelperTurtleBotSetVelocity(handles.velPub, linVFilt, angVFilt); function [objectTrack, imgControl] = initControl() % INITCONTROL - Initialization function to determine which control % and object detection algorithms to use objectTrack = @exampleHelperTurtleBotFindBlueBall; imgControl = @exampleHelperTurtleBotPointController; end function cliffCallback(~, ~) %CLIFFCALLBACK - Raise a flag when cliff sensor is activated cliffFlag = 1; end function bumpCallback(~, message) %BUMPCALLBACK Save the received array of bumper contacts bumpArray = message.Data; end end function setSound(soundPub, type) %SETSOUND - Plays a sound from the Turtlebot persistent soundMsg if isempty(soundMsg) soundMsg = rosmessage(soundPub); end switch type case 'turn off' type = 1; case 'recharge start' type = 2; case 'error sound' type = 4; case 'cleaning end' type = 6; otherwise disp('Invalid Sound'); return; end soundMsg.Value = type; send(soundPub,soundMsg); end