%% Control the TurtleBot with Teleoperation
% This example shows keyboard control of the TurtleBot® through the use of the ExampleHelperTurtleBotCommunicator class. The instructions describe how to set up the object and how to start the keyboard control. Instructions on how to use keyboard control are displayed when the function is launched. To change parameters of the function, edit the exampleHelperTurtleBotKeyboardControl function or the  ExampleHelperTurtleBotKeyInput class. For an introduction to using the TurtleBot with MATLAB®, see the getting started examples (Get Started with a Real TurtleBot or  Get Started with Gazebo and a Simulated TurtleBot)
% Prerequisites: Communicate with a TurtleBot, Explore Basic Behavior of a TurtleBot
%
%% Hardware Support Package for TurtleBot
% This example gives an overview of working with a TurtleBot using its native ROS interface. The ROS Toolbox™ Support Package for TurtleBot based Robots provides a more streamlined interface to TurtleBot. It allows you to:
% Acquire sensor data and send control commands without explicitly calling ROS commands.
% Communicate transparently with a simulated robot in Gazebo or with a physical TurtleBot.
% To install the support package, open Add-Ons > Get Hardware Support Packages on the MATLAB Home tab and select ROS Toolbox Support Package for TurtleBot based Robots. Alternatively, use the rosAddons command.
%
%% Connect to the TurtleBot
% Make sure you have a TurtleBot running either in simulation through Gazebo® or on real hardware. Refer to Get Started with Gazebo and a Simulated TurtleBot or Get Started with a Real TurtleBot for the startup procedure. If you are using simulation, Gazebo TurtleBot World is good for exploring.
% Initialize ROS. Connect to the TurtleBot by replacing ipaddress with the IP address of the TurtleBot.

ipaddress = 'http://192.168.233.133:11311';
rosinit(ipaddress)

%
% If you are working with real TurtleBot hardware, make sure that you start the Kinect® camera. Run the following in a terminal on the TurtleBot:
% roslaunch turtlebot_bringup 3dsensor.launch
% Subscribe to the odometry and laser scan topics and make sure that you can receive messages on these topics.

handles.odomSub = rossubscriber('/odom', 'BufferSize', 25);
receive(handles.odomSub,3);
handles.laserSub = rossubscriber('/scan', 'BufferSize', 5);
receive(handles.laserSub,3);

%
% Create a publisher for controlling the robot velocity.

handles.velPub = rospublisher('/mobile_base/commands/velocity');

%
%% Control the Robot
% Run the exampleHelperTurtleBotKeyboardControl function, which allows you to control the TurtleBot with the keyboard. Mark the inserted code example as code (highlight and press 'Alt+Enter') to execute the function.
% exampleHelperTurtleBotKeyboardControl(handles);
% Following are samples of the Command Window, the world plot, and the Gazebo world after some keyboard teleoperation by the user:
%
%
%
% If you move the TurtleBot too quickly, the obstacle plotting can become messy because of relative inaccuracies in the odometry topic at high speeds. Here is an example of a messy world plot:
%
% A sample plot of a real TurtleBot moving around an office space is shown:
%
%% Disconnect from the Robot
% Once you have exited the function by pressing q, clear the publishers and subscribers on the host.

clear

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

rosshutdown

%
%% Next Steps
% Refer to the next example: Obstacle Avoidance Using TurtleBot
% Copyright 2014-2016 The MathWorks, Inc.
%