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
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号