Drive the robot by a specified distance in a specified amount of time

Context and Summary

Let’s break down what is happening in this program:

  • The inputs to the function are the distance to drive, and the speed.
  • Line 7 of the program specifies that you will be publishing to the ROS topic raw_vel.
  • Line 10 defines a ROS message which will be sent to the raw_vel topic.
  • In line 13, a one by two vector with the left and right wheel velocities is assigned to the Matlab structure message.Data. In this program the velocities are defined at the input to the function, but they could also come from a velocity vector, pre-computed list, etc.
  • In line 16, we use the send command to send the data in message to the ROS topic specified by pubvel (in this case, raw_vel)
  • In line 19 we record the current time as determined by ROS using the function rostic. As explained in the text, we don’t want to use tic and toc since the simulator’s clock might run a bit slower than real time.

As soon as we use the send command, your robot will start moving according to the wheel velocities in message.Data, and will not stop until we tell it to. So, what is happening in the rest of this program? At this point, we are not using a distance sensor on the robot (we will introduce this soon).

  • Line 20 starts a loop that monitors how long the robot has been moving forward.
  • In line 21 we use the rostoc command to check how much time has elapsed since we started moving. This is very close to the amount of time the robot has been moving. We use the simple fact that distance is the product of velocity and time to find the elapsed time needed to travel the distance we specified in the function call. We say that if we have reached that maximum time, we send a zero velocity command in lines 26 and 27.
  • If we have not reached the maximum elapsed time for our distance, we stay inside the loop and keep checking the time.

Source Code Listing

download source 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

function output = driveforward(distance, speed)
% driveforward is a simple function that controls the NEATO to drive straight forward 
% at a designated speed for a set distance

%This line says we are going to publish to the topic `/raw_vel' 
%which are the left and right wheel velocities
pubvel = rospublisher('/raw_vel') 

%Here we are creating a ROS message
message = rosmessage(pubvel);  

%Set the right and left wheel velocities 
message.Data = [speed, speed]; 

% Send the velocity commands to the NEATO
send(pubvel, message);

% rostic allows us to do timing based on the simulator clock
rostic;
while 1
    % calculate the delta t from when we started moving using rostoc
    elapsed = rostoc;
    if elapsed > distance/speed % Here we are saying the if the elapsed time is greater than 
        %distance/speed, we have reached our desired distance and we should stop
        
        message.Data = [0,0]; % set wheel velocities to zero if we have reached the desire distance
        send(pubvel, message); % send new wheel velocities
        break %leave this loop once we have reached the stopping time
    end
end

end