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