%% Automated Parking Valet with ROS in MATLAB
% This example shows how to distribute the 
% <docid:driving_examples#mw_a648d83c-5fe4-4459-9978-5e4d0ea396e5 Automated Parking Valet> 
% application among various nodes in a ROS network. 
% Depending on your system, this example is provided for ROS and ROS 2 
% networks using either MATLAB&reg; or Simulink&reg; . The example shown here uses 
% ROS and MATLAB. For the other examples, see:
%
% * <docid:ros_ug.mw_e95b152e-b4c4-4543-9448-789527ac100f Automated Parking
% Valet with ROS in Simulink> 
% * <docid:ros_ug.mw_d2b78c5f-df0e-4e50-a12f-9f8811efe099 Automated Parking
% Valet with ROS 2 in MATLAB> 
% * <docid:ros_ug.mw_4542650a-300e-4d8e-a736-2617b35de20b Automated Parking
% Valet with ROS 2 in Simulink> 
%
% Copyright 2019 The MathWorks, Inc.

%% Overview
% This example is an extension of the 
% <docid:driving_examples#mw_a648d83c-5fe4-4459-9978-5e4d0ea396e5 Automated Parking Valet> 
% example in Automated Driving Toolbox(TM). A typical autonmous application
% has the following components. 
% 
% <<../exampleHelperROSValetComponentsOfTypicalAutonomousApplication.PNG>>
% 
% For simplicity, this example concentrates on Planning, Control, and a 
% simplified Vehicle Model. The example uses prerecorded data to
% substitute localization information. 
% 
% This application demonstrates a typical split of various functions into ROS nodes. 
% The following picture shows how the above example is split into various
% nodes. Each node: Planning, Control and Vehicle is a ROS node implementing 
% the functionalities shown as below. The interconnections between the nodes 
% show the topics used on each interconnection of the nodes.
% 
% <<../exampleHelperROSValetNodesAndTopics.PNG>>
%

%% Setup
% First, load a route plan and a given costmap used by the behavior planner 
% and path analyzer. Behavior Planner, Path Planner, Path Analyzer, Lateral 
% and Lognitudinal Controllers are implemented by helper classes, which are 
% setup with this example helper function call.
exampleHelperROSValetSetupGlobals;

% The initialized globals are organized as fields in the global structure
% |valet|.
disp(valet)

% Initialize the ROS network. 
rosinit;
masterHost = 'localhost';
%%
% Use nodes to split the functions in the application. 
% This example uses three nodes: |planningNode|, |controlNode|, and |vehicleNode|. 

%% Planning
% The Planning node calculates each path segment based on the current vehicle position.
% This node is responsible for generating the smooth path and publishes the
% path to the network. 
% 
% This node publishes these topics:
% 
% * |/smoothpath|
% * |/velprofile|
% * |/directions|
% * |/speed|
% * |/nextgoal|
% 
% The node subscribes to these topics:
% 
% * |/currentvel|
% * |/currentpose|
% * |/desiredvel|
% * |/reachgoal|
%
% On receiving a |/reachgoal| message, the node
% runs the |exampleHelperROS2ValetPlannerCallback| callback, which plans the next segment.
% 
% <<../exampleHelperROSValetPlanningNode.PNG>>
%

% Create planning node
planningNode = ros.Node('planning', masterHost);

% Create publishers for planning node. Specify the message types the first
% time you create a publisher or subscriber for a topic.
planning.PathPub = ros.Publisher(planningNode, '/smoothpath', 'std_msgs/Float64MultiArray');
planning.VelPub = ros.Publisher(planningNode, '/velprofile', 'std_msgs/Float64MultiArray');
planning.DirPub = ros.Publisher(planningNode, '/directions', 'std_msgs/Float64MultiArray');
planning.SpeedPub = ros.Publisher(planningNode,'/speed','std_msgs/Float64MultiArray');
planning.NxtPub = ros.Publisher(planningNode, '/nextgoal', 'geometry_msgs/Pose2D');

% Create subscribers for planning node
planning.CurVelSub = ros.Subscriber(planningNode, '/currentvel', 'std_msgs/Float64');
planning.CurPoseSub = ros.Subscriber(planningNode, '/currentpose', 'geometry_msgs/Pose2D');
planning.DesrVelSub = ros.Subscriber(planningNode, '/desiredvel', 'std_msgs/Float64');

% Create GoalReachSub part of planning node and provide the callback 
GoalReachSub = ros.Subscriber(planningNode, '/reachgoal', 'std_msgs/Bool');
GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetPlannerCallback(msg, planning, valet);

%% Control
% The Control node is responsible for longitudinal and lateral controllers. 
% This node publishes these topics:
% 
% * |/steeringangle|
% * |/accelcmd|
% * |/decelcmd|
% * |/vehdir|
% * |/reachgoal|
% 
% The node subscribes to these topics:
% 
% * |/smoothpath|
% * |/directions|
% * |/speed|
% * |/currentpose|
% * |/currentvel|
% * |/nextgoal|
% * |/velprofile|
%
% On receiving a |/velprofile| message, the node
% runs the |exampleHelperROS2ValetControlCallback| callback, which sends control messages to the vehicle
% 
% <<../exampleHelperROSValetControllerNode.PNG>>
%

% Create control node.
controlNode = ros.Node('control', masterHost);

% Create publishers for control node.
control.SteeringPub = ros.Publisher(controlNode, '/steeringangle', 'std_msgs/Float64');
control.AccelPub = ros.Publisher(controlNode, '/accelcmd', 'std_msgs/Float64');
control.DecelPub = ros.Publisher(controlNode, '/decelcmd', 'std_msgs/Float64');
control.VehDirPub = ros.Publisher(controlNode, '/vehdir', 'std_msgs/Float64');
control.VehGoalReachPub = ros.Publisher(controlNode, '/reachgoal');

% Create subscribers for control node.
control.PathSub = ros.Subscriber(controlNode, '/smoothpath');
control.DirSub = ros.Subscriber(controlNode, '/directions');
control.SpeedSub = ros.Subscriber(controlNode, '/speed');
control.CurPoseSub = ros.Subscriber(controlNode, '/currentpose');
control.CurVelSub = ros.Subscriber(controlNode, '/currentvel');
control.NextGoalSub = ros.Subscriber(controlNode, '/nextgoal');

% Create subscriber for |/velprofile| for control node and provide the callback function.
VelProfSub = ros.Subscriber(controlNode, '/velprofile');
VelProfSub.NewMessageFcn = @(~,msg)exampleHelperROSValetControlCallback(msg, control, valet);

%% Vehicle
% The Vehicle node is responsible for simulating the vehicle model. 
% This node publishes these topics:
% 
% * |/currentvel|
% * |/currentpose|
% 
% The node subscribes to these topics:
% 
% * |/accelcmd|
% * |/decelcmd|
% * |/vehdir|
% * |/steeringangle|


% On receiving a |/steeringangle| message, the vehicle simulator is run in 
% the |exampleHelperROS2ValetVehicleCallback| callback.
%
% <<../exampleHelperROSValetVehicleNode.PNG>>
%

% Create vehicle node.
vehicleNode = ros.Node('vehicle', masterHost);

% Create publishers for vehicle node.
vehicle.CurVelPub = ros.Publisher(vehicleNode, '/currentvel');
vehicle.CurPosePub = ros.Publisher(vehicleNode, '/currentpose');

% Create subscribers for vehicle node.
vehicle.AccelSub = ros.Subscriber(vehicleNode, '/accelcmd');
vehicle.DecelSub = ros.Subscriber(vehicleNode, '/decelcmd');
vehicle.DirSub = ros.Subscriber(vehicleNode, '/vehdir');

% Create subscriber for |/steeringangle|, which runs the vehicle simulator 
% callback.
SteeringSub = ros.Subscriber(vehicleNode, '/steeringangle', ...
    @(~,msg)exampleHelperROSValetVehicleCallback(msg, vehicle, valet));

%% Initialize Simulation
% To initialize the simulation, send the first velocity message and current pose message.
% This message causes the planner to start the planning loop
curVelMsg = rosmessage(vehicle.CurVelPub);
curVelMsg.Data = valet.vehicleSim.getVehicleVelocity;
send(vehicle.CurVelPub, curVelMsg);

curPoseMsg = rosmessage(vehicle.CurPosePub);
curPoseMsg.X = valet.currentPose(1);
curPoseMsg.Y = valet.currentPose(2);
curPoseMsg.Theta = valet.currentPose(3);
send(vehicle.CurPosePub, curPoseMsg);

reachMsg = rosmessage(control.VehGoalReachPub);
reachMsg.Data = true;
send(control.VehGoalReachPub, reachMsg);

%% Main Loop
% The main loop waits for the |behavioralPlanner| to say the vehicle reached the prepark position.    
while ~reachedDestination(valet.behavioralPlanner)
    pause(1);
end

% Show the vehicle simulation figure.
showFigure(valet.vehicleSim);

%% Park Maneuver 
% The parking maneuver callbacks are slightly different from the normal driving 
% manueuver. Replace the callbacks for the |/velprofile| and |/reachgoal| subscribers.
VelProfSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkControlCallback(msg, control, valet);
GoalReachSub.NewMessageFcn = @(~,msg)exampleHelperROSValetParkManeuver(msg, planning, valet);

pause(1);
reachMsg = rosmessage(control.VehGoalReachPub);
reachMsg.Data = false;
send(control.VehGoalReachPub, reachMsg);

% Receive a message from the |/reachgoal| topic using the subcriber. This 
% waits until a new message is received. Display the figure. The vehicle
% has completed the full automated valet manuever.
receive(GoalReachSub);

exampleHelperROSValetCloseFigures;
snapnow;

%%
% Delete the simulator and shutdown all the nodes by clearing publishers,
% subscribers and node handles.
delete(valet.vehicleSim);

% Clear variables that were created above.
clear('valet');

GoalReachSub.NewMessageFcn = [];
VelProfSub.NewMessageFcn = [];

clear('planning', 'planningNode', 'GoalReachSub');
clear('control', 'controlNode', 'VelProfSub');
clear('vehicle', 'vehicleNode', 'SteeringSub');
clear('curPoseMsg', 'curVelMsg', 'reachMsg');
clear('masterHost');

% Shutdown the ROS network.
rosshutdown;
