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); }