%% Sign Following Robot with ROS 2 in MATLAB
% This example shows you how to use MATLAB® to control a simulated robot running on a separate ROS-based simulator over ROS 2 network.
% In this example, you run a MATLAB script that implements a sign-following algorithm and controls the simulated robot to follow a path based on signs in the environment. The algorithm receives the location information and camera information from the simulated robot, which is running in a separate ROS-based simulator. The algorithm detects the color of the sign and sends the velocity commands to turn the robot based on the color. In this example, the algorithm is designed to turn left when robot encounters a blue sign and turn right when robot encounters a green sign. FInally the robot stops when it encounters a red sign.
%  To see this example using ROS 1 or Simulink®, see Sign Following Robot with ROS.
%
%% Start a Robot Simulator and ROS Bridge
% Start a ROS-based simulator for a differential-drive robot and configure the MATLAB® connection with the robot simulator. Because the simulator uses ROS 1 topics, start the ROS bridge to share the topics with a ROS 2 network.
% This example uses a virtual machine (VM) available for download at Virtual Machine with ROS 2 Melodic and Gazebo.
% Start the Ubuntu® virtual machine desktop.
% In the Ubuntu desktop, click the Gazebo Sign Follower ROS icon to start the Gazebo world built for this example.
% Click the ROS Bridge (Dashing) icon to start the ROS bridge to relay messages between your ROS 2 node and the Turtlebot3 ROS-enabled robot.
% In MATLAB Command Window, set the ROS_DOMAIN_ID environment variable to 25 to match the robot simulator ROS bridge settings and run ros2 topic list to verify that the topics from the robot simulator are visible in MATLAB.

setenv('ROS_DOMAIN_ID','25')
ros2('topic','list')

%
%% Setup ROS 2 Communication
% Create a ROS 2 node using the specified domain ID.

domainID = 25;
n = ros2node("matlab_example_robot",domainID);

% Create publishers and subscribers to relay messages to and from the robot simulator over the ROS 2 network. You need subscribers for the image and odometry data. To control the robot, set up a publisher to send velocity commands using the /cmd_vel topic.

imgSub = ros2subscriber(n, "/camera/rgb/image_raw");

odomSub = ros2subscriber(n, "/odom");

[velPub, velMsg] = ros2publisher(n, "/cmd_vel", "geometry_msgs/Twist");

% Define the image processing color threshold parameters. Each row defines the threshold values for the different colors.

colorThresholds = [100 255 0 55 0 50; ... % Red
                   0 50 50 255 0 50; ...  % Green
                   0 40 0 55 50 255]';    % Blue

%
%% Create a sign following controller stateflow chart
% This example provides an example helper MATLAB Stateflow® chart that takes in the image size, coordinates from processed image, and the robot odometry poses. The chart provides linear and angular velocity to drive the robot based on these inputs.

controller = ExampleHelperSignFollowingControllerChart;
open('ExampleHelperSignFollowingControllerChart');

%
%
%% Run Control Loop
% This section runs the controller to receive images and move the robot to follow the sign. The controller does the following steps:
% Gets the latest image and odometry message from the ROS network.
% Runs the algoritm for detecting image features (ExampleHelperSignFollowingProcessImg).
% Generates control commands from the Stateflow® chart using step.
% Publishes the velocity control commands to the ROS network.
%  To visualize the masked image the robot sees, change the value of doVisualization variable to true.

ExampleHelperSignFollowingSetupPreferences;

% Control the visualization of the mask
doVisualization = false;

r = rateControl(10);
receive(imgSub); % Wait to receive an image message before starting the loop
receive(odomSub);
while(~controller.done)
    % Get latest sensor messages and process them
    imgMsg = imgSub.LatestMessage;
    odomMsg = odomSub.LatestMessage;
    [img,pose] = ExampleHelperSignFollowingProcessMsg(imgMsg, odomMsg);
    
    % Run vision and control functions
    [mask,blobSize,blobX] = ExampleHelperSignFollowingProcessImg(img, colorThresholds);
    step(controller,'blobSize',blobSize,'blobX',blobX,'pose',pose);
    v = controller.v;
    w = controller.w;
    
    % Publish velocity commands
    velMsg.linear.x = v;
    velMsg.angular.z = w;
    send(velPub,velMsg);
    
    % Optionally visualize
    % NOTE: Visualizing data will slow down the execution loop.
    % If you have Computer Vision Toolbox, we recommend using
    % vision.DeployableVideoPlayer instead of imshow.
    if doVisualization
        imshow(mask);
        title(['Linear Vel: ' num2str(v) ' Angular Vel: ' num2str(w)]);
        drawnow('limitrate');
    end
    % Pace the execution loop.
    waitfor(r);
end

% You should see the robot moving in the ROS-based robot simulator as shown below.
%
% Copyright 2019 The MathWorks, Inc.