please explain the code>
10 vues (au cours des 30 derniers jours)
Afficher commentaires plus anciens
rosinit
sim = ExampleHelperRobotSimulator('complexMap');
setRobotPose(sim, [4 2 -pi/2]);
enableROSInterface(sim, true);
sim.LaserSensor.NumReadings = 50;
scanSub = rossubscriber('scan');
[velPub, velMsg] = rospublisher('/mobile_base/commands/velocity');
tftree = rostf;
pause(1);
path = [4.5, 2; 3.5 4; 8 5.75; 1 10; 1 14; 9 15; 5 18; 9 15; 6.5 12; 9 15; 14 15; 12 18; 14 15; 13 12; 14 15; 16 15; 16 12; 16 15; 20 19.5; 25 16; 20 9;
15.5 5.5; 25 4; 16 4; 16 1; 16 4; 13.5 4; 13.5 2; 13.5 8; 13.5 4; 8 4; 8 2];
plot(path(:,1), path(:,2),'k--d');
controller = robotics.PurePursuit('Waypoints', path);
controller.DesiredLinearVelocity = 0.4;
controlRate = robotics.Rate(10);
goalRadius = 0.1;
robotCurrentLocation = path(1,:);
robotGoal = path(end,:);
distanceToGoal = norm(robotCurrentLocation - robotGoal);
%pyenvmap = robotics.OccupancyGrid(14,13,20);
map=robotics.OccupancyGrid(25,20,20);
figureHandle = figure('Name', 'Map');
axesHandle = axes('Parent', figureHandle);
mapHandle = show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Update 0');
updateCounter = 1;
while(distanceToGoal>goalRadius)
scan = receive(scanSub);
pose = getTransform(tftree, 'map', 'robot_base', scan.Header.Stamp,'Timeout', 2);
position = [pose.Transform.Translation.X,pose.Transform.Translation.Y];
orientation = quat2eul([pose.Transform.Rotation.W,pose.Transform.Rotation.X,pose.Transform.Rotation.Y, pose.Transform.Rotation.Z], 'ZYX');
robotPose = [position, orientation(1)];
ranges = scan.Ranges;
angles = scan.readScanAngles;
ranges(isnan(ranges)) = sim.LaserSensor.MaxRange;
insertRay(map, robotPose, ranges, angles,sim.LaserSensor.MaxRange);
[v, w] = controller(robotPose);
velMsg.Linear.X = v;
velMsg.Angular.Z = w;
send(velPub, velMsg);
if ~mod(updateCounter,50)
mapHandle.CData = occupancyMatrix(map);
title(axesHandle, ['OccupancyGrid: Update ',num2str(updateCounter)]);
end
updateCounter = updateCounter+1;
distanceToGoal = norm(robotPose(1:2) - robotGoal);
waitfor(controlRate);
end
show(map, 'Parent', axesHandle);
title(axesHandle, 'OccupancyGrid: Final Map');
rosshutdown
displayEndOfDemoMessage(mfilename)
2 commentaires
Réponses (0)
Voir également
Catégories
En savoir plus sur Robotics dans Help Center et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!