Reading Image Data
We will talk to the Image Server via a network connection. Once we open the connection, the Image Server will constantly transmit to us the position of the target (if any), along with some other useful information. Let’s create a new class called PiCamera and set it up as follows:
package robot;
import robotCore.Logger;
import robotCore.Network;
public class PiCamera implements Network.NetworkReceiver
{
private Network m_network;
public void Connect(String host, int port)
{
m_network = new Network();
m_network.Connect(this, host, port);
}
@Override
public void ProcessData(String data)
{
Logger.Log("PiCamera", 0, String.format("Data: %s", data));
}
}
We are going to use the Network class to make the network connection, so in our PiCamera class we create an instance of this class. We then create a Connect function to allow us to connect to the Image Server. Note that we will need to supply the IP address as well as the port number. We also pass in our PiCamera instance which supports the NetworkReceiver class. This allows the Network class to call our ProcessData function each time it receives new data from the Image Server.
Now to use this new PiCamera class we will need to add it to our Robot class. First we declare a variable for the camera by adding the following line to the list of variable declarations:
public static final PiCamera m_piCamera = new PiCamera();
Then we initialize this variable in our robotInit() function with the following line:
m_piCamera.Connect(k_cameraIp, k_cameraPort);
Note that we are using constants here for the camera IP and Port so we need to define them at the top of our class:
public static final String k_cameraIp = "172.24.1.1"; public static final int k_cameraPort = 5800;
Your Robot.java file should now look like:
package robot;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
import subsystem.IntakeSubsystem;
import subsystem.ShooterSubsystem;
import subsystem.SpinnerSubsystem;
import wpilib.Scheduler;
public class Robot extends IterativeRobot
{
public static final String k_cameraIp = "172.24.1.1";
public static final int k_cameraPort = 5800;
public static final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
public static final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
public static final SpinnerSubsystem m_spinnerSubsystem = new SpinnerSubsystem();
public static final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
public static final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
public static OI m_OI = null;
public static final PiCamera m_piCamera = new PiCamera();
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotInit()");
m_driveSubsystem.Init();
m_exampleSubsystem.Init();
m_spinnerSubsystem.Init();
m_shooterSubsystem.Init();
m_intakeSubsystem.Init();
m_OI = new OI();
m_piCamera.Connect(k_cameraIp, k_cameraPort);
}
/*
* Called at the start of autonomous mode
*/
@Override
public void autonomousInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "autonomousInit()");
}
/**
* Called periodically during autonomous
*/
@Override
public void autonomousPeriodic()
{
Logger.Log("Robot", -1, "autonomousPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called at the start of teleop mode
*/
@Override
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
}
/**
* Called periodically during operator control
*/
@Override
public void teleopPeriodic()
{
Logger.Log("Robot", -1, "teleopPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called a the start of test mode
*/
@Override
public void testInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "testInit()");
}
/**
* Called periodically during test mode
*/
@Override
public void testPeriodic()
{
Logger.Log("Robot", 0, "testPeriodic()");
Sleep(10);
}
/**
* Main program entry point
*
*/
public static void main(String args[])
{
Robot Robot = new Robot();
Robot.Start(args);
}
}
// END SNIPPET: serial-snippet
Now when we run our program, it will immediately connect to the Image Server and that server will start transmitting the target data which will be received by our ProcessData function which simply logs it to the console. When we run the program, we will then see a constant stream of data printed to the console which looks something like:
14392:PiCamera(0):Data: R 200 148 120 75 160 148 90 939 326 14432:PiCamera(0):Data: R 205 144 121 75 156 144 90 940 326 14462:PiCamera(0):Data: R 212 139 120 74 150 139 90 941 326 14493:PiCamera(0):Data: R 216 137 120 74 148 137 90 942 326 14523:PiCamera(0):Data: R 219 144 119 74 154 144 90 943 326 14563:PiCamera(0):Data: R 221 152 119 73 161 152 90 944 326 14593:PiCamera(0):Data: R 223 158 119 73 168 158 90 945 326 14624:PiCamera(0):Data: R 221 166 119 74 176 166 90 946 326 14664:PiCamera(0):Data: R 219 177 119 73 188 177 90 947 326 14694:PiCamera(0):Data: R 218 173 118 72 184 173 90 948 326 14725:PiCamera(0):Data: R 218 172 118 74 184 172 90 949 326 14755:PiCamera(0):Data: R 219 176 118 72 186 176 90 950 326 14795:PiCamera(0):Data: R 219 172 119 73 183 172 90 951 326 14826:PiCamera(0):Data: R 219 174 119 73 184 174 90 952 326
Here the R at the beginning of the line tells us that this is data which gives us the bounding rectangle, along with some other information. The numbers that follow are, in order:
1: Left edge of the target bounding rectangle
2: Top edge of the target bounding rectangle
3: Width of target bounding rectangle
4: Height of target bounding rectangle
5: Vertical position of the upper left corner of the image
6: Vertical position of the upper right corner of the image
7: The target line position (i.e. the position of the horizontal red line)
8: The current frame number
9: The target center position (i.e. the position of the vertical red line)
If you move the robot around, you will be able to see the data change.
Now instead of printing out the data, we want to save the it for later use. We will need to parse the data out of the string and will use the utility function ParseIntegers from the RobotBase class to do it. Let’s add a class that will encapsulate that data:
public class PiCameraData
{
public int m_x;
public int m_y;
public int m_width;
public int m_height;
public int m_y1;
public int m_y2;
public int m_targetVertPos;
public int m_targetHorzPos;
public int m_frameNo;
PiCameraData(int x, int y, int width, int height, int y1, int y2, int targetVertPos, int targetHorzPos, int frameNo)
{
m_x = x;
m_y = y;
m_width = width;
m_height = height;
m_y1 = y1;
m_y2 = y2;
m_targetVertPos = targetVertPos;
m_targetHorzPos = targetHorzPos;
m_frameNo = frameNo;
}
}
And add an instance of this class that we will used to store the most recently received data from the ImageServer:
private PiCameraData m_data = null;
Finally we will add a function that will parse the nine integers from the data string and store them in this structure:
private void ProcessCameraData(String args)
{
int a[] = Robot.ParseIntegers(args, 9);
if (a != null)
{
PiCameraData data = new PiCameraData(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[8], a[7]);
synchronized(this)
{
m_data = data;
}
}
}
Now this function is pretty straightforward. After we parse the integers (note that ParseIntegers can return null if there were not enough integers in the string), we create a new instance of our PiCameraData class and initialize it with the data we just parsed (be careful about the order). We then store that data set so that we can access it later.
There is one aspect of this function which may not be familiar, however, and that is the call to synchronized(this). This is needed because the Network receiver will be running in another thread. If you don’t know anything about threads then you might want to read up on it a bit. Basically, a thread is a piece of code that is running in parallel (i.e. at the same time) to your regular code. This means that if there is any shared data, it is possible that both pieces of code may be trying to access it at the same time. This can lead to data corruption. To get around this problem we need a way to prevent another thread from accessing shared data while we are working on it. There are a number of mechanisms that can be used to accomplish this, but for Java one of the most straightforward is the synchronized function. This function can take pretty much any Java Object as an argument which acts like a lock. If process A holds the lock, and process B attempts to access it, process B will be suspended until process A releases the lock. The syncronized function grabs the specified object and that lock is held while the code block that follows is executed. Once the closing ‘}’ of the block is reached, the lock is release allowing other threads to access it.
So in this case, we are using the current instance of the PiCamera class as the lock while we modify the m_data variable. Now let’s create a function that can retrieve the camera data. Note that this function will be called from a different thread that that which sets the data. Add the following function to your PiCamera class:
public PiCameraData GetCameraData()
{
synchronized(this)
{
return(m_data);
}
}
Once again, we use the current instance of the PiCamera class as the lock before we access the data we are going to return. This ensures that the data is not being changed while we are trying to return it.
Finally, we need to call our ProcessCameraData function when our ProcessData function is called by the Network class. Change your ProcessData function as follows:
@Override
public void ProcessData(String data)
{
Logger.Log("PiCamera", -1, String.format("Data: %s", data));
switch (data.charAt(0))
{
case 'R':
ProcessCameraData(data.substring(1).trim());
break;
default:
Logger.Log("PiCamera", 3, String.format("Invalid command: %s", data));
break;
}
}
We first check to see that the first character of the received string is an ‘R’ (we might want to add other command later) and if is, we send the remainder of the string to our ProcessCameraData function. If the first character is anything else, we log that as an error. Note also that we changed the logging level of the first Logger call to -1 so we don’t see all of that output all the time.
You PiCamera.java file should now look like:
package robot;
import robotCore.Logger;
import robotCore.Network;
public class PiCamera implements Network.NetworkReceiver
{
private Network m_network = null;
private PiCameraData m_data = null;
public class PiCameraData
{
public int m_x;
public int m_y;
public int m_width;
public int m_height;
public int m_y1;
public int m_y2;
public int m_targetVertPos;
public int m_targetHorzPos;
public int m_frameNo;
PiCameraData(int x, int y, int width, int height, int y1, int y2, int targetVertPos, int targetHorzPos, int frameNo)
{
m_x = x;
m_y = y;
m_width = width;
m_height = height;
m_y1 = y1;
m_y2 = y2;
m_targetVertPos = targetVertPos;
m_targetHorzPos = targetHorzPos;
m_frameNo = frameNo;
}
}
public void Connect(String host, int port)
{
m_network = new Network();
m_network.Connect(this, host, port);
}
private void ProcessCameraData(String args)
{
int a[] = Robot.ParseIntegers(args, 9);
if (a != null)
{
PiCameraData data = new PiCameraData(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[8], a[7]);
synchronized(this)
{
m_data = data;
}
}
}
public PiCameraData GetCameraData()
{
synchronized(this)
{
return(m_data);
}
}
@Override
public void ProcessData(String data)
{
Logger.Log("PiCamera", -1, String.format("Data: %s", data));
switch (data.charAt(0))
{
case 'R':
ProcessCameraData(data.substring(1).trim());
break;
default:
Logger.Log("PiCamera", 3, String.format("Invalid command: %s", data));
break;
}
}
}