please explain the code>

조회 수: 8 (최근 30일)
TAMAL KUMAR
TAMAL KUMAR 2020년 10월 12일
댓글: Rik 2020년 10월 12일
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
Sudhakar Shinde
Sudhakar Shinde 2020년 10월 12일
Rik
Rik 2020년 10월 12일
Have a read here and here. It will greatly improve your chances of getting an answer.

댓글을 달려면 로그인하십시오.

답변 (0개)

카테고리

Help CenterFile Exchange에서 Robotics에 대해 자세히 알아보기

제품


릴리스

R2020b

Community Treasure Hunt

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

Start Hunting!

Translated by