%% Read Model and Simulation Properties from Gazebo
% This example shows how to interact with the Gazebo® Simulator from MATLAB®. It shows how to pause the Gazebo simulation, read the physics properties, and retrieve information about objects in the Gazebo world.
% Prerequisites: Getting Started with Gazebo and a Simulated Turtlebot
%% Connect to a Gazebo World
% Open a Gazebo world from the desktop on your virtual machine or Linux® machine. If you are using the Ubuntu® virtual machine from Getting Started with Gazebo and a Simulated Turtlebot, use Gazebo Empty or Gazebo Playground.
% In your MATLAB instance on the host computer, run the following commands, replacing ipaddress with the IP address of your virtual machine. This command initializes ROS and connects your MATLAB instance to the ROS master at the specified IP address.

ipaddress = '192.168.203.129'; % Replace with your IP
rosinit(ipaddress)

% Note: If the network you are using to connect to the simulation is not your default network, you can manually specify your IP address. The syntax is as follows. Replace IP_OF_VM with the IP address of the simulation and IP_OF_HOST_COMPUTER with the IP address of the host.
% rosinit('IP_OF_VM','NodeHost','IP_OF_HOST_COMPUTER');
%
% Show all the available ROS topics on the ROS network.

rostopic list

% If the list of topics is not displayed, then the network has not been set up properly. For network setup steps, refer to Getting Started with Gazebo and a Simulated Turtlebot.
%
% Set up a connection to the Gazebo world through the  ExampleHelperGazeboCommunicator class, which makes interaction with Gazebo easier

gazebo = ExampleHelperGazeboCommunicator;

%
%% Read Physics Properties of the Gazebo Engine
% Using the ExampleHelperGazeboCommunicator object, extract the physics properties of the simulation.

phys = readPhysics(gazebo)

% Gazebo uses SI units.
%
% To explore, change the gravity of the simulation so that it goes in the opposite direction in z. Reduce the value, too. Reset the simulation for the physics to affect all objects.

phys.Gravity = [0 0 0.1];       % Units are m/s^2
setPhysics(gazebo,phys);
resetSim(gazebo);

pause(5);                        % Let gravity take effect for 5 seconds

%
%% Pause, Unpause and Restart Simulation
% You see the TurtleBot® (and anything else in the world) slowly accelerating up and away from the ground. Pause the simulation using the ExampleHelperGazeboCommunicator.

pauseSim(gazebo);
pause(3);

% When you pause, the scene might look like this figure:
%
%
%
% Resume the simulation. See that the objects start moving up again.

resumeSim(gazebo);
pause(3);

% The TurtleBot floats up again
% The resetSim function does not reset the physics to the original values. Manually reset the physics using setPhysics.
%
%% Run Simulation at 1/8 Speed
% This section describes how to run the Gazebo simulator in slow motion. Using slow motion can be very useful for careful examination of abrupt or short-term physical interactions and behaviors.
% Run the simulation at a reduced speed by changing the update rate on the physics. You can see how it looks running at 1/8 speed.

pauseSim(gazebo);
phys = readPhysics(gazebo);
phys.UpdateRate = phys.UpdateRate/8;
setPhysics(gazebo,phys);

%
% Set gravity back to the normal value.

phys.Gravity = [0 0 -9.8];   % Set gravity back to normal value (m/s^2)
setPhysics(gazebo,phys);
resumeSim(gazebo);
pause(5);

% When you resume the simulation the TurtleBot falls back to the ground, but in slow motion.
%
% Switch the update rate back to normal so the simulation runs in real time.

phys.UpdateRate = phys.UpdateRate*8;   % Set update rate back to normal value
setPhysics(gazebo,phys);
pause(3);

%
% Reset the simulation.

resetSim(gazebo);

%
%% Obtain List of Models in Gazebo World
% The ExampleHelperGazeboCommunicator class allows you to find all models in the world.

models = getSpawnedModels(gazebo)

%
%% Obtain Links from a Model
% Retrieve specific information about a model (the Kobuki® TurtleBot® in this case) by creating an object for it with the  ExampleHelperGazeboSpawnedModel class.

kobuki = ExampleHelperGazeboSpawnedModel('mobile_base',gazebo)
[kobukiLinks, kobukiJoints] = getComponents(kobuki)

%
%
%% Read Properties of a Model - Pose and Orientation
% Using the ExampleHelperGazeboSpawnedModel object for the TurtleBot, obtain the position, orientation, and velocity by using the getState function.

[position, orientation, velocity] = getState(kobuki)

%
%% Obtain List of All Models from Gazebo Online Model Database
% Use the exampleHelperGazeboListModels function to get all Gazebo models available for download from the Gazebo website.

builtInModels = exampleHelperGazeboListModels;

%
%% Shutdown
% 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

%
%% Next Steps
% Refer to the next example: Add, Build, and Remove Objects in Gazebo
%
% Copyright 2014-2015 The MathWorks, Inc.