private void subscriptionHandler(Message message)
{
//NavigationOdometry standardString = (NavigationOdometry)message;
GeometryPoseWithCovarianceStamped standardString = (GeometryPoseWithCovarianceStamped)message;
//this.Dispatcher.Invoke(new Action(delegate
//{
// mytextbox.Text = "X:" + standardString.pose.pose.position.x.ToString() + "\r\n";
// mytextbox.Text += "Y:" + standardString.pose.pose.position.y.ToString() + "\r\n";
// mytextbox.Text += "Z:" + standardString.pose.pose.position.z.ToString() + "\r\n";
//}));
}
private void AmclPoseHandler(Communication.Message message)
{
GeometryPoseWithCovarianceStamped standardString = (GeometryPoseWithCovarianceStamped)message;
double posX = (double)standardString.pose.pose.position.x;
double posY = (double)standardString.pose.pose.position.y;
double posThetaZ = (double)standardString.pose.pose.orientation.z;
double posThetaW = (double)standardString.pose.pose.orientation.w;
double posTheta = (double)2 * Math.Atan2(posThetaZ, posThetaW);
properties.pose.Position = new Point(posX, posY);
properties.pose.AngleW = posTheta;
}
private void robotInfoHandler(Communication.Message message)
{
GeometryPoseWithCovarianceStamped standardString = (GeometryPoseWithCovarianceStamped)message;
updateRobotInfo(standardString.pose);
}
