public NavigationOdometry()
{
header = new StandardHeader();
child_frame_id = "";
pose = new GeometryPoseWithCovariance();
twist = new GeometryTwistWithCovariance();
}
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 GeometryPoseWithCovarianceStamped()
{
header = new StandardHeader();
pose = new GeometryPoseWithCovariance();
}
