Implementation

Parts

Realsense camera
2-3 turtle bots that have the multimaster_fkie package installed
- At the time of writing, this package was only installed on the black and pink turtlebots!
One lab computer
AR Tags (1 for each bot)

Tag Implementation

ROS Multi-Master Setup & Launch Files (shell)

In order to have bots apply the transforms computed by the main thread, we created multiple shared channels using ROS's multimaster_fkie package. To do this we had to execute a series of shell commands:

SSH into every turtlebot and roslaunch the turtlebot_bringup launch file
Modify the /proc/sys/net/ipv4/icmp_echo_ignore_broadcasts file to be 0 instead of 1, enabling the bot to receive broadcasts
Run rosrun master_discovery_fkie master_discovery _mcast_group:=224.0.0.1 to establish a common IP channel for the multi-master group.
Next, running rosrun master_sync_fkie master_sync would enable every machine on this subnet to have access to the topics of other machines in the group.
To diagnose whether the multi-master was running, we used rosservice call /master_discovery/list_masters to see all the different linked hosts in a list.
We now had the ability to create topics that were visible to all bots in the subnet. To learn more about multimaster_fkie see the guide and associated PDF here.

We set up multimaster starting with localhost and moving onto each bot in the game. We then set up the realsense camera using the same launch files as Labs 4 and 6, namely rs_camera.launch from the realsense2_camera package and the ar_track.launch file from the lab4_cam package. (We imported both of these packages into our project code)

Stick Frame Integration (turtlebot_control.py)

Leveraging Joel and Jason's classifier work, we created a script that would reliably detect to which bot the stick was pointing. We ran this script as part of our main that we'd run for every bot in the game. It took in the turtlebot_ID or the color of the bot for which this script was running. As output, it gave the role assignment of that bot ("pursuer" or "evader").

The stick frame was oriented so that the x-axis pointed in the direction of the first principle component
Using tf.lookup_transform from each turtlebot AR tag frame to the stick frame, we were able to find the transform to apply to the bot relative to the stick frame
If the translational x component of this Twist vector was near 0, that meant that the bot was already quite close to the stick frame, making it likely that this particular bot was the one to be chosen.
We picked the bot with the smallest transform x value
To eliminate noise, we required the user to point the stick towards the chosen bot for 15 iterations in a row at a rate of 20 Hz (i.e. for 0.75 seconds).
The bot with the lowest difference from the stick frame's x value was assigned the role of "pursuer." If this was bot was the one that was running the script, then we would return "pursuer." Else we would return "evader."

Pursuit-Evasion (turtlebot_control.py)

In order to run the actual game of tag, we would need to use the role assignment of each bot (generated above) and use it to determine the bot's next move. We split up the logic between "pursuer" bots and "evader" bots.

Get the role by using the stick classification script above
Using a mapping from bot IDs/colors to actual strings representing frame names (i.e. "black": "ar_marker_8"), we could determine the active pursuer and the corresponding list of evaders
role=="pursuer"
- The current bot will calculate transforms via tf.lookup_transform with source frame as the current bot and target frame as the evader frame, and do this for all specified evaders.
- Next, the script picks the evader with the lowest translational distance. Although we our demo did not include pursuit of multiple evaders, our code handles this case.
- This transform will then get published to a topic specific to the pursuer bot. Our multi-master shared topic naming scheme was "shared/<color>". More about this in the section on enabling bots to move.
- Loop until the translational distance between the bots became less than our constant value, "game_over_distance."
role=="evader"
- The current bot will compute the transform from the pursuer bot to itself using tf.lookup_transform from the pursuer frame to the current bot frame.
- It will then rotate away from the pursuer, matching the pursuer's pose relative to the current bot's frame.
  - This seems like an intuitive evasion tactic, and, given infinite space and resources, this strategy would prevent the pursuer from catching up to the evader, as long as the evader's rotation away from the pursuer took less time than the time needed for the pursuer to catch the chosen evader.
- Next it will publish the pursuer-to-evader transform to its shared multi-master topic (i.e. "shared/<color>") for translation in pubsub.py

Enabling Bots to Move (pubsub.py)

Once we had a transform assigned for each bot, we needed the bots to physically move. It wasn't enough to have the Twist objects published to the shared multi-master topic -- each bot had to publish the results directly to its corresponding "<color>/mobile_base/commands/velocity" channel in order to actually move. Thus we wrote a separate script called pubsub.py that would handle this specific task of translating commands from one topic to another.

The script received the turtlebot_ID (its color) via command line argument.
(shell) export ROS_MASTER_URI=http://<color>.local:11311
- This step was necessary in order to publish to the bot's specific "commands/velocity" topic
Subscribe to the "shared/<color>" channel for the given bot.
Upon receiving a message in this channel, immediately publish it to the other channel.
- To do this, we created a publisher node whose publish function sent a Twist to the "<color>/mobile_base/commands/velocity" topic.
- pub.publish was called in the subscriber callback, thereby sending the relevant Twist information computed by localhost to the specified bot for execution

Computer Vision Implementation:

1. Color Space Segmentation:

We first convert the image from RBG to HSV, and segment in the HSV space. We initially thought that using color space segmentation alone will be enough to recognize the stick. However, we learned that the color segmentation is quite noisy (see the red circles below).

After realizing that color segmentation alone was not enough we were faced with two options:

Ultimately, we decided to maximize our learning experience by building a more robust system. We did not have classical computer vision experience; however, we were excited to learn and the following is what we came up with.

2. HSV to Grayscale and Gaussian Blur:

We then convert the color segmented image to grayscale for further processing. A Gaussian Blur is also applied to be rid of noise and generate a smoother image segment.

3. Thresholding and Morphological Dilation:

A threshold is applied to further decrease the amount of remaining noise. We thresholded based on the intensity of each pixel, because after gaussian blur noise is generally going to show up with smaller intensity. We then dilate the blocks to fill up small gaps and increase the smoothness of the image which is helpful for contour detection.

4. Contour Detection and Rectangle Fitting:

After removing as much noise as we could, we needed a way to identify which "blob" is the stick. In order to do this, we used OpenCV contour detection algorithms to fit rectangles to the "blobs". Next we built a classifier to determine which box contains the stick.

Our classifier is a function of a boxes aspect ratio and area. We tuned the classifier using a heuristic guess and check process. We initially believed that we should enforce an aspect ratio cutoff because sticks should be rectangular (as opposed to square). However, when the sticks maximum variance is in the cameras Z direction, the 2D color image of the stick looks more square. Upon lots of testing, we found the best approach was to weight the aspect ratio 0 and choose the box with the largest area, however that area must be larger than some threshold. This led to simple algorithm that selects which box is the stick or indicates if the stick is not present.

5. Segmentation

Once we had the correct rectangle, we were able to use it as a mask to select the portion of the original image that contains the stick.

6. Morphological Erosion:

After experimenting with our segmented image from steps 1 through 5, we realized that the RealSense camera is not perfect. We found that towards the edges of the stick the depth sensor would read the wall behind the stick instead of the edges. This caused downstream errors when we tried to predict the direction the stick was pointing. We also realized that all we needed was the depth cloud from the center of the stick. Therefore we used morphological erosion with a 9x9 kernel to erode much of the stick. Adding the erosion step greatly increased the accuracy of our CV system.

7. Calculating Stick Frame Relative To The Camera:

Once we had the segmented and eroded image, we then:

Projected the 3D point cloud on the 2D image. This enabled us to gain a point cloud for the center of the stick.
We calculated the origin of the stick frame relative to the camera frame. We took the mean(x, y, z) of all points in the point cloud.
We calculate the sticks direction. First we took the first principle component of the depth cloud to give us the direction of maximum variance. This should give us the direction the stick is pointing relative to the camera.
We used the Rodrigues formula to calculate the rotation from the cameras x axis (vector ~ [1, 0, 0]) to the sticks first principle component.
We combined the translation and rotation matrices to create a twist. This allows us to define the stick frame relative to the camera frame.

Integration:

1. Representing Turtles In The Stick Frame

We transformed the turtles (represented by ar_marker_4 and ar_marker_3) from the camera frame to the stick frame.

2. Likelihood Estimator

Once we had the turtles represented in the stick frame, we needed to determine which one the stick was pointing at. Since we wanted to build a robust and scalable system, we needed to account for the case when many turtle bots we close together. We felt the best way to handle this was to make the classifier a likelihood estimator that way we can determine whether it is unsure and then narrow down the selection.

Due to the lack of training examples, and with an emphasis on reducing complexity we decided to develop a heuristic approach for defining the probability density function (p(i)). Our method fit based on cosine(theta), here is the process:

Normalize all vectors (<v,w> = cos(theta) when vectors v and w are unit length).
Remove all bots where cos(theta_i) < threshold (we found 0.7 – theta ~= 45 degrees to be a good threshold)
If all cos(theta_i) < 0.7 for all i, then return Null with p = 1.0. Else:
1. Define p(i) = cos(theta_i)/sum(cos(theta_j) for all j)
2. Set p(i) = for all i such that cos(theta_i) < 0.7.
3. Set p(null) = 0.0
Select the bot that has the maximizes p(i).

Page updated

Report abuse