/// <summary>
/// Request KinectTracker to find specified object
/// </summary>
/// <param name="skeletonId">Id of the skeleton calibrated</param>
/// <param name="timeOut">Amout of time to wait for a response in milliseconds</param>
/// <returns>true if calibration was successfull, false otherwise</returns>
public virtual bool Calibrate(out int skeletonId, int timeOut)
{
// Stores the command to be sent to object finder
Command cmdCalibrate;
// Stores the response from object finder and the candidate while moving
Response rspCalibrate = null;
skeletonId = -1;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
cmdCalibrate = new Command(sgnCalibrate.CommandName, "", CmdMan.AutoId++);
// 2. Send the object finderGoto command
CmdMan.Console("\tCalibrating skeleton [" + skeletonId + "]");
if (!CmdMan.SendAndWait(cmdCalibrate, timeOut, out rspCalibrate))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse human finder response
if (!rspCalibrate.Success)
{
// 3.2. Failed response
CmdMan.Console("\tCalibration failed");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnCalibrate.Analyze(rspCalibrate);
if (saResult.Success && (saResult.ParameterCount == 1))
{
saResult.Update <int>(0, ref skeletonId);
}
CmdMan.Console("\tCalibrate complete");
return(true);
}
/// <summary>
/// Request head to move the specified orientation
/// </summary>
/// <param name="pan">The pan of the face</param>
/// <param name="tilt">The tilt of the face</param>
/// <param name="timeOut">Amout of time to wait for a head response in milliseconds</param>
/// <returns>true if head moved to the specified location. false otherwise</returns>
public bool LookAt(ref double pan, ref double tilt, int timeOut)
{
// Stores the command to be sent to head
Command cmdLookAt;
// Stores the response from head and the candidate while lookingfor
Response rspLookAt = null;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
string parameters = pan.ToString("0.00") + " " + tilt.ToString("0.00");
cmdLookAt = new Command(sgnHeadLookAt.CommandName, parameters, CmdMan.AutoId++);
// 2. Send the lookat command
CmdMan.Console("\tMoving head to position " + parameters);
if (!CmdMan.SendAndWait(cmdLookAt, timeOut, out rspLookAt))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse head response
if (!rspLookAt.Success)
{
// 3.2. Failed response
CmdMan.Console("\tHead did not move");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnHeadLookAt.Analyze(rspLookAt);
if (saResult.Success && (saResult.ParameterCount == 2))
{
saResult.Update <double>(0, ref pan);
saResult.Update <double>(1, ref tilt);
}
CmdMan.Console("\tMoving head to position complete");
return(true);
}
/// <summary>
/// Request robot base to move to the specified orientation
/// </summary>
/// <param name="distance">The distance the robot must move</param>
/// <param name="angle">The angle the robot must turn before move</param>
/// <param name="timeOut">Amout of time to wait for an arm response in milliseconds</param>
/// <returns>true if robot moved to the specified location. false otherwise</returns>
public bool MoveBase(ref double distance, ref double angle, int timeOut)
{
// Stores the command to be sent to robot base
Command cmdMove;
// Stores the response from robot base and the candidate while lookingfor
Response rspMove = null;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
string parameters = distance.ToString("0.00") + " " + angle.ToString("0.00") + " 0.00";
cmdMove = new Command(sgnMove.CommandName, parameters, CmdMan.AutoId++);
// 2. Send the lookat command
CmdMan.Console("\tMoving base with " + cmdMove.StringToSend);
if (!CmdMan.SendAndWait(cmdMove, timeOut, out rspMove))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.2. Parse base response
if (!rspMove.Success)
{
// 3.2. Failed response
CmdMan.Console("\tRobot's base did not move");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnMove.Analyze(rspMove);
if (saResult.Success)
{
saResult.Update <double>(0, ref distance);
saResult.Update <double>(1, ref angle);
}
CmdMan.Console("\tMove robot complete on [" + rspMove.Parameters + "]");
return(true);
}
/// <summary>
/// Request PersonFinder to find specified person
/// </summary>
/// <param name="humanName">Name of the person to find</param>
/// <param name="hFOVorX">When this method returns contains one of the following values:
/// a) The horizontal fov if the response contains 3 parameters (name and FOVs)
/// b) The x coordinate of the centroid of the face based on the center of the camera lens if the response contains 4 parameters (name and coords)
/// c) Double.NaN if no response was received or the response does not contain position data</param>
/// <param name="vFOVorY">When this method returns contains one of the following values:
/// a) The vertical fov if the response contains 3 parameters (name and FOVs)
/// b) The y coordinate of the centroid of the face based on the center of the camera lens if the response contains 4 parameters (name and coords)
/// c) Double.NaN if no response was received or the response does not contain position data</param>
/// <param name="z">When this method returns contains one of the following values:
/// a) The z coordinate of the centroid of the face based on the center of the camera lens if the response contains 4 parameters (name and coords)
/// b) Double.NaN if no response was received or the response does not contain coordinate position data</param>
/// <param name="timeOut">Amout of time to wait for a response in milliseconds</param>
/// <returns>true if specified person was found. false otherwise</returns>
public virtual bool FindHuman(ref string humanName, out double hFOVorX, out double vFOVorY, out double z, int timeOut)
{
// Stores the command to be sent to person finder
Command cmdFindHuman;
// Stores the response from person finder and the candidate while moving
Response rspFindHuman = null;
hFOVorX = Double.NaN;
vFOVorY = Double.NaN;
z = Double.NaN;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
cmdFindHuman = new Command(sgnFindHuman.CommandName, humanName, CmdMan.AutoId++);
// 2. Send the person finderGoto command
CmdMan.Console("\tFinding human [" + humanName + "]");
if (!CmdMan.SendAndWait(cmdFindHuman, timeOut, out rspFindHuman))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse human finder response
if (!rspFindHuman.Success)
{
// 3.2. Failed response
CmdMan.Console("\tHuman not found");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult result = sgnFindHuman.Analyze(rspFindHuman);
if (result.Success)
{
result.Update(0, ref humanName);
if (result.ParameterCount >= 3)
{
result.Update(1, ref hFOVorX);
result.Update(2, ref vFOVorY);
}
if (result.ParameterCount >= 4)
{
result.Update(3, ref z);
}
}
CmdMan.Console("\tFind human complete");
return(result.Success);
}
/// <summary>
/// Request the manipulator aperture pecentage and tilt angle
/// </summary>
/// <param name="aperturePercentage">Percentage aperture of the gripper of the manipulator.</param>
/// <param name="tilt">Tilt angle of the manipulator.</param>
/// <param name="timeOut">Amout of time to wait for an man response in milliseconds</param>
/// <returns>true if data fetch was successfully. false otherwise</returns>
public virtual bool Status(out int aperturePercentage, out double tilt, int timeOut)
{
// Stores the command to be sent to man
Command cmdStatus;
// Stores the response from man and the candidate while moving
Response rspStatus = null;
bool result;
aperturePercentage = 0;
tilt = 0;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
string parameters = "";
cmdStatus = new Command(sgnStatus.CommandName, parameters, CmdMan.AutoId++);
// 2. Send the manAbsPos command
CmdMan.Console("\tReading man orientation " + parameters);
if (!CmdMan.SendAndWait(cmdStatus, timeOut, out rspStatus))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse man response
if (!rspStatus.Success)
{
// 3.2. Failed response
CmdMan.Console("\tManipulator did not respond");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnStatus.Analyze(rspStatus);
result = saResult.Success &&
(saResult.ParameterCount == 2) &&
(saResult.Update <int>(0, ref aperturePercentage) &
saResult.Update <double>(1, ref tilt));
if (!result)
{
CmdMan.Console("\tInvalid response");
}
else
{
CmdMan.Console("\tGet man position complete");
}
return(result);
}
/// <summary>
/// Request manipulator to move to the specified tilt angle in radians
/// </summary>
/// <param name="tilt">The specified tilt angle in radians</param>
/// <param name="timeOut">Amout of time to wait for an man response in milliseconds</param>
/// <returns>true if manipulator moved to specified tilt. false otherwise</returns>
public virtual bool Tilt(ref double tilt, int timeOut)
{
// Stores the command to be sent to man
Command cmdTilt;
// Stores the response from man and the candidate while moving
Response rspTilt = null;
bool result;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
cmdTilt = new Command(sgnTilt.CommandName, tilt.ToString("0.00"), CmdMan.AutoId++);
// 2. Send the manGoto command
CmdMan.Console("\tSetting manipulator tilt [" + tilt + "]");
if (!CmdMan.SendAndWait(cmdTilt, timeOut, out rspTilt))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse manipulator response
if (!rspTilt.Success)
{
// 3.2. Failed response
CmdMan.Console("\tManipulator did not move");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnTilt.Analyze(rspTilt);
result = saResult.Success &&
(saResult.ParameterCount == 1) &&
(saResult.Update <double>(0, ref tilt));
if (!result)
{
CmdMan.Console("\tInvalid response");
}
else
{
CmdMan.Console("\tSet manipulator tilt complete");
}
return(result);
}
/// <summary>
/// Request robot base to move to the specified region
/// </summary>
/// <param name="x">The x coordinate position of the robot in the map</param>
/// <param name="y">The y coordinate position of the robot in the map</param>
/// <param name="angle">The angle the robot must turn on arrival</param>
/// <param name="timeOut">Amout of time to wait for an arm response in milliseconds</param>
/// <returns>true if robot moved to the specified location. false otherwise</returns>
public bool Position(out double x, out double y, out double angle, int timeOut)
{
// Stores the command to be sent to robot base
Command cmdPosition;
// Stores the response from robot base and the candidate while lookingfor
Response rspPosition = null;
x = 0;
y = 0;
angle = 0;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
cmdPosition = new Command(sgnPosition.CommandName, "", CmdMan.AutoId++);
// 2. Send the lookat command
CmdMan.Console("\tGeting robot position " + cmdPosition.StringToSend);
if (!CmdMan.SendAndWait(cmdPosition, timeOut, out rspPosition))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.2. Parse base response
if (!rspPosition.Success)
{
// 3.2. Failed response
CmdMan.Console("\tCan not get robot position");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnPosition.Analyze(rspPosition);
if (saResult.Success && saResult.ParameterCount >= 3)
{
saResult.Update <double>(0, ref x);
saResult.Update <double>(1, ref y);
saResult.Update <double>(2, ref angle);
}
CmdMan.Console("\tRobot Position [" + rspPosition.Parameters + "]");
return(true);
}
/// <summary>
/// Request manipulator to open the grip
/// </summary>
/// <param name="percentage">Percentage aperture of the grip</param>
/// <param name="timeOut">Amout of time to wait for an man response in milliseconds</param>
/// <returns>true if manipulator opend the grip. false otherwise</returns>
public virtual bool OpenGrip(ref int percentage, int timeOut)
{
// Stores the command to be sent to man
Command cmdOpenGrip;
// Stores the response from man and the candidate while moving
Response rspOpenGrip = null;
bool result;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
if ((percentage >= 0) || (percentage <= 100))
{
cmdOpenGrip = new Command(sgnOpenGrip.CommandName, percentage.ToString(), CmdMan.AutoId++);
}
else
{
cmdOpenGrip = new Command(sgnOpenGrip.CommandName, "", CmdMan.AutoId++);
percentage = 50;
}
// 2. Send the manOpenGrip command
CmdMan.Console("\tClosing manipulator grip to [" + percentage.ToString() + "%]");
if (!CmdMan.SendAndWait(cmdOpenGrip, timeOut, out rspOpenGrip))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse manipulator response
if (!rspOpenGrip.Success)
{
// 3.2. Failed response
CmdMan.Console("\tManipulator did not move");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult saResult = sgnOpenGrip.Analyze(rspOpenGrip);
result = saResult.Success &&
(saResult.ParameterCount == 1) &&
(saResult.Update <int>(0, ref percentage));
if (!result)
{
CmdMan.Console("\tInvalid response");
}
else
{
CmdMan.Console("\tOpen manipulator grip complete");
}
return(result);
}
/// <summary>
/// Request ObjectFinder to find specified object with the camera located on Bottom
/// </summary>
/// <param name="objectName">Name of the object to find</param>
/// <param name="x">When this method returns contains one of this values:
/// a) The x-coordinate of the object position if the response contains three position values
/// b) The location of the object respect to the HFOV of the camera (angle measured in radians) if the response contains two position values.
/// c) Double.NaN if the response does not contains position values
/// </param>
/// <param name="y">When this method returns contains one of this values:
/// a) The y-coordinate of the object position if the response contains three position values
/// b) The location of the object respect to the VFOV of the camera (angle measured in radians) if the response contains two position values.
/// c) Double.NaN if the response does not contains position values
/// </param>
/// <param name="z">When this method returns contains one of this values:
/// a) The z-coordinate of the object position if the response contains three position values
/// b) Double.NaN
/// </param>
/// <param name="timeOut">Amout of time to wait for a response in milliseconds</param>
/// <returns>true if specified object was found. false otherwise</returns>
public virtual bool FindObjectBottom(ref string objectName, out double x, out double y, out double z, int timeOut)
{
// Stores the command to be sent to object finder
Command cmdFindObjectBottom;
// Stores the response from object finder and the candidate while moving
Response rspFindObjectBottom = null;
x = Double.NaN;
y = Double.NaN;
z = Double.NaN;
// 1. Prepare the command
if (!GetResource())
{
return(false);
}
cmdFindObjectBottom = new Command(sgnFindObjectBottom.CommandName, objectName, CmdMan.AutoId++);
// 2. Send the object finderGoto command
CmdMan.Console("\tFinding object [" + objectName + "]");
if (!CmdMan.SendAndWait(cmdFindObjectBottom, timeOut, out rspFindObjectBottom))
{
// 2.1. Cant send socket. Operation canceled.
CmdMan.Console("\tCan't send command. Operation canceled");
FreeResource();
return(false);
}
// 3. End of move
FreeResource();
// 3.1. Parse head response
if (!rspFindObjectBottom.Success)
{
// 3.2. Failed response
CmdMan.Console("\tHuman not found");
return(false);
}
// 4.0 Recover values from response
SignatureAnalysisResult result = sgnFindObjectBottom.Analyze(rspFindObjectBottom);
if (result.Success)
{
result.Update <string>(0, ref objectName);
if (result.ParameterCount == 4)
{
result.Update <double>(1, ref x);
result.Update <double>(2, ref y);
result.Update <double>(3, ref z);
}
else if (result.ParameterCount == 3)
{
result.Update <double>(1, ref x);
result.Update <double>(2, ref y);
z = Double.NaN;
}
}
CmdMan.Console("\tFind object complete");
return(true);
}
