%% Communicate with the TurtleBot
%
%% Introduction
% This example introduces the TurtleBot® platform and the ways in which MATLAB® users can interact with it. Specifically, the code in this example demonstrates how to publish messages to the TurtleBot (such as velocities) and how to subscribe to topics that the TurtleBot publishes (such as odometry).
% The TurtleBot must be running for this example to work.
% Prerequisites: docid:robotics_examples.example-GettingStartedWithGazeboExample or docid:robotics_examples.example-GettingStartedWithRealTurtleBotExample
%
%% Hardware Support Package for TurtleBot
% This example gives an overview of working with a TurtleBot using its native ROS interface. The Robotics System 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 "TurtleBot based Robots". Alternatively, use the roboticsAddons command.
%
%% Connect to the TurtleBot
% The TurtleBot must be running. If you are using a real TurtleBot and followed the hardware setup steps in  docid:robotics_examples.example-GettingStartedWithRealTurtleBotExample, the robot is running. If you are using a TurtleBot in simulation and followed the setup steps in docid:robotics_examples.example-GettingStartedWithGazeboExample, launch one of the Gazebo® worlds from the desktop (Gazebo TurtleBot World, for instance).
% In your MATLAB instance on the host computer, run the following command. Replace the sample IP address (192.168.1.1) with the IP address of the TurtleBot. This line initializes ROS and connects to the TurtleBot.
% ipaddress = '192.168.1.1'
%
%

rosinit(ipaddress)

%
% If the network you are using to connect to the TurtleBot is not your default network adapter, you can manually specify the IP address of the adapter that is used to connect to the robot. This might happen if you use a Wireless network, but also have an active Ethernet connection. Replace IP_OF_TURTLEBOT with the IP address of the TurtleBot and IP_OF_HOST_COMPUTER with the IP address of the host adapter that is used to connect to the robot:
% rosinit('IP_OF_TURTLEBOT','NodeHost','IP_OF_HOST_COMPUTER');
% Display all the available ROS topics by entering the following command:
%
%

rostopic list

%
% If you do not see any topics, then the network has not been set up properly. Refer to the beginning of this document for network setup steps.
% If you are using Gazebo, the list looks something like this list, though there are many more topics than the ones listed here.
% <<rostopic_list.png>>
%
%% Move the Robot and Play a Sound
% You can control the movement of the TurtleBot by publishing a message to the /mobile_base/commands/velocity topic. The message has to be of type geometry_msgs/Twist and contains data specifying desired linear and angular velocities. The TurtleBot's movements can be controlled through two different values: the linear velocity along the X-axis controls forward and backward motion and the angular velocity around the Z-axis controls the rotation speed of the robot base.
% Set a variable velocity to use for a brief TurtleBot movement.
%
%

velocity = 0.1;     % meters per second

%
% Create a publisher for the /mobile_base/commands/velocity topic and the corresponding message containing the velocity values.
%
%

robot = rospublisher('/mobile_base/commands/velocity') 
velmsg = rosmessage(robot)

%
%

%* Set the forward velocity (along the X-axis) of the robot based on the
% |velocity| variable and publish the command to the robot. The TurtleBot 
% will move forward a small distance and then come to a stop. 
%

%
%

velmsg.Linear.X = velocity;
send(robot,velmsg);

%
% For safety reasons, the TurtleBot will only keep moving, if it continuously receives velocity data on the /mobile_base/commands/velocity topic.
% To view the type of the message published by the velocity topic, execute the following:
%
%

rostopic type /mobile_base/commands/velocity

%
% The topic expects messages of type geometry_msgs/Twist, which is exactly the type of the velmsg that you created above.
% To view which nodes are publishing and subscribing to a given topic, use the command: rostopic info TOPICNAME. The following command lists the publishers and subscribers for the velocity topic. MATLAB is listed as one of the publishers.
%
%

  rostopic info /mobile_base/commands/velocity

%
% OPTIONAL: If you are using the real TurtleBot, you can send sound commands to it.
%
%

  soundpub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound')
  soundmsg = rosmessage(rostype.kobuki_msgs_Sound);
  soundmsg.Value = 6;      % Any number 0-6
  send(soundpub,soundmsg);

%
%% Receive Robot Position and Orientation
% The TurtleBot uses the /odom topic to publish its current position and orientation (collectively denoted as pose). Since the TurtleBot is not equipped with a GPS system, the pose will be relative to the pose that the robot had when it was first turned on.
% Create a subscriber for the odometry messages
%
%

  odom = rossubscriber('/odom')

%
% Wait for the subscriber to return data, then extract the data and assign it to variables x, y, and z:
%
%

  odomdata = receive(odom,3);
  pose = odomdata.Pose.Pose;
  x = pose.Position.X;
  y = pose.Position.Y;
  z = pose.Position.Z;

%
% Note: If you see an error, then it is likely that the receive command timed out. Make sure that odometry is being published and that your network is set up properly.
% Display the x, y, and z values
%
%

  [x,y,z]

%
% The orientation of the TurtleBot is stored as a quaternion in the pose.Orientation variable. Use quat2eul to convert into the more convenient representation of Euler angles. To display the current orientation, theta, of the robot in degrees, execute the following lines.
%
%

  quat = pose.Orientation;
  angles = quat2eul([quat.W quat.X quat.Y quat.Z]);
  theta = rad2deg(angles(1))

%
%% Receive Image Data
% Make sure that your Kinect® camera is running. If you list the topics again with rostopic list, you can see that many topics beginning with /camera are listed. With real TurtleBot hardware, you can find the following topic:
% /camera/rgb/image_color/compressed
% To subscribe to this topic, use the following command:
%
%

  if ismember('/camera/rgb/image_color/compressed', rostopic('list'))
    imsub = rossubscriber('/camera/rgb/image_color/compressed');
  end

%
% If you are using Gazebo, the topic list is different. Use the following topic instead:
% /camera/rgb/image_raw
% To subscribe to this topic, use the following command:
%
%

  if ismember('/camera/rgb/image_raw', rostopic('list'))
    imsub = rossubscriber('/camera/rgb/image_raw');
  end

%
% After subscribing to an image topic, wait for the data and then display it with imshow.
%
%

  img = receive(imsub);
  figure
  imshow(readImage(img));

%
% In Gazebo the image looks similar to this sample:
% <<gazebo_comm_image.png>>
% An example of a real world image from the Kinect camera looks like this:
% <<turtlebot_view.png>>
% To display a continuously updating image from the Kinect camera, use the following while loop:
%
%

  tic;
  while toc < 20
      img = receive(imsub);
      imshow(readImage(img))
  end

%
%% Disconnect from the Robot
% It is good practice to clear the workspace of publishers, subscribers, and other ROS-related objects when you are finished with them:
% clear
%
% It is recommended to 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: docid:robotics_examples.example-TurtleBotBasicExplorationExample
%
%
% Copyright 2014-2015 The MathWorks, Inc.