please explain the code>

8 views (last 30 days)
TAMAL KUMAR
TAMAL KUMAR on 12 Oct 2020
Commented: Rik on 12 Oct 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 Comments
Rik
Rik on 12 Oct 2020
Have a read here and here. It will greatly improve your chances of getting an answer.

Sign in to comment.

Answers (0)

Products


Release

R2020b

Community Treasure Hunt

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

Start Hunting!