please explain the code>
8 views (last 30 days)
Show older comments
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 Comments
Answers (0)
See Also
Categories
Find more on Robotics in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!