
---------------------------------------
TurtleBot.m class Release Notes (v08)
---------------------------------------
Autonomous Robotic Systems 2023
Department of Electrical and Computer Engineering
University of Coimbra
Pedro Martins, pedromartins@isr.uc.pt


# ----------------------------------------------------------------------------- 
# Overview of Available Methods
# ----------------------------------------------------------------------------- 

# ---------------------------
## --- Constructor --- 
# ---------------------------
tbot = TurtleBot();
tbot = TurtleBot(IP_TURTLEBOT, IP_HOST_COMPUTER);   % <- recomended approach 

# setup custom sensors-state message (support only in Linux64 and Mac systems)
tbot = TurtleBot('check-versions');                % see package versions 
tbot = TurtleBot('setup-path');                    % setup PATH locations  
tbot = TurtleBot('setup-sensors-msg');             % setup sensors-state custom message 
tbot = TurtleBot('check-sensors-msg');             % check if message is available

# ---------------------------
## --- Query system ---
# ---------------------------
# get version number
vs = tbot.getVersion(); 

# get wheel baseline (distance between wheels) [in meters]
baseline = tbot.getWheelBaseline();

# get wheel radius [in meters]
radius = tbot.getWheelRadius();

% get number of encoder ticks per revolution
tk = tbot.getNumberTicksPerRevolution();

% return true if TurtleBot object is connected to a real robot, false otherwise 
flag = tbot.isRealRobot();

% get Battery Level (in percentage: [0, 1])
percentage = tbot.getBatteryLevel();

# ---------------------------
## --- Robot control ---
# ---------------------------
% Send linear and angular velocity commands to robot  
tbot.setVelocity(v, w);

% Stop robot 
tbot.stop();

% read 2D pose + timestamp
[x, y, theta, ptimestamp] = tbot.readPose();

% read 2D pose + timestamp (overload of readPose)
[x, y, theta, ptimestamp] = tbot.getPose();

% define/overwrite pose of robot 
tbot.setPose(x, y, theta);

% reset pose: (x,y,theta) = (0,0,0) 
tbot.resetPose();

# ---------------------------
## --- Read sensors ---
# ---------------------------
% read (simulated) encoders data 
[dsr, dsl, pose2D, timestamp] = tbot.readEncodersData();

% read (simulated) encoders data + Gaussian Noise
[dsr, dsl, pose2D, timestamp] = tbot.readEncodersDataWithNoise(noise_std);

% read (real) encoders ticks - only available w/ connected to the real Robot
[left_ticks, right_ticks, timestamp] = tbot.readEncodersTicks();

% read lidar data
[scanMsg, lddata, ldtimestamp] = tbot.readLidar();

% read lidar data (overload of readLidar)
[scanMsg, lddata, ldtimestamp] = tbot.getLidarData();

% in of range lidar data indexs (obstacles)
[vidx] = tbot.getInRangeLidarDataIdx(lddata);

% out of range lidar data indexs (free angles)
[nidx] = tbot.getOutRangeLidarDataIdx(lddata);

% read inertial data
[imuMsg, a, w, angles] = tbot.readIMU();

% read inertial data (overload of readIMU)
[imuMsg, a, w, angles] = tbot.getIMUData();

% read compass orientation data (IMU) 
[phi, angles] = tbot.readCompassData();

% read compass orientation data (IMU) (overload of readCompassData)
[phi, angles] = tbot.getCompassData();



# ----------------------------------------------------------------------------- 
# 3D Maps Notes
# ----------------------------------------------------------------------------- 

-------------------------------------------------
Maps files:
-------------------------------------------------

Seven maps are provided, with names: {bxmap, csqmap, fmap, lcmap, scmap, stmap and umap}.
In all cases, the working area is 4x4 meters.
Several occupation grids are available: 

- "map"_grid1.png -> each image pixel represents 1x1 cm of working area (400x400x3 image).
- "map"_grid5.png -> each image pixel represents 5x5 cm of working area (80x80x3 image).
- "map"_grid10.png -> each image pixel represents 10x10 cm of working area (40x40x3 image).

Each map also contains a .svg file (map image in vector format) and the corresponding 3D model to load in the Gazebo simulator (folder "gzb_models"). 


-------------------------------------------------
3D Maps Installation:
-------------------------------------------------

[Install]: 
1) Copy the entire "maps" folder to the virtual machine (to any location): 
- Some virtual machines support simple drag-and-drop functionality.  
- Windows users can use WinSCP (https://winscp.net/) to copy data between you computer and the virtual machine (use a sftp connection). 
- MacOs users can use the free Cyberduck App. 
- Linux users just need to write in the location path: sftp://user@192.168.1.xxx (replace with the IP of virtual machine). 
    
2) Open terminal, navigate to the directory location of the maps folder, and run the install script as:   
>> user@ubuntu:~$ sh install_maps.sh
The message "---done---" should appear upon a successful installation. 
The home folder (/home/user) should hold eight new scripts (start-gazebo-"map".sh). 


[Uninstall]: 
Similarly, all maps and respective configuration files can be removed by running the uninstall script. 
>> user@ubuntu:~$ sh uninstall_maps.sh


-------------------------------------------------
Start up Gazebo+ROS with a 3D map 
-------------------------------------------------

Run, in the command line, the script associated with the desired map:    

>> user@ubuntu:~$ ./start-gazebo-bxmap.sh 
>> user@ubuntu:~$ ./start-gazebo-csqmap.sh 
>> user@ubuntu:~$ ./start-gazebo-fmap.sh
>> user@ubuntu:~$ ./start-gazebo-lcmap.sh
>> user@ubuntu:~$ ./start-gazebo-scmap.sh       # <- use this map lab 04
>> user@ubuntu:~$ ./start-gazebo-stmap.sh
>> user@ubuntu:~$ ./start-gazebo-umap.sh    


-------------------------------------------------
Manually place a 3D map in Gazebo (only if required)
-------------------------------------------------

Start up Gazebo+ROS with a empty world: user@ubuntu:~$ ./start-gazebo-empty.sh 

Stop the simulation (in the bottom-left "play" button).

Click in the "Insert Tab" in the left menu (search) and select the desired map.  
Upon loading the model, manual placing (with the mouse) is required. Place it anywhere. 
In order to match the 3D model of the map with the 2D grid projections the following pose offsets are required:

- bxmap: pose offset: x = 2; y = 2; z = 0 (meters)
- csqmap: pose offset: x = 2; y = 2; z = 0 (meters)       
- fmap: pose offset: x = 2.1; y = 2.1; z = 0 (meters)
- lcmap: pose offset: x = 2; y = 2; z = 0 (meters)
- scmap: pose offset: x = 2; y = 2; z = 0 (meters)
- stmap: pose offset: x = 2; y = 2; z = 0 (meters)
- umap: pose offset: x = 1.75; y = 2; z = 0 (meters) 

Go back to the "World" tab, in the left menu. Select the corresponding map model in the "Models" section, and edit the pose property (x,y,z) with the information above.  

Start the simulation.   



# ----------------------------------------------------------------------------- 
# Operate with real TurtleBot robot 
# ----------------------------------------------------------------------------- 

1) Connect to a robot via secure-shell protocol (ssh): 
>> ssh pi@192.168.1.200 	# or 192.168.1.201 for 2nd robot   

2) Startup ROS server
>> roslaunch turtlebot3_bringup turtlebot3_robot.launch 

Wait for startup, while a set of log messages are being shown. Proceed after receiving ‘[INFO][xxx]: Calibration End’ status. The ROS server is now running.
The shell session should remain open during the entire operation with the robot. 
For terminate ROS server, press: [ctrl + c] 

3) Connect using MatLab 
Update your code with updated connection settings:
IP_TURTLEBOT = "192.168.1.200";        		% TurtleBot IP
IP_HOST_COMPUTER = "192.168.1.xxx";    		% local machine IP
tbot = TurtleBot(IP_TURTLEBOT, IP_HOST_COMPUTER);	% init connection



# ----------------------------------------------------------------------------- 
# Compile SensorState.msg ROS Message (access to encoders data w/ real TurtleBot)
# ----------------------------------------------------------------------------- 

Only supported in linux64 and mac64 systems.  Requires python** and cmake. 
(** Matlab2021b requires python 2.7. Matlab2022b requires python 3.9).     

- Check which python and cmake versions are installed (and detected by MatLab):
TurtleBot(‘check-versions’);  
If packages are installed but not detected, go to the setupPath() function (in TurtleBot.m ) and modify the PATH settings according to your system. 
After that run: TurtleBot('setup-path'); 

- Compile auxiliar sensors-state ROS message: 
TurtleBot('setup-sensors-msg'); 

- Check if new ROS message is available:
TurtleBot('check-sensors-msg'); 





# ----------------------------------------------------------------------------- 
# Versions Update Notes
# ----------------------------------------------------------------------------- 

# ------------------------------------- 
# v08 (01/02/2023)
# ------------------------------------- 

- added readEncodersTicks() function, that provides access to real encoder data (when connect to real robot). It requires setup of SensorState.msg custom message, see bellow. 

- (*) build custom SensorState.msg by calling constructor w/ single argument (options):  
    - TurtleBot('check-versions');                % see package current python & cmake versions 
    - TurtleBot('setup-path');                    % setup PATH locations (runs setupPath() function)  
    - TurtleBot('setup-sensors-msg');             % setup sensors-state custom message 
    - TurtleBot('check-sensors-msg');             % check if custom message is available

    - added auxiliar custom message (sensors-state) setup functions
        - setupPath();
        - setupSensorStateMessage();

    - (*) Note: available only in linux64 and mac64 systems. 

- the getNumberTicksPerRevolution() provides the amount of ticks per revolution 

- added readCompassData() function. 

- overload of several functions:  
    - getPose(.) -> readPose(.)
    - getLidarData(.) -> readLidar(.)
    - getIMUData(.) -> readIMU(.)
    - getCompassData(.) -> readCompassData(.)

- improved class Destructor, added rinit flag

- updated drawTurtleBot(.) w/ content-obscured p-code. 


# ------------------------------------- 
# v07 (04/01/2023)
# ------------------------------------- 

- updated settings for two real TurtleBots:
    - The 1st TurtleBot has a IP Address: 192.168.1.200
    - The 2nd TurtleBot has a IP Address: 192.168.1.201
- revised demos (demoPose.m, demoLidar.m, demoKeyboardOperation.m)
- removed deprecated function readLidar_()
- revised maps settings:
    -> updated maps visual settings (all maps).
    -> renamed map (csquare -> csqmap).
    -> updated installation scripts.


# ------------------------------------- 
# v06 (03/06/2022)
# ------------------------------------- 

- support for resetPose() and setPose(.) while using the real TurtleBot robot.
- new isRealRobot() logical function that returns true, if connected to the real robot.
- new getBatteryLevel() function that returns the battery life (in percentage).
- added getWheelRadius(.) that returns the wheel radius.
- added readIMU() function that reads inertial IMU data (gyroscope and accelerometer).
- bug fixes (readEncodersData from v05b).
- added demo file: demoKeyboardSpeedCtl.m that allows to manually control the turtlebot's speed using the keyboard keys. 

- map updates:
    - extra 3D/2D maps (bxmap, mvmap and lcmap).
    - updated installation scripts.


# ------------------------------------- 
# v05 (09/05/2022)
# ------------------------------------- 

- new readEncodersData() function that returns (simulated) incremental motion for each wheel.
- new readEncodersDataWithNoise(noise_std) that allows to inject noise in encoders data.
- new getWheelBaseline() that returns the baseline distance between wheels (b).  
- minor update to drawTurtleBot(.) external file (optional).

- map updates: 
    -> new 3D/2D map (scmap).  
    -> installation scripts for 3D maps & respective configuration files.
    -> support for automatic placement of maps in Gazebo environment. 
    -> run scripts to startup everything at once (user@ubuntu:~$ ./start-gazebo-scmap.sh).  

Additional notes: 

- the function [dsr, dsl, pose2D, timestamp] = readEncodersData(), returns: 
    dsr - incremental motion of the right wheel (in meters)
    dsl - incremental motion of the left wheel (in meters)
    pose2D - [x,y,theta] 2D pose given by the turtlebot's odometry model
    timestamp - data reading timestamp (in seconds)

-> the pose2D vector can't be used for pose estimation purposes in lab04   

-> the readEncodersData() and readPose() can't be used together in a loop. However, a single call of readPose(.) is required to startup the initial (internal) data of readEncodersData(), see the template bellow:

-> % recomended template for lab04 
tbot = TurtleBot(.)     % init TurtleBot
tbot.setPose(.);        % set initial pose (don't use resetPose(.) in scmap)
tbot.setVelocity(.);    % set velocities
[.] = tbot.readPose();  % read pose once (required to startup the encoder data) 
r = rateControl(5);     % init ratecontrol obj (loop at 5Hz)
tic;  
while (toc < 30)        % run for a given time (s)  
        [.] = tbot.readEncodersData();      % read data from encoders
        [.] = tbot.readLidar();             % read lidar data

        % ...

        % Display 
        plot(.)
        waitfor(r);     % adaptive pause
end 
tbot.stop();            % stop robot

  



# ------------------------------------- 
# v04 (09/04/2022) 
# ------------------------------------- 

TurtleBot.m class v04 update:  

- updated readLidar() functionality (added polar data readings, data selection and timestamp, see bellow).
- new lidar data selection functions: getInRangeLidarDataIdx(.) and getOutRangeLidarDataIdx(.).  
- the previous readLidar() routine is kept for backward compatibility, now called 'readLidar_()'.  
- added timestamp to readPose() and readLidar().
- reserved a IP for a 2nd TurtleBot robot.

- external file w/ drawTurtleBot(.) function that allows to display a robot in a matlab figure (optional).  


Additional notes: 

The lidar readings data (lddata) is now a struct with the following fields:   
    lddata.Ranges - (radial/polar) distance measurement [meters] (360 x 1) vector.
    lddata.Angles - angular measurement [radians] (360 x 1) vector.
    lddata.Cartesian - X/Y cartesian data (360 x 2) matrix.
The lsdata (struct field) arrays could have 'Inf' (or zero) values to represent no laser reflections (representing too near or too far readings). Use getInRangeLidarDataIdx(.) and getOutRangeLidarDataIdx(.) functions to select the desired data (see demoLidar.m). 

The pose reading and lidar scan now have timestamps. The diference between these measurements (together with the current estimate of the velocity) can be used to minimize the lidar data deviation when converting from lidar (centred) readings into world coordinates. 



# ------------------------------------- 
# v03 (13/03/2022) 
# ------------------------------------- 

TurtleBot.m class v03 update:  

- Updated setPose(x,y,theta) and resetPose() functions (improvements and bug fixes).
- The setVelocity(v,w) now clips the velocities (v,w) by the maximum allowed by the robot (v_max = 0.22 m/s, w_max = 2.84 rad/s).
- The readLidar() function also returns readings to Cartesian coordinates.

- map updates (see Map Notes.txt) 
    - 4 startup maps (csquare, fmap, stmap and umap).  
    - manual map placement under the Gazebo environment.

# ------------------------------------- 
# v02 (25/02/2022) 
# ------------------------------------- 

TurtleBot.m class v02 update: 

- Usability improvements in the constructor:  the robot's object can be initialized as tbot = TurtleBot(IP_TURTLEBOT, IP_HOST_COMPUTER) directly (however, the standard constructor remains available). 
- All angles are now expressed in radians. Please make the necessary adjustments on your code. 
- New resetPose() method that allows to reset the location to the robot in gazebo (without the need of manual intervention).
- New setPose(x,y,theta) method that allows to place the robot at a given pose. 
- Note: resetPose() and  setPose(.) functions only work when using gazebo (not in the real TurtleBot robot). 

The previously provided demos (demoPose.m and demoLidar.m) were also been updated with some of these features.  


# ------------------------------------- 
# v01 (14/02/2022) 
# ------------------------------------- 

- initial release. 

