Home » walking与Matlab入门教程 » walking与Matlab入门教程-介绍示例模型

walking与Matlab入门教程-介绍示例模型

walking与Matlab入门教程-介绍示例模型

说明:

  • 介绍在Matlab的示例模型

示例模型:

  • 键盘控制 exampleHelperROS2TurtleBotKeyboardControl.m
function poses = exampleHelperROS2TurtleBotKeyboardControl(handles)
keyObj = ExampleHelperTurtleBotKeyInput();
velMsg = ros2message(handles.velPub);
reply = 0;
 while reply~='q'
        forwardV = 0;   % Initialize linear and angular velocities
        turnV = 0;        
        reply = getKeystroke(keyObj);
        switch reply
            case 'i'         % i
                forwardV = 0.5;
            case 'k'     % k
                forwardV = -0.5;
            case 'j'     % j
                turnV = 1;
            case 'l'     % l
                turnV = -1;
        end
        velMsg.linear.x = forwardV;
       velMsg.angular.z = turnV;
        send(handles.velPub,velMsg);
        odomMsg = handles.odomSub.LatestMessage;
        handles.poses = [handles.poses;exampleHelperGet2DPose(odomMsg)];     
 end
 poses = handles.poses;
   closeFigure(keyObj);
end
  • 键盘控制 ExampleHelperTurtleBotKeyInput.m
classdef ExampleHelperTurtleBotKeyInput < handle
    %ExampleHelperTurtleBotKeyInput - Class for obtaining keyboard input
    %   OBJ = ExampleHelperTurtleBotKeyInput() creates a figure with instructions for Turtlebot
    %   keyboard control. The object keeps track of the figure and axes handles
    %   and converts keyboard input into ASCII values.
    %
    %   ExampleHelperTurtleBotKeyInput methods:
    %       getKeystroke            - Returns ASCII value for keystroke
    %       closeFigure             - Closes the figure and cleans up
    %
    %   ExampleHelperTurtleBotKeyInput properties:
    %       Figure                  - Stores the figure handle
    %       Axes                    - Stores the axes handle
    %
    %   See also exampleHelperTurtleBotKeyboardControl

    %   Copyright 2014-2015 The MathWorks, Inc.

    properties
        Figure = [];            % Stores the figure handle
        Axes = [];              % Stores the axes handle
    end

    methods
        function obj = ExampleHelperTurtleBotKeyInput()
            %ExampleHelperTurtleBotKeyInput - Constructor for KeyInput class
        
            callstr = 'set(gcbf,''Userdata'',double(get(gcbf,''Currentcharacter''))) ; uiresume ' ;
        
            obj.Figure = figure(...
                'Name','Press a key', ...
                'KeyPressFcn',callstr, ...
                'Position',[500 500  500 300],...
                'UserData','Timeout');
            obj.Axes = axes('Color','k','Visible','Off','XLim',[0,100],'YLim',[0,100]);
            text(50,80,'i = Forward','HorizontalAlignment','center','EdgeColor','k');
            text(50,40,'k = Backward','HorizontalAlignment','center','EdgeColor','k');
            text(25,60,'j = Left','HorizontalAlignment','center','EdgeColor','k');
            text(75,60,'l = Right','HorizontalAlignment','center','EdgeColor','k');
            text(50,20,'q = Quit','HorizontalAlignment','center','EdgeColor','r');
            text(50,0,'Keep this figure in scope to give commands','HorizontalAlignment','center');
        end
    
        function keyout = getKeystroke(obj)
            %GETKEYSTROKE - Returns ASCII value for keystroke
        
            try
                uiwait(obj.Figure);
                keyout = get(obj.Figure,'Userdata') ;
            catch
                keyout = 'q';
            end
        end
    
        function closeFigure(obj)
            %CLOSEFIGURE - Closes the figure and cleans up
        
            try
                figure(obj.Figure);
                close(obj.Figure);
            catch
            end
        end
    end

end
  • 绘制雷达数据 ExampleHelperQoSTurtleBotPlotScan.m
function ExampleHelperQoSTurtleBotPlotScan(scanMsg,plotobj,odomSub)
% Lidar subscriber's callback to plot the latest odometry and lidar sensor
% reading

odomMsg = odomSub.LatestMessage;
% laserdata = rosReadCartesian(scanMsg);
laserdata = rosReadCartesian(scanMsg);
pose = readPose(odomMsg);
plotData(plotobj,pose,laserdata * [0 1; -1 0]);
end


function pose = readPose(msg)
%readPose Extract the robot odometry reading as [x y theta] vector
poseMsg = msg.pose.pose;

% Extract the x, y, and theta coordinates
xpos = poseMsg.position.x;
ypos = poseMsg.position.y;
quat = poseMsg.orientation;
angles = quat2eul([quat.w quat.x quat.y quat.z]);
theta = angles(1);

pose = [xpos, ypos, theta];
end
  • 绘制相机数据 ExampleHelperQoSTurtleBotPlotImage.m
function ExampleHelperQoSTurtleBotPlotImage(imageMsg,AxesHandleCamera)
img = permute(reshape(imageMsg.data,[3 1280 720]),[3 2 1]); % to be updated with rosReadImage
image(AxesHandleCamera,img)
end
  • 绘制里程计数据 exampleHelperROS2PlotOdom.m
function exampleHelperROS2PlotOdom(msg,hAxes,linespec)
%exampleHelperROS2PlotOdom Subscriber callback function for odometry data
%   exampleHelperROS2PlotOdom(MESSAGE,AXES,LINESPEC) returns no arguments.
%   Instead it plots the position of the robot, using the provided marker,
%   and noting the timestamp of the message next to the position.
%
%   See also ManageQualityOfServicePoliciesInROS2Example.

%   Copyright 2019 The MathWorks, Inc.

% Set up axes
hold(hAxes,"on")

% Plot the robot position
msgTimestamp = double(msg.header.stamp.sec)+...
    double(msg.header.stamp.nanosec)*1e-9;
plot(hAxes,msg.pose.pose.position.x,msg.pose.pose.position.y,linespec)
text(hAxes,msg.pose.pose.position.x,msg.pose.pose.position.y,sprintf(" %.0d",msgTimestamp))
end
  • 获取位置信息 exampleHelperGet2DPose.m
function pose = get2DPose(odomMsg)
% GET2DPOSE Helper function that extracts [x y theta] 2D pose 
% from a ROS odometry message.
% Copyright 2018 The MathWorks, Inc.

    % Unwrap position
    position = odomMsg.pose.pose.position;
    x = position.x;
    y = position.y;

    % Unwrap orientation
    orientation = odomMsg.pose.pose.orientation;
    q = [orientation.w, orientation.x, ...
         orientation.y, orientation.z];
    r = quat2eul(q);
    theta = r(1); % Extract Z component

    pose = [x, y, theta];

end
  • 处理传感数据 ExampleHelperQoSTurtleBotSetupVisualizer.m
function [plotobj,axHandle,checkPointText,Axes] = ExampleHelperQoSTurtleBotSetupVisualizer(velPub)
% Initialize the visualizers of robot's current postion with Lidar scan,
% and camera images
plotobj = ExampleHelperQoSTurtleBotVisualizer([-2,6,-2,6]);
figHandle = figure('Name','Camera Image','Visible','on');
axHandle = axes('Parent',figHandle);
% Create a teleoperation controller visulizer
f = figure(...
    'Visible','on',...
    'Name','Press a key',...
    'KeyPressFcn',{@getKey,velPub},...
    'DeleteFcn',@deleteKey);

Axes = axes(...
    f,'Color','k',...
    'Visible','Off',...
    'XLim',[0,100],...
    'YLim',[0,100]);
text(...
    Axes,50,70,'i = Forward',...
    'HorizontalAlignment','center',...
    'EdgeColor','k');
text(...
    Axes,50,30,'k = Backward',...
    'HorizontalAlignment','center',...
    'EdgeColor','k');
text(...
    Axes,25,50,'j = Left',...
    'HorizontalAlignment','center',...
    'EdgeColor','k');
text(...
    Axes,75,50,'l = Right',...
    'HorizontalAlignment','center',...
    'EdgeColor','k');
text(...
    Axes,50,10,'s = Stop',...
    'HorizontalAlignment','center',...
    'EdgeColor','r');
text(...
    Axes,50,0,'Keep this figure in scope to give commands',...
    'HorizontalAlignment','center');
checkPointText = text(...
    Axes,20,90,'Find the next sign!',...
    'HorizontalAlignment','center',...
    'FontSize',14,...
    'FontWeight','bold',...
    'Color','b');
end

function getKey(~,eventData,velPub)
% getKey UI callback to read a key pressed and send the velocity command 
keyPressed = eventData.Key;
switch keyPressed
    case 'i'         % i
        forwardV = 0.3;
        turnV = 0;
    case 'k'     % k
        forwardV = -0.3;
        turnV = 0;
    case 'j'     % j
        forwardV = 0;
        turnV = 0.3;
    case 'l'     % l
        forwardV = 0;
        turnV = -0.3;
    case 's'
        forwardV = 0;
        turnV = 0;
end
if isempty(forwardV) || isempty(turnV)
    forwardV = 0;
    turnV = 0;
end
setVel(velPub,forwardV,turnV);
end

function deleteKey(obj,~)
obj.KeyPressFcn = [];
end

function setVel(pub,vLin,vAng)
velMsg = ros2message(pub);
velMsg.linear.x = vLin;
velMsg.angular.z = vAng;
send(pub,velMsg);
end

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: walking与matlab入门教程