Simulink model for Turtlebot movement along a trajectory avoiding objects

I am working on creating a model in Simulink that allows me to control a Turtlebot waffle pi so that it moves along a 6-point path. Along the path there will be obstacles in the form of red and green spheres. The objective is for the robot to reach all the points of the path avoiding the obstacles, if the sphere is red, avoid to the right and if it is green, avoid to the left.

I have already created the following Simulink model where the most important thing is in the control function since it describes the behavior of the robot, however the error that keeps appearing is the following: Error: Block ''Project_3/MATLAB Function'' does not fully set the dimensions of output 'next_index'. Which I do not find sense since the variable next_index is defined from the beginning. I attach the images of the model that I am creating.

function [area, x_obst, y_obst, target_x, target_y, obstacle, current_index, target_theta,last_point] = WaypointSelector(a, cent, waypoints, current_index)
%Extract obstacle's area and position
[~,i] = max(a);
area = max(a);
if ~isempty(i)
    obstacle = cent(i,:);
else
    obstacle = [0,0];
end
% Cordanates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Extract target waypoint
target_x = waypoints(current_index, 1);
target_y = waypoints(current_index, 2);
last_point = size(waypoints, 1);
target_theta = waypoints(current_index, 3);
target_theta = atan2(sin(target_theta), cos(target_theta));

end

CONTROL FUNCTION:

function [v, omega, next_index] = ComputeControl(target_x, target_y, obstacle, current_index, target_theta, last_point, current_x, current_y, x_ang, y_ang, z_ang, w_ang, ranges)

    % Constants
    max_velocity = 0.2; % max linear velocity
    max_yaw_rate = 0.4; % max angular velocity
    max_distance = 0.05; % max distance to consider waypoint reached
    l = 640; % size of the camera's image
    h = 480;

    % Initialize output
    next_index = int32(current_index);  % Initialize as integer (or the appropriate type)
    
    % Coordinates of the obstacles
    x_obst = obstacle(1);
    y_obst = obstacle(2);

    % Position and direction of the robot
    current_angle = quat2eul([x_ang y_ang z_ang w_ang]);
    yaw_angle = current_angle(3);
    if yaw_angle > pi
        current_theta = yaw_angle - 2*pi;
    else
        current_theta = yaw_angle;
    end

    % Compute distance and angle to the target waypoint
    dx = target_x - current_x;
    dy = target_y - current_y;
    distance = sqrt(dx^2 + dy^2); % Distance to the waypoints
    dist_obs = min(ranges); % Distance to the closest obstacle (Radar)
    angle_to_target = atan2(dy, dx); % Angle needed to reach the waypoints

    % Adjust the angle to be in the range [-pi, pi]
    if dx < 0
        if dy > 0
            angle_to_target = angle_to_target + pi;
        else
            angle_to_target = angle_to_target - pi;
        end
    end

    % Compute delta theta and the error in the direction
    d_theta = target_theta - current_theta;
    angle_error = current_theta - angle_to_target;

    % Compute the control logic commands to achieve the waypoints and avoid the obstacles
    if (x_obst - l/2) > 0
        omega = max_yaw_rate;
        v = max_velocity;
    else
        if current_index > last_point
            v = 0;
            omega = 0;
            next_index = current_index;
        else
            if distance <= max_distance && abs(d_theta) <= 0.05
                next_index = current_index + 1;
            else
                next_index = current_index; % Ensure next_index is assigned
            end
            if distance > max_distance
                if abs(angle_error) > 0.01
                    if abs(angle_error) > 0.5
                        omega = max_yaw_rate;
                    elseif abs(angle_error) > 0.3 && abs(angle_error) <= 0.5
                        omega = 0.5 * max_yaw_rate;
                    else
                        omega = 0.1 * max_yaw_rate;
                    end
                    if current_theta > angle_to_target && current_index < 7
                        omega = -omega;
                    end
                    if distance >= 3 * max_distance
                        v = max_velocity;
                    else
                        v = max_velocity * (distance / (distance + 1));
                    end
                else
                    omega = 0;
                    if distance >= 3 * max_distance
                        v = max_velocity;
                    else
                        v = max_velocity * (distance / (distance + 1));
                    end
                end
            else
                if abs(d_theta) > 0.05
                    omega = max_yaw_rate * d_theta;
                    v = 0;
                    if current_theta > target_theta
                        omega = -omega;
                    end
                else
                    omega = 0;
                    v = 0;
                end
            end
        end
    end

    % Limit the velocities just in case
    v = min(v, max_velocity);
    omega = min(max(omega, -max_yaw_rate), max_yaw_rate);
end

[Aravind]

Hi,

Can you please share your Simulink files and other related files?

[David Ricardo]

Of course. Attached here.

Answer 1

[Aravind]

Hi,

In your Simulink model, I noticed that you have two “MATLAB Function” blocks arranged in series. When “MATLAB Function“ blocks are placed in series, it can often be challenging for the solver to determine the appropriate signal dimension sizes if the output sizes are set to “inherited”.

To address this issue, you can manually specify the signal sizes by following these steps:

1. Click into the “MATLAB Function” block. This will open a script within MATLAB.
2. Under the 'FUNCTION' tab, there should be a button labeled 'Edit Data'. Click on it.
3. Select the variable that is used as an output from the “MATLAB Function” block.
4. Change the 'size' from -1, which is “inherited”, to the dimension that the output should be. In your current example, the 'size' of “next_index” must be set to 1.
5. Specify the dimensions for the other outputs if needed as well.

I have attached a screenshot of the above procedure for your reference. Hope this helps!

Answer 2

[David Ricardo]

Thank you. I worked perfectly!