%% Using ROS Bridge to Establish Communication between ROS and ROS 2
% ROS 2 is newer version of ROS with different architecture. Both the networks are separate and there is no direct communication between the nodes in ROS and ROS 2. The ros1_bridge package provides a network bridge which enables the exchange of messages between ROS and ROS 2. The bridge manages all the conversion required and sends messages across both the networks. For more information, see ros1_bridge. This example uses a virtual machine available for download at  Virtual Machine with ROS 2 Melodic and Gazebo. The ros1_bridge package is installed on this virtual machine.
% This example shows how to control the TurtleBot3 in Gazebo using keyboard commands from the MATLAB®. The Gazebo Simulator is available in ROS 1 networks only. You can use ros1_bridge to exchange the Gazebo topics such as '/odom' or '/cmd_vel' to ROS 2.
% The below diagram depicts the message exchange between ROS 1 and ROS 2 networks using ros1_bridge. The '/odom' topic contains nav_msgs/Odometry messages sent from the ROS 1 network with Gazebo. The ROS 2 node subscribes to the /odom topic that has been bridged from ROS 1 and publishes a '/cmd_vel' message based on the robot pose. The bridge then takes the '/cmd_vel' message and publishes it on the ROS 1 network.
%
%
%% Prerequisites:
% Download our Virtual Machine with ROS 2 Melodic and Gazebo.
% Connect to ROS 2 Network
%
%% Download Virtual Machine
% You can download a virtual machine image that already has ROS 2 and Gazebo installed. This virtual machine is based on Ubuntu® Linux® and is pre-configured to support the ROS 2 examples in ROS Toolbox™.
% Download our Virtual Machine with ROS 2 Melodic and Gazebo.
% Launch the virtual machine.
%% Communicate Outside Subnet
% You may need to create an XML file on the VM named DEFAULT_FASTRTPS_PROFILE.xml to configure IP addresses to communicate under different subnets (see Communicate Outside Subnet section in Connect to ROS 2 Network). In the example XML file replace <address> entries with host and VM IP addresses and replace <domainId> entry with your specified domain. Create the same file, with the same contents, on your host computer in the MATLAB current working directory.
% Example file:
% <?xml version="1.0" encoding="UTF-8"?>
% <profiles>
%    <participant profile_name="participant_win" is_default_profile="true">
%       <rtps>
%          <builtin>
%             <metatrafficUnicastLocatorList>
%                <locator/>
%             </metatrafficUnicastLocatorList>
%             <domainId>25</domainId>
%             <initialPeersList>
%                <locator>
%                   <udpv4>
%                      <address>192.168.2.147</address>
%                   </udpv4>
%                </locator>
%                <locator>
%                   <udpv4>
%                      <address>192.168.2.1</address>
%                   </udpv4>
%                </locator>
%             </initialPeersList>
%          </builtin>
%       </rtps>
%    </participant>
% </profiles>
%
%% Launch Gazebo
% On the VM desktop, click Gazebo Empty. This Gazebo world contains a Turtlebot robot, which publishes and subscribes to messages on a ROS 1 network.
%
%% Start the Bridge
% Click the ROS Bridge icon. This bridge setups publishers and subscribers for all the ROS 1 topics on a ROS 2 network.
%
% In the Terminal window, notice that the bridge is up and running.
%
% Open one more terminal and enter the following commands
% export ROS_DOMAIN_ID=25
% source /opt/ros/dashing/setup.bash
%
% Now check that Gazebo topics are present in ROS 2.
% ros2 topic list
%
% Echo the /odom topic to see messages being published.
% ros2 topic echo /odom
%
%
%% Control the TurtleBot3 from ROS 2
% In MATLAB on your host machine, set the proper domain ID for the ROS 2 network using the 'ROS_DOMAIN_ID' environment variable. The ID must be a character vector.

setenv('ROS_DOMAIN_ID', '25');

% Create a ROS 2 node. Subscribe to the odometry topic that is bridged from ROS 1.

ros2Node = ros2node('exampleNode');
handles.odomSub = ros2subscriber(ros2Node,'/odom')

% Receive the odometry messages from the bridge and use the exampleHelperGet2DPose function to unpack the message into a 2D pose. Get the start position of the robot.

odomMsg = receive(handles.odomSub);
poseStart = exampleHelperGet2DPose(odomMsg)
handles.poses = poseStart;

% Create a publisher for controlling the robot velocity. The bridge takes these messages and sends them on the ROS 1 network.

handles.velPub = ros2publisher(ros2Node,'/cmd_vel','geometry_msgs/Twist')

% Run the exampleHelperROS2TurtleBotKeyboardControl function, which allows you to control the TurtleBot3 with the keyboard. The handles input contains the ROS 2 subscriber, ROS 2 publisher, and poses as a structure. The function sends control commands on the ROS 2 network based on the keyboard inputs. The bridge transfers those messages to the ROS 1 network for the Gazebo simulator.

poses = exampleHelperROS2TurtleBotKeyboardControl(handles);

% The figure that opens listens to keyboard inputs for controlling the robot in Gazebo. Hit the keys and watch the robot move. Press Q to exit.
%
%
%
%% Plot Data Received from ROS
% Plot the results to show how TurtleBot3 moved in Gazebo. The poses variable has stored all the updated /odom messages that were received from the ROS 1 network.

odomMsg = receive(handles.odomSub);
poseEnd = exampleHelperGet2DPose(odomMsg)
poses = [poses;poseEnd];
figure
plot(poses(:,1),poses(:,2),'b-', ... 
     poseStart(1),poseStart(2),'go', ...
     poseEnd(1),poseEnd(2),'ro');
xlabel('X [m]');
ylabel('Y [m]');
legend('Trajectory','Start','End');

% Clear the publishers and subscribers on the host.

clear

% Copyright 2019 The MathWorks, Inc.
%