## Step 4: Prep to build

### Install the build dependencies for colcon

### Install the dependencies for the RAFT optical flow algorithm

### Install the dependencies for RealSense
(these are the steps from https://github.com/IntelRealSense/realsense-ros/tree/ros2-beta)

## Step 5: Build the workspace

# Using the Bubble Sensor

## ROS2 Components
### 1. Publish Depth Data: RealSense
The easiest method is to use the following script to launch a publisher for the camera.

*Option 1*

Or, if you want to set the initial parameters for the camera, you can use launch the ros2 package with arguments for realsense.

*Option 2*

### 2. Modify the RealSense camera parameters, using:

#### Option 1: rqt (dynamic reconfigure UI panel)

Running RQT is fairly straightforward. You can use the convenience script if you like.

*Option 1.A*
Open a new terminal window.

*Option 1.B*

Now that you RQT is running, you can then navigate to:

`Plugins > Configuration > Dynamic Reconfigure`

Under the `Parameter Reconfigure` panel, there is an option to set the camera properties.
Make the following changes:

#### Option 2: using ros2 param set
You can also set parameters without launching RQT. Use your terminal to run commands like:

NOTE: Make sure to use the right path for your camera. Intel's Realsense wrapper defaults to /camera/camera but that can be edited using the `start_camera1.sh` script.

## Visualizing the Depth Data
### Option 1: RQT
RQT is handy because it has built-in plots and image views. It also allows you to launch the dynamic reconfiguration and send specific messages.

The main panels to use for the bubble are:
plot: pressure data
image: color image
image: depth image
image: optical flow
plot: shear magnitude

### Option 2: ROS2 RViz
RViz also allows you to visualize the incoming data. It supports point clouds.

You can enable pointcloud data by changing:
`pointcloud.enable:=false` to `pointcloud.enable:=true`
in the start_camera1.sh (and start_camera2.sh) script

This should show you 2 image panels (RS depth and RGB) and a point cloud window.

If the punyo.rviz configuration file does not work, you can set up the pointcloud visualizer by setting the following:
* Add a PointCloud2 element
* Global: Fixed Frame

You may need to reboot after installation if you get an error about `failing to create an opengl context`

## Image Processing Node

This is an example of processing the image data to compute optical flow.

Option 1

Option 2

This will show two image panels. One shows the RGB image with superimposed flow vectors. The other shows the optical flow for the current frame relative to the baseline frame using a colorwheel visualization.

The optical flow image is published as:
`/bubble_1/flow`

The flow vectors can be used to estimate shear force. These shear force estimates may be used to trigger events.

These shear estimates are published on a topic called:
`/bubble_1/force`
and the x and y values of the shear force torque wrench are available as:

To get the magnitude of the largest shear vector, the value is available as:

You can switch between two optical flow algorithms.

In `src/punyo/punyo/config/bubble.yaml` you can switch:

Farneback

optical_flow_parameter: 1

RAFT - You can also set the Pytorch device mode

optical_flow_parameter: 2
device_parameter: cpu or cuda

## ROS2 + Micro-ROS

### 1. Getting Pressure Data into ROS 2
To get pressure data into ROS 2, we need to connect the Micro-ROS client running on the microcontroller with the ROS 2 distributed data service with the help of a XRCE-DDS agent.

Start the Micro-ros agent to publish the data received from the microcontroller (MC).

** IMPORTANT: Before running the agent**
The client running on the MC uses the references style of creating ROS2 entities. This means that you need to update the `ros.ref` to match the serial number of your microcontroller. If you run the agent without updating the `ros.ref` file, you will see an error stating that the entity was not found. 

For more details on the refs format, see https://micro.ros.org/docs/tutorials/advanced/create_dds_entities_by_ref/ or check the contents of the `start_pressure1.sh` script.
To find out the serial number you should be using, you can run the script as-is, and an error will appear in the logs indicating the serial number of your MC. Alternatively, you can check your devices under `/dev/serial/by-id` on your pc.

There isn't variable support in the refs format, and so you will need to copy/paste the serial number into 8 locations.
e.g. change `bubble_35A4E5A250555733362E3120FF091A1E` to `bubble_yourserialnumber` 

Be sure to keep the prefixes (`rt/`) and suffices (`__dr` `__dw` `__t`) intact

**IMPORTANT: Configuring whether to broadcast topics or keep them on localhost**

By default, the `start_...` scripts and the `ros.ref` configuration file will keep the topics local to the pc you are running on. You can keep the localhost only mode:

*localhost option in ref file*

			<interfaceWhiteList>
				<address>127.0.0.1</address>
			</interfaceWhiteList>set_en

*localhost option in set_env.sh script*

`export ROS_LOCALHOST_ONLY=1`


You can switch to the broadcasting mode with:

*localhost option in ref file*

*localhost option in set_env.sh script*

`export ROS_LOCALHOST_ONLY=0`

**Option 1: Using a locally-installed Micro XRCE-DDS agent.**

To install it, go the section "Installing the Agent standalone" at https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#install-agent

*NOTE: Ubuntu 22.04/jammy users*
You may run into a build issue as jammy uses Openssl 3.x and the full Micro XRCE-DDS agent build references Openssl 1.1.1 (used for Ubuntu 20.04)

To work around this, you can just build the uagent target as follows:

Once installed, run the following:

Make sure that the user you are running this under is a member of the dialout group. If you are unable to add the user to that group, you can run the command using `sudo`.

If there is a problem reading data from the serial port, your user may need to be a part of the dialout group for permissions on your serial port.

To add the current user to the dialout group:

You must log out and log in again for updated group permissions to take effect.

See the latter half of https://www.arduino.cc/en/Guide/Linux for more info.

If you can't install the MicroXRCEagent, you can use a docker image to run the agent.

**Option 2: Using a docker image to run the Micro XRCE-DDS agent**

Note: As of Jan-4-2023, there isn't a docker image built for jammy yet.

### 2. Viewing the pressure data in ROS 2

In RQT:
Navigate to Plugins > Visualization > Plot
to get a plot panel added to the RQT screen.

With the plot placeholder in RQT, you can then add the pressure data in one of two ways:
1) In the Topic path shown in the plot panel that you just added, start entering `/bubble` (note that the leading slash may already by there) so that RQT will auto-populate the drop-down with the topics that start with `/bubble`. 
Once you see entries, you can hit down-arrow to scroll through the options. Pick the one that ends in `pressure` and add `/data` to the full topic name so that the pressure data is plotted. If you don't add `/data`, the data will not be plotted.
2) You can use RQT's Topic Monitor to drag and drop the pressure `data` over to the plot panel. Go to Plugins > Topics > Topic Monitor. Click the expand arrow to expand the pressure topic. You should then see `data`. Drag and drop the `data` over to the plot. The plot should be updated automatically.



## Troubleshooting
### ROS2 localhost publishing

### Topics
To see the topics being published:

### Pressure data
Sometimes the micro-ros client on the microcontroller may need to be reset, especially if pressure data is not being sent. Check the `CLIENT_README.md` for more details.

Unplug and plug the USB hub back into your PC.

Use the reset button on the microcontroller.

Restart the `./start_pressure1.sh` script. (ctrl-c then run it again)


### Camera data
Restart the `./start_camera1.sh` script. (ctrl-c then run it again)

## Launch script

For convenience, you may want to try a shell script that launches the pressure agent, camera and flow scripts, and eventually rviz.

## Miscellaneous
If you are trying the sound demo, you will need to install the pygame package.

And confirm that the pressure topic is valid in the start_demo.sh script.