please explain the code>

11 Ansichten (letzte 30 Tage)
TAMAL KUMAR
TAMAL KUMAR am 12 Okt. 2020
Kommentiert: Rik am 12 Okt. 2020
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 Kommentare
Rik
Rik am 12 Okt. 2020
Have a read here and here. It will greatly improve your chances of getting an answer.

Melden Sie sich an, um zu kommentieren.

Antworten (0)

Kategorien

Mehr zu Robotics finden Sie in Help Center und File Exchange

Produkte


Version

R2020b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by