%% Add, Build, and Remove Objects in Gazebo
% This example explores more in-depth interaction with the Gazebo® Simulator from MATLAB®. Topics include creating simple models, adding links and joints to models, connecting models together, and applying forces to bodies.
% Prerequisites: Getting Started With Gazebo and a Simulated Turtlebot
%% Connect to Gazebo®
% On your Linux® machine, start Gazebo. If you are using the virtual machine from Getting Started With Gazebo and a Simulated Turtlebot, click Gazebo Empty World on the desktop.
% Initialize ROS by replacing ipaddress with the IP address of the virtual machine. Create an instance of the ExampleHelperGazeboCommunicator class.

rosinit('http://192.168.233.133:11311')
gazebo = ExampleHelperGazeboCommunicator;

%
%% Spawn a Simple Sphere
% To create a model, use the ExampleHelperGazeboModel class. Define properties (using addLink) and spawn a ball using the spawnModel function.

ball = ExampleHelperGazeboModel('Ball')
spherelink = addLink(ball,'sphere',1,'color',[0 0 1 1])
spawnModel(gazebo,ball,[8.5,0,1])

% All units for Gazebo commands are specified in SI units. Depending on your view, you might have to zoom out to see the ball, because it is placed at [8.5, 0, 1]. Here is an image of the scene:
%
%
%% Build and Spawn Bowling Pins
% Create vectors x and y for the location of the bowling pins (in meters).

x = [1.5    1.5    1.5   1.5   2.5     2.5  2.5    3.5    3.5   4.5];
y = [-1.5  -0.5    0.5   1.5   -1      0    1     -0.5    0.5   0];

% Define a basic model for the bowling pin using the ExampleHelperGazeboModel object. Use addLink to create the cylinder and the ball.
%

pin = ExampleHelperGazeboModel('BowlPin');

link1 = addLink(pin,'cylinder',[1 0.2],'position',[0,0,0.5]);
link2 = addLink(pin,'sphere',0.2,'position',[0,0, 1.2],'color',[0.7 0 0.2 1]);

% The output of addLink produces a variable containing the assigned name of the link. These variables create the joint.
% Use addJoint to define the relationship between the two links. In this case they are attached together by a revolute joint.
%

joint = addJoint(pin,link1,link2,'revolute',[0 0],[0 0 1]);

% The arguments of the addJoint function are object, parent, child, type, limits, and axis.
% After defining bowlPin once, You can create all ten bowling pins from the preceding ExampleHelperGazeboModel. The following for loop spawns the models in Gazebo using the x and y vectors.
%

for i = 1:10    
    spawnModel(gazebo,pin,[x(i),y(i),0.7]);
    pause(1);
end

% After adding the pins to the world, it looks like this figure:
%
%
%% Remove Models
% If the TurtleBot® exists in the scene, remove it. Look in the list of models. Remove the one named mobile_base.

if ismember('mobile_base',getSpawnedModels(gazebo))
    removeModel(gazebo,'mobile_base');
end

%
%% Spawn Built-In Models
% Create an ExampleHelperGazeboModel for a Jersey barrier. The object finds this model on the Gazebo website.

barrier = ExampleHelperGazeboModel('jersey_barrier','gazeboDB');

% Spawn two Jersey barriers in the world using spawnModel.
%

spawnModel(gazebo,barrier,[1.5,-3,0]); % Right barrier
pause(1);
spawnModel(gazebo,barrier,[1.5,3,0]); % Left barrier

% Note: You need an Internet connection to spawn models that are not included in these examples. However, if you have previously spawned a model in your Gazebo simulation, it is cached, so you can spawn it later without an Internet connection.
% The scene looks like this figure:
%
%
%% Apply Forces to the Ball
% Retrieve the handle to the ball through the  ExampleHelperGazeboSpawnedModel class.

spawnedBall = ExampleHelperGazeboSpawnedModel(ball.Name,gazebo)

% Define parameters for the application of force. Here the duration is set to 1 second and the force vector is set to -75 Newtons in the x direction.
%

duration = 1; % Seconds
forcevec = [-75 0 0]; % Newtons

% Apply the force to the model using the applyForce function.
%

applyForce(spawnedBall, spherelink, duration, forcevec);
pause(5);

% Following are images of the collision and the aftermath
%
%
%
%% Remove Models and Shut Down
% Delete the models created for this example.

exampleHelperGazeboCleanupBowling;

% Clear the workspace of publishers, subscribers, and other ROS-related objects when you are finished with them.

clear

% Use rosshutdown once you are done working with the ROS network. Shut down the global node and disconnect from Gazebo.

rosshutdown

% When you are finished, close the Gazebo window on your virtual machine.
%
%% See Also
% Apply Forces and Torques in Gazebo
% Copyright 2014-2015 The MathWorks, Inc.
%