public void updateRobotInfo(GeometryPoseWithCovariance data)
{
double posX = (double)data.pose.position.x;
double posY = (double)data.pose.position.y;
double posThetaZ = (double)data.pose.orientation.z;
double posThetaW = (double)data.pose.orientation.w;
double posTheta = (double)2 * Math.Atan2(posThetaZ, posThetaW);
robotInfo.loc = new Point3D(posX, posY, 0);
robotInfo.mapLocation = new Point3D(GlobalVariables.ConvertMetertoUnitLength(posX), GlobalVariables.ConvertMetertoUnitLength(posY), 0);
robotInfo.heading = posTheta * 180 / Math.PI;
updateRobotLocation(robotInfo.mapLocation, robotInfo.heading);
}
public bool checkRobotVersion(String msg)
{
RegistrationAgent.mainWindowPointer.LogConsole("Robot version.", "logRobot");
long version = (Int64)Version.ROBOT_V1_CODE;
if (version == (Int64)Version.ROBOT_V1_CODE)
{
setRobot3Dmodel(GlobalVariables.getPathRobot3DModel());
updateRobotLocation(new Point3D(GlobalVariables.ConvertMetertoUnitLength(0), GlobalVariables.ConvertMetertoUnitLength(0), 0), 0);
}
else if (version == (Int64)Version.ROBOT_V2_CODE)
{
setRobot3Dmodel(GlobalVariables.getPathRobot3DModel());
updateRobotLocation(new Point3D(GlobalVariables.ConvertMetertoUnitLength(0), GlobalVariables.ConvertMetertoUnitLength(10), 0), 0);
}
else
{
return(false);
}
return(true);
}
