Matlab与ROS----雷达

一、雷达

laser = rossubscriber('/scan');
%scan = receive(laser);
figure
%plot(scan);
tic;
while toc < 10
  scan = receive(laser,3);
  plot(scan);
end

二、避障

spinVelocity = 0.6;       % 角速度
forwardVelocity = 0.1;    % 前进角速度
backwardVelocity = -0.02; % 后退角速度
distanceThreshold = 0.6;  % 安全距离
laser = rossubscriber('/scan');
robot = rospublisher('/mobile_base/commands/velocity') ;
velmsg = rosmessage(robot);

 tic;
  while toc < 20
      % 读取雷达信息
      scan = receive(laser);
      plot(scan);
      data = readCartesian(scan);
      x = data(:,1);
      y = data(:,2);
      % 计算障碍物最近距离
      dist = sqrt(x.^2 + y.^2);
      minDist = min(dist);     
      % 机器人决策行动
      if minDist < distanceThreshold
          % 靠近，后退
          velmsg.Angular.Z = spinVelocity;
          velmsg.Linear.X = backwardVelocity;
      else
          % 前进
          velmsg.Linear.X = forwardVelocity;
          velmsg.Angular.Z = 0;
      end   
      send(robot,velmsg);
  end