%% Automated Parking Valet with ROS 2 in MATLAB
% This example shows how you can distribute Automated Parkeing Valet
% application among various nodes in a ROS network. This example extends
% <docid:tbd Automated Parking Valet> example. 
%
% Copyright 2019 The MathWorks, Inc.

%% Overview
% 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 pre-recorded data to
% substitutes Localization. This example is an extension to [LINK]
% Automated Parking Valet[LINK] example in Automated Driving Toolbox. This
% example 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. Each of the shaded blue areas represent 
% a ROS node. Each node will subscribe to various topics specified in the input 
% and call callback that implements the functionality depicted. 
% The picture also shows the topics used by the nodes to publish and subscribe. 
% 
% <<../exampleHelperROSValetNodesAndTopics.PNG>>
%

%% Setup
% First, we load routeplan and 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. As part 
% of the setup these cleasses are initialized in the helper below.
exampleHelperROSValetSetupGlobals;

% The initialized globals are organized as fields in the global structure
% 'valet'
valet

%% Create ROS nodes
% Next we create nodes to split the functions in the application. We create
% planning, control and vehical nodes. 

% Clear the nodes, in case they exist before creating them.
clear('planning', 'planningNode', 'GoalReachSub');
clear('control', 'controlNode', 'VelProfSub');
clear('vehicle', 'vehicleNode', 'SteeringSub');

% Planning node calculates each path segments based on current vehicle position.
% This node is responsible for generating the smooth path and publish the same.
% As a result, this node publishes smoothpath, velprofile, directions, speed and
% nextgoal topics. The node subscribes to currentvel, currentpose,
% desiredvel and reachgoal topics. On receiving the reachgoal message, the node
% runs the ros2AutomatedValetPlannerCallback, which plans the next segment.

% Create planning node
planningNode = ros2node('planning');

% Create publishers for planning node
planning.PathPub = ros2publisher(planningNode, '/smoothpath', 'std_msgs/Float64MultiArray');
planning.VelPub = ros2publisher(planningNode, '/velprofile', 'std_msgs/Float64MultiArray');
planning.DirPub = ros2publisher(planningNode, '/directions', 'std_msgs/Float64MultiArray');
planning.SpeedPub = ros2publisher(planningNode,'/speed','std_msgs/Float64MultiArray');
planning.NxtPub = ros2publisher(planningNode, '/nextgoal', 'geometry_msgs/Pose2D');

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

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

% Control node is responsible for longitudinal and lateral controllers. 
% This node publishes steeringangle, accelcmd, decelcmd, vehdir and
% reachgoal topics. The node subscribes to smoothpath, directions, speed,
% currentpose, currentvel, nextgoal and velprofile topics. 
% On receiving the velprofile message, the node
% runs the ros2AutomatedValetControlCallback which sends control messages to vehicle

% Create control node
controlNode = ros2node('control');

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

% Create subscribers for control node. Since all the message types for the
% topics are determined above, we can use the shorter version to create
% subscribers
control.PathSub = ros2subscriber(controlNode, '/smoothpath');
control.DirSub = ros2subscriber(controlNode, '/directions');
control.SpeedSub = ros2subscriber(controlNode, '/speed');
control.CurPoseSub = ros2subscriber(controlNode, '/currentpose');
control.CurVelSub = ros2subscriber(controlNode, '/currentvel');
control.NextGoalSub = ros2subscriber(controlNode, '/nextgoal');

% Create VelProfSub for control node and provide the callback
VelProfSub = ros2subscriber(controlNode, '/velprofile');
VelProfSub.NewMessageFcn = @(msg)exampleHelperROS2ValetControlCallback(msg, control, valet);

% Vehicle node is responsible for vehicle simulator. This node publishes 
% the currentvel, currentpose and subscribes to
% accelcmd, decelcmd and vehdir topics. It also subscribes to steeringangle. On
% receing steeringangle, the vehicle simulator is run in ros2AutomatedValetVehicleCallback.

% Create vehicle node
vehicleNode = ros2node('vehicle');

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

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

% Create SteeringSub which runs the vehicle simulator as part of callback
SteeringSub = ros2subscriber(vehicleNode, '/steeringangle', ...
    @(msg)exampleHelperROS2ValetVehicleCallback(msg, vehicle, valet));

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

curPoseMsg = ros2message(vehicle.CurPosePub);
curPoseMsg.x = valet.currentPose(1);
curPoseMsg.y = valet.currentPose(2);
curPoseMsg.theta = valet.currentPose(3);
send(vehicle.CurPosePub, curPoseMsg);

reachMsg = ros2message(control.VehGoalReachPub);
reachMsg.data = true;
send(control.VehGoalReachPub, reachMsg);

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

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

%% Park Maneuver 
% Parking maneuver callbacks are slightly different from normal parking lot
% drive. Hence we switch the callbacks.
VelProfSub.NewMessageFcn = @(msg)exampleHelperROS2ValetParkControlCallback(msg, control, valet);
GoalReachSub.NewMessageFcn = @(msg)exampleHelperROS2ValetParkManeuver(msg, planning, valet);
 
reachMsg = ros2message(control.VehGoalReachPub);
reachMsg.data = false;
send(control.VehGoalReachPub, reachMsg);

% Wait to receive reached final parking position message
GoalReachSub.receive;

% Take a snapshot for the example
exampleHelperROSValetCloseFigures;
snapnow;

%% Cleanup
% 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');
%% End
