www.gusucode.com > ros 工具箱 matlab源码程序 > ros/+robotics/+ros/SimpleActionClient.m
classdef SimpleActionClient < ...
robotics.ros.internal.mixin.NodeDependent & ...
robotics.ros.internal.mixin.Unsaveable & ...
robotics.ros.internal.mixin.JavaAccess & ...
handle
%SimpleActionClient Create a simple ROS action client
% Use SimpleActionClient to connect to an action server and request the
% execution of action goals. You can get feedback on the execution
% progress and cancel the goal at any time. SimpleActionClient
% encapsulates a simple action client and allows you to track a
% single goal at a time.
%
% CLIENT = robotics.ros.SimpleActionClient(NODE, 'ACTIONNAME') creates
% a CLIENT for the ROS action with name ACTIONNAME. NODE is the
% robotics.ros.Node object that is used to connect to the ROS
% network. If an action with this name is available in the
% ROS network, the client determines the action type automatically.
% If the action is not available, this function displays an error. CLIENT
% is a simple action client and allows you to track a single goal at a
% time.
%
% CLIENT = robotics.ros.SimpleActionClient(NODE, 'ACTIONNAME', 'ACTIONTYPE')
% creates an action client for the ROS action with name ACTIONNAME and
% type ACTIONTYPE. If no action with this name is available in the ROS network,
% it is created. If an action with the same name is available, but its
% type does not match ACTIONTYPE, the function displays an error.
%
%
% SimpleActionClient properties:
% ActionName - Name of action associated with this client (Read-only)
% ActionType - Type of action (Read-only)
% IsServerConnected - Indicates if client is connected to action server (Read-only)
% Goal - The tracked goal (Read-only)
% GoalState - State of the tracked goal (Read-only)
% ActivationFcn - Function executed on goal activation
% FeedbackFcn - Function executed for goal feedback
% ResultFcn - Function executed for goal result
%
% SimpleActionClient methods:
% waitForServer - Wait for action server to start
% sendGoal - Send goal message to action server
% sendGoalAndWait - Send goal message and wait for result
% cancelGoal - Cancel last goal this client sent
% cancelAllGoals - Cancel all goals on the server
% rosmessage - Create goal message
%
%
% Example:
% node = robotics.ros.Node('/tbot_client');
%
% % Create action client for TurtleBot movement
% % The turtlebot_move action server needs to be running
% tbot = robotics.ros.SimpleActionClient(node, '/turtlebot_move', 'turtlebot_actions/TurtlebotMove')
%
% % Wait for the action server to start up
% waitForServer(tbot)
%
% % Request forward movement and wait until the TurtleBot is done
% goalmsg = rosmessage(tbot);
% goalmsg.ForwardDistance = 1.0;
% resultmsg = sendGoalAndWait(tbot, goalmsg);
%
% % Display the actual distance moved by the robot and the final goal state
% resultmsg.ForwardDistance
% tbot.GoalState
%
% % Set a custom callback for goal results. Disable feedback.
% tbot.FeedbackFcn = []
% tbot.ResultFcn = @(~,res) disp(['TurtleBot moved ' num2str(res.Message.ForwardDistance) ' meters forward'])
%
% % Send a goal to the server. This call will return immediately.
% sendGoal(tbot, goalmsg);
%
% See also ROSACTIONCLIENT, ROSACTION.
% Copyright 2016 The MathWorks, Inc.
properties (SetAccess = private)
%ActionName - Name of action associated with this client
ActionName = ''
%ActionType - Type of action associated with this client
ActionType = ''
%IsServerConnected - Indicates if client is connected to action server
% See also waitForServer.
IsServerConnected = false
%Goal - The tracked goal
% The simple action client tracks the execution of this goal. The
% client can only track a single goal at a time.
Goal
%GoalState - State of the tracked goal
% This reflects the state of the goal that is currently tracked
% by this action client (last goal message sent to the
% action server).
% Possible values are: 'pending', 'active', 'recalled', 'rejected',
% 'preempted', 'aborted', 'succeeded', or 'lost'
GoalState = ''
end
properties (Dependent)
%ActivationFcn - Function executed on goal activation
% This function is called when the last sent goal is accepted by
% the action server and it transitions into the 'active' state.
%
% The callback function should accept at least one input
% argument, CLIENT, which is the associated SimpleActionClient
% object. Additional input arguments can be provided with VARARGIN.
%
% The function signature is as follows:
%
% function activationFcn(CLIENT, VARARGIN)
%
ActivationFcn
%FeedbackFcn - Function executed for goal feedback
% This function is called at a regular rate during execution of
% the last sent goal. The received feedback message contains
% information about the goal progress.
%
% The callback function should accept at least two input
% argument. The first argument, CLIENT, is the associated SimpleActionClient
% object. The second argument, FBMSG, is the received feedback
% message. Additional input arguments can be provided with VARARGIN.
%
% The function signature is as follows:
%
% function feedbackFcn(CLIENT, FBMSG, VARARGIN)
%
% You pass additional arguments to the callback function by including
% both the callback function and the arguments as elements of a cell array
% when setting the property.
FeedbackFcn
%ResultFcn - Function executed for goal result
% This function is called when the last sent goal finishes
% execution on the server and is in a final state. The received
% message contains information about the goal result.
%
% The callback function should accept at least two input
% arguments. The first argument, CLIENT, is the associated SimpleActionClient
% object. The second argument, RESULT, is a structure with information
% about the goal result. Additional input arguments can be provided with VARARGIN.
% The RESULT structure contains the following fields:
% - Message - Received result message
% - State - Final goal state
% - StatusText - Status text associated with state
%
% The function signature is as follows:
%
% function resultFcn(CLIENT, RESULT, VARARGIN)
%
% You pass additional arguments to the callback function by including
% both the callback function and the arguments as elements of a cell array
% when setting the property.
ResultFcn
end
properties (Access = ?matlab.unittest.TestCase)
%JavaActionClient - Simple action client Java object
JavaActionClient
end
properties (Access = private)
%ClientCallbacks - Java object handling callbacks from action client
% This object is passed to Java calls to sendGoal and
% sendGoalAndWait and acts as an intermediary between Java
% and MATLAB callback functions.
ClientCallbacks
%ActivationListener - Bound listener object for activation notices
ActivationListener
%FeedbackListener - Bound listener object for received goal feedback
FeedbackListener
%ResultListener - Bound listener object for received goal result
ResultListener
%InternalActivationFcn - Internal storage for the ActivationFcn callback
InternalActivationFcn
%InternalFeedbackFcn - Internal storage for the FeedbackFcn callback
InternalFeedbackFcn
%InternalResultFcn - Internal storage for the ResultFcn callback
InternalResultFcn
%GoalMessageType - The message type of the goal message (sent by client)
GoalMessageType
%FeedbackMessageType - The message type of the feedback message (sent by server)
FeedbackMessageType
%ResultMessageType - The message type of the result message (sent by server)
ResultMessageType
end
properties (Access = ?matlab.unittest.TestCase)
%Delegate - The delegate for most ROS-related functionality
Delegate
%Parser - Helper object for parsing tasks
Parser
end
methods
function obj = SimpleActionClient(node, actionNamespace, varargin)
%SimpleActionClient Create a ROS action client object
% See also SimpleActionClient.
narginchk(2,3);
% Assign the delegates for most action client business logic
delegateFactory = robotics.ros.internal.action.ActionDelegateFactory.getInstance;
clientDelegate = delegateFactory.findDelegate('simpleClientDelegate');
parser = delegateFactory.findDelegate('simpleClientParser');
% Figure out if we need to use the global node or not
node = clientDelegate.checkGlobalNode(node);
% Parse the input arguments
defaultActionType = '';
[node, actionNamespace, actionType] = ...
parser.parseConstructorInput(defaultActionType, node, actionNamespace, varargin{:});
% Construct superclass
obj = obj@robotics.ros.internal.mixin.NodeDependent(node);
obj.Delegate = clientDelegate;
obj.Parser = parser;
[obj.ActionName, obj.ActionType] = obj.Delegate.resolveNameType( ...
node, actionNamespace, actionType, defaultActionType);
obj.GoalMessageType = [obj.ActionType 'Goal'];
obj.FeedbackMessageType = [obj.ActionType 'Feedback'];
obj.ResultMessageType = [obj.ActionType 'Result'];
% Create Java action client and initial goal message
obj.JavaActionClient = obj.Delegate.createActionClient(node.JavaNode, obj.ActionName, obj.ActionType);
goalMsg = obj.Delegate.createGoalMessage(obj.GoalMessageType);
obj.Goal = goalMsg.empty(0,1);
% Initialize callback functions
obj.ClientCallbacks = com.mathworks.toolbox.robotics.ros.action.SimpleActionClientCallbacks;
[obj.ActivationListener, obj.FeedbackListener, obj.ResultListener] = ...
obj.Delegate.attachEventListeners(obj.ClientCallbacks);
% Set default callback functions
obj.FeedbackFcn = [];
obj.ResultFcn = [];
obj.ActivationFcn = [];
obj.FeedbackFcn = obj.defaultFeedbackFcn;
obj.ResultFcn = obj.defaultResultFcn;
obj.ActivationFcn = obj.defaultActivationFcn;
end
function delete(obj)
%DELETE Shut down action client
% DELETE(CLIENT) shuts down the ROS action client object CLIENT.
% If the goal that was last sent to the action server is active,
% it is not cancelled.
if ~isempty(obj.Delegate)
obj.Delegate.shutdown(obj.JavaActionClient);
end
end
function goalMsg = rosmessage(obj)
%ROSMESSAGE Create goal message based on action type
% GOALMSG = ROSMESSAGE(CLIENT) creates and returns a new goal
% message GOALMSG. The message type of GOALMSG is determined
% by the action type associated with this action client.
%
% Example:
% % Create action client and goal message
% fibclient = robotics.ros.SimpleActionClient(node, '/fibonacci');
% goalmsg = ROSMESSAGE(fibclient);
goalMsg = obj.Delegate.createGoalMessage(obj.GoalMessageType);
end
function waitForServer(obj, varargin)
%waitForServer Wait for action server to start
% waitForServer(CLIENT) blocks MATLAB from running the current program
% until the action server is started up and available to
% receive goals. Press Ctrl+C to abort the wait.
%
% waitForServer(CLIENT, TIMEOUT) specifies a TIMEOUT period, in
% seconds. If the server does not start up in the timeout
% period, this function displays an error.
narginchk(1,2);
defaultTimeout = Inf;
timeout = obj.Parser.parseWaitForServerInput(defaultTimeout, varargin{:});
isConnected = obj.Delegate.waitForServer(obj.JavaActionClient, timeout);
if ~isConnected
error(message('robotics:ros:actionclient:WaitServerTimeout', num2str(timeout)));
end
end
function sendGoal(obj, goalMsg)
%sendGoal Send goal message to action server
% sendGoal(CLIENT, GOALMSG) sends a goal message object,
% GOALMSG, to the action server. This goal is tracked by
% the action client. The function does not wait
% for the goal to be executed and returns immediately.
% If the ActivationFcn, FeedbackFcn, and ResultFcn callbacks
% are defined, they are called when the goal is processing on
% the action server.
% All callbacks associated with a previously sent goal are
% disabled, but the previous goal is not cancelled.
%
% See also sendGoalAndWait, ResultFcn, FeedbackFcn, ActivationFcn.
obj.Parser.parseSendGoalInput(goalMsg, obj.GoalMessageType);
obj.Goal = goalMsg;
obj.Delegate.sendGoal(obj.JavaActionClient, goalMsg, obj.ClientCallbacks);
end
function cancelGoal(obj)
%cancelGoal Cancel last goal this client sent
% cancelGoal(CLIENT) sends a cancel request for the tracked goal
% (last sent to the action server).
% If the goal is in the 'active' state, the server preempts
% its execution. If the goal is in the 'pending' state, the goal
% is recalled.
% If no goal has been sent by this CLIENT or if the tracked goal
% has finished execution, this function returns immediately.
obj.Delegate.cancelGoal(obj.JavaActionClient);
end
function cancelAllGoals(obj)
%cancelAllGoals Cancel all goals on the server
% cancelAllGoals(CLIENT) sends a request to the action server
% to cancel all currently pending or active goals. This
% includes goals from other action clients.
obj.Delegate.cancelAllGoals(obj.JavaActionClient);
end
function [resultMsg, state, statusText] = sendGoalAndWait(obj, goalMsg, varargin)
%sendGoalAndWait Send goal message and wait for result
% RESULTMSG = sendGoalAndWait(CLIENT, GOALMSG) sends a goal message
% object, GOALMSG, to the action server and blocks MATLAB
% from running the current program until the action server
% returns the result, RESULTMSG.
% Press Ctrl+C to abort the wait.
%
% RESULTMSG = sendGoalAndWait(CLIENT, GOALMSG, TIMEOUT) specifies
% a TIMEOUT period, in seconds. If the server does not return
% the result in the timeout period, the function displays an
% error.
%
% [RESULTMSG, STATE, STATUSTEXT] = ____ also returns the
% final goal state, STATE, and the associated status text,
% STATUSTEXT. STATE contains information if the goal
% execution succeeded or not.
%
% If the ActivationFcn, FeedbackFcn, and ResultFcn callbacks
% are defined, they are called when the goal is processing on
% the action server.
narginchk(2,3);
% Parse the inputs
defaultTimeout = Inf;
timeout = obj.Parser.parseSendGoalAndWaitInput(defaultTimeout, obj.GoalMessageType, ...
goalMsg, varargin{:});
obj.Goal = goalMsg;
[goalDone, resultMsg, state, statusText] = obj.Delegate.sendGoalAndWait(...
obj.JavaActionClient, goalMsg, obj.ClientCallbacks, timeout, obj.ResultMessageType);
if ~goalDone
error(message('robotics:ros:actionclient:WaitGoalTimeout', num2str(timeout)));
end
end
end
%% Custom Getters and Setters for Properties
methods
function isConnected = get.IsServerConnected(obj)
isConnected = obj.Delegate.isServerConnected(obj.JavaActionClient);
end
function state = get.GoalState(obj)
state = obj.Delegate.getGoalState(obj.JavaActionClient);
end
function feedbackFcn = get.FeedbackFcn(obj)
feedbackFcn = obj.InternalFeedbackFcn;
end
function set.FeedbackFcn(obj, fcnHandle)
%set.FeedbackFcn Setter for FeedbackFcn callback property
if isempty(fcnHandle)
% Allow empty input to disable the callback
obj.FeedbackListener.Callback = [];
obj.InternalFeedbackFcn = [];
return;
end
% Make sure this is a valid function handle and parse any user data
[funcHandle, userData] = obj.Parser.parseFcnSetterInput(fcnHandle, 'FeedbackFcn');
obj.FeedbackListener.Callback = {@robotics.ros.internal.action.ActionCallbacks.onActionFeedbackCallback, ...
obj, obj.FeedbackListener, obj.FeedbackMessageType, funcHandle, userData};
obj.InternalFeedbackFcn = fcnHandle;
end
function resultFcn = get.ResultFcn(obj)
resultFcn = obj.InternalResultFcn;
end
function set.ResultFcn(obj, fcnHandle)
%set.ResultFcn Setter for ResultFcn callback property
if isempty(fcnHandle)
% Allow empty input to disable the callback
obj.ResultListener.Callback = [];
obj.InternalResultFcn = [];
return;
end
% Make sure this is a valid function handle and parse any user data
[funcHandle, userData] = obj.Parser.parseFcnSetterInput(fcnHandle, 'ResultFcn');
obj.ResultListener.Callback = {@robotics.ros.internal.action.ActionCallbacks.onActionResultCallback, ...
obj, obj.ResultListener, obj.ResultMessageType, funcHandle, userData};
obj.InternalResultFcn = fcnHandle;
end
function activationFcn = get.ActivationFcn(obj)
activationFcn = obj.InternalActivationFcn;
end
function set.ActivationFcn(obj, fcnHandle)
%set.ActivationFcn Setter for ActivationFcn callback property
if isempty(fcnHandle)
% Allow empty input to disable the callback
obj.ActivationListener.Callback = [];
obj.InternalActivationFcn = [];
return;
end
% Make sure this is a valid function handle and parse any user data
[funcHandle, userData] = obj.Parser.parseFcnSetterInput(fcnHandle, 'ActivationFcn');
obj.ActivationListener.Callback = {@robotics.ros.internal.action.ActionCallbacks.onActionActivationCallback, ...
obj, obj.ActivationListener, funcHandle, userData};
obj.InternalActivationFcn = fcnHandle;
end
end
methods (Access = protected)
function onNodeShutdownComplete(obj, ~, ~)
%onNodeShutdownComplete Called when the Java node finishes shut down
% This callback is triggered when the node that this
% action client is attached to shuts down.
% Set property to empty, so that delete function finishes cleanly
% cleanly.
obj.JavaActionClient = [];
obj.delete;
end
end
methods (Static, Access = ?matlab.unittest.TestCase)
function fcn = defaultActivationFcn
%defaultActivationFcn Default initialization for ActivationFcn
fcn = @(~) disp('Goal active');
end
function fcn = defaultFeedbackFcn
%defaultFeedbackFcn Default initialization for FeedbackFcn
fcn = @(~,msg) disp(['Feedback: ', showdetails(msg)]);
end
function fcn = defaultResultFcn
%defaultResultFcn Default initialization for ResultFcn
fcn = @(~,res) disp(['Final state ' res.State ' with result: ' ...
showdetails(res.Message)]);
end
end
end