Waypoint_nav.m

clear all

close all

% initialize

rosshutdown % to 'close' any previous sessions

rosinit('192.168.139.130'); % initialize Matlab ROS node

tbot = turtlebot % the data structure that allows access to the turtlebot and its sensors

% this command sets the current robot pose estimate to zero (it does not

% move the robot). This is equivalent to setting the ``world coordinate

% frame'' to coincide with the current robot frame.

resetOdometry(tbot)

% the variable robot_poses stores the history of robot pose received from odometry. It

% is declared as global for computational efficiency

global robot_poses

% we initialize robot_poses as an empty matrix with 10000 columns.

% This can store up to 1000 seconds of odometry, received at 10Hz.

robot_poses = zeros(4,10000);

% create a Matlab "timer" for receiving the odometry. This will effectively create a new thread

% that executes 'curr_pose = get_pose_from_tbot_odometry(tbot);' every 100 msec.

% this will return the current robot pose in the variable curr_pose, and

% will also store the current pose, together with its timestamp, in robot_poses

odometry_timer = timer('TimerFcn','curr_pose = get_pose_from_tbot_odometry(tbot);','Period',0.1,'ExecutionMode','fixedRate');

% start the timer. This will keep running until we stop it.

start(odometry_timer)

% these are the variables that are used to define the robot velocity

lin_vel = 0; % meters per second

rot_vel = 0; % rad/second

% create a Matlab "timer" for continuoulsy sending velocity commands to the robot.

% This will create a new thread that runs the command setVelocity(tbot,lin_vel,rot_vel) every 100msec

velocity_command_timer = timer('TimerFcn','setVelocity(tbot,lin_vel,rot_vel)','Period',0.1,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(velocity_command_timer)

%% We should experiment with two sets of waypoints... choose which one we want to use:

waypoint_set =2; % 1 or 2

if waypoint_set==1

% we make the robot move on a square

% the size of the square

square_size = 1;

% generate the waypoints

dp = .5; % spacing between waypoints

pp = [0:0.5:square_size];

waypoints = [pp;zeros(size(pp))];

waypoints = [waypoints [square_size*ones(size(pp));pp]];

waypoints = [waypoints [pp(end:-1:1);square_size*ones(size(pp))]];

waypoints = [waypoints [zeros(size(pp));pp(end:-1:1)]];

end

if waypoint_set==2

% a set of waypoints that create a more interesting trajectory

waypoints = [[0;0] [0;-1] [0.6;-1] [ 0.6;0] [1.6;0] [1;0] [1;-1] [1.6;-1] [2;-1] [2;0] [2.6;0] [2.6;-.5] [2;-.5] [2.6;-1]];

end

% create a Matlab timer for plotting the trajectory

plotting_timer = timer('TimerFcn','plot_trajectory(robot_poses, waypoints)','Period',5,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(plotting_timer)

% the variable "waypoints" is a 2xN matrix, which contains the x-y

% coordinates of the N waypoints that the robot needs to go to.

% these coordinates are in the "world" coordinate frame.

% the number of waypoints

N_waypoints = size(waypoints,2);

Plot_trajectory.m

function plot_trajectory(robot_poses, waypoints)

max_ind = max(find(robot_poses(4,:)));

figure(121)

if nargin==2

plot(waypoints(1,:),waypoints(2,:),'r*-')

hold on

end

plot(robot_poses(1,1:max_ind),robot_poses(2,1:max_ind),'b.')

axis equal

xlabel('x (m)')

ylabel('y (m)')

title('Robot trajectory')

figure(122)

subplot(3,1,1)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(1,1:max_ind))

ylabel('x (m)')

title('Robot pose over time ')

subplot(3,1,2)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(2,1:max_ind))

ylabel('y (m)')

subplot(3,1,3)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1),180/pi*robot_poses(3,1:max_ind))

xlabel('Time (sec)')

ylabel('Angle (deg)')

get_pose_from_tbot_odometry.m

function curr_pose = get_pose_from_tbot_odometry(tbot)

global robot_poses

persistent call_count

%this counts how many times we've received odometry

if isempty(call_count)

call_count=1;

end

% get the odometry data

[odom,odomdata] = getOdometry(tbot);

pose = odomdata.Pose.Pose;

% x position of the robot in the world frame

x = odomdata.Pose.Pose.Position.X;

% y position of the robot in the world frame

y = odomdata.Pose.Pose.Position.Y;

% quaternion of the robot's orientation in the world frame

q= odomdata.Pose.Pose.Orientation;

% get the angle from the quaternion

phi = 2*atan2(q.Z,q.W);

% get the timestamp of the odoemtry reading

time = odomdata.Header.Stamp;

% get the time in seconds

t = time.Sec + time.Nsec/1e9;

robot_poses(:,call_count) = [x;y;phi;t];

call_count = call_count+1 ;

% return current pose also

curr_pose = [x;y;phi];

You are provided with the Matlab file waypoint-nav. m. This initializes the robot and a few timers, as described in the preceding lab (you may need to change the code for ROS/turtlebot initialization to make this work in similarly to Lab 1). Additionally, the function defines two different sets of waypoints that we want your the robot to follow. To make the robot navigate through the waypoints, we can apply the following algorithm 1. Start: set the current goal waypoint to be the first way point in the list 2. Do the following, until the robot's position is within distance dthresh from the current goal waypoint (a) Find the position of the current goal waypoint in the robot's coordinate framer (b) Find the angle that forms with the current direction of motion of the robot (c) Set the rotational velocity of the robot as w kp 6, where kp is a control gain, to be selected. The linear velocity of the robot, v is selected as a constant number 3. Once the robot reaches the current goal waypoint (within a distance of dthresh), we set the goal waypoint as the next one in the list, and repeat from step (2). If the last waypoint has been r eached, set the goal way point as the first one in the list again. The above algorithm uses a P-controller for the robot's rotational velocit similar to what was done in Lab 1. The difference is that here we are trying to make the robot move towards a specific goal point, rather than keep a fixed orientation. Implement the above algorithm, and answer the following Try different values of the control gain kp, the linear velocity v, and the distance threshold dthresh observe the behavior of the robot when it tries to follow the two sets of waypoints provided. What effect does each of these parameters have on the performance of the algorithm? As you probably saw in the above step, the algorithm does not work when the velocity of the robot is too large (what is the maximum v that will work for your implementation The reason for this is that, if the robot is moving too fast, there is not enough time to correct the robot's heading, so that it reaches the next way point. So choosing a small linear velocity v makes the algorithm more likely to succeed. However choosing a small v will also make the robot take a very long time to move through a set of waypoints which is undesirable. Propose a way to adjust the robot's linear velocity as a function of 0, that will allow successful navigation, as well as allow the robot to move quickly between waypoints. (Many possible solutions exist here! The first set of waypoints provided will make the robot move on a square. Compare the performance of the algorithm above, to the algorithms developed in the previous lab for following a square. Which one works better, and why? You are provided with the Matlab file waypoint-nav. m. This initializes the robot and a few timers, as described in the preceding lab (you may need to change the code for ROS/turtlebot initialization to make this work in similarly to Lab 1). Additionally, the function defines two different sets of waypoints that we want your the robot to follow. To make the robot navigate through the waypoints, we can apply the following algorithm 1. Start: set the current goal waypoint to be the first way point in the list 2. Do the following, until the robot's position is within distance dthresh from the current goal waypoint (a) Find the position of the current goal waypoint in the robot's coordinate framer (b) Find the angle that forms with the current direction of motion of the robot (c) Set the rotational velocity of the robot as w kp 6, where kp is a control gain, to be selected. The linear velocity of the robot, v is selected as a constant number 3. Once the robot reaches the current goal waypoint (within a distance of dthresh), we set the goal waypoint as the next one in the list, and repeat from step (2). If the last waypoint has been r eached, set the goal way point as the first one in the list again. The above algorithm uses a P-controller for the robot's rotational velocit similar to what was done in Lab 1. The difference is that here we are trying to make the robot move towards a specific goal point, rather than keep a fixed orientation. Implement the above algorithm, and answer the following Try different values of the control gain kp, the linear velocity v, and the distance threshold dthresh observe the behavior of the robot when it tries to follow the two sets of waypoints provided. What effect does each of these parameters have on the performance of the algorithm? As you probably saw in the above step, the algorithm does not work when the velocity of the robot is too large (what is the maximum v that will work for your implementation The reason for this is that, if the robot is moving too fast, there is not enough time to correct the robot's heading, so that it reaches the next way point. So choosing a small linear velocity v makes the algorithm more likely to succeed. However choosing a small v will also make the robot take a very long time to move through a set of waypoints which is undesirable. Propose a way to adjust the robot's linear velocity as a function of 0, that will allow successful navigation, as well as allow the robot to move quickly between waypoints. (Many possible solutions exist here! The first set of waypoints provided will make the robot move on a square. Compare the performance of the algorithm above, to the algorithms developed in the previous lab for following a square. Which one works better, and why