private void DoRecvEgm(UdpClient egm_socket, ref IPEndPoint ep)
{
var recv = egm_socket.Receive(ref ep);
egm_robot = new EgmRobot();
egm_robot.MergeFrom(new CodedInputStream(recv));
lock (this)
{
if (egm_robot.HasFeedBack && egm_robot.FeedBack.HasJoints && egm_robot.FeedBack.HasCartesian && egm_robot.HasMotorState && egm_robot.HasRapidExecState)
{
last_recv = now_func();
var joint_msg = egm_robot.FeedBack.Joints.Joints;
if (actual_joint_position == null || actual_joint_position.Length != joint_msg.Count)
{
actual_joint_position = new double[joint_msg.Count];
}
for (int i = 0; i < joint_msg.Count; i++)
{
actual_joint_position[i] = joint_msg[i] * (Math.PI / 180.0);
}
var cart = egm_robot.FeedBack.Cartesian;
tcp_pose.position.x = cart.Pos.X / 1000.0;
tcp_pose.position.y = cart.Pos.Y / 1000.0;
tcp_pose.position.z = cart.Pos.Z / 1000.0;
tcp_pose.orientation.w = cart.Orient.U0;
tcp_pose.orientation.x = cart.Orient.U1;
tcp_pose.orientation.y = cart.Orient.U2;
tcp_pose.orientation.z = cart.Orient.U3;
enabled = egm_robot.HasMotorState;
ready = egm_robot.RapidExecState.State == EgmRapidCtrlExecState.Types.RapidCtrlExecStateType.RapidRunning;
}
}
}
