IEnumerator SendImage() { yield return(new WaitForEndOfFrame()); int width = 640; int height = 480; Texture2D tex = new Texture2D(width, height, TextureFormat.RGB24, false); tex.ReadPixels(new Rect(0, 0, width, height), 0, 0); tex.Apply(); byte[] data = tex.EncodeToJPG(); var now = DateTime.Now; var timeSpan = now - lastFrame; var timeSinceStart = now - camStart; var timeMessage = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds); var headerMessage = new HeaderMsg(count, timeMessage, "camera"); Debug.Log("data length: " + data.Length); string format = "jpeg"; var compressedImageMsg = new CompressedImageMsg(headerMessage, format, data); Debug.Log(compressedImageMsg); ros.Publish(CompressedImagePublisher.GetMessageTopic(), compressedImageMsg); Destroy(tex); ros.Render(); }
public void ReceiveTF_MultipleMessages_InterpolatesTimes() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); TFStream stream = topic.GetOrCreateFrame(simple_frame_id); int time1_secs = 1000; int time2_secs = 2000; TimeMsg time1 = MakeTimeMsg(time1_secs, 0); Vector3 unityPosition1 = new Vector3(1, 2, 3); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time1, parent_frame_id), simple_frame_id, new TransformMsg(unityPosition1.To <FLU>(), new QuaternionMsg() )) })); TimeMsg time2 = MakeTimeMsg(time2_secs, 0); Vector3 unityPosition2 = new Vector3(2, 3, 4); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time2, parent_frame_id), simple_frame_id, new TransformMsg(unityPosition2.To <FLU>(), new QuaternionMsg() )) })); TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0); Vector3 pointAtTime1 = stream.GetWorldTF(time1).translation; Vector3 pointAtTime1_5 = stream.GetWorldTF(time1_5).translation; Vector3 pointAtTime2 = stream.GetWorldTF(time2).translation; Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2; Assert.AreEqual(pointAtTime1, unityPosition1); Assert.AreEqual(pointAtTime1_5, unityPosition1_5); Assert.AreEqual(pointAtTime2, unityPosition2); }
IEnumerator SendImu() { yield return(new WaitForEndOfFrame()); var now = DateTime.Now; var timeSpan = now - lastFrame; var timeSinceStart = now - camStart; var timeMessage = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds); var headerMessage = new HeaderMsg(count, timeMessage, "imu"); Quaternion vehicleImu; vehicleImu = vehicle.transform.rotation; double xey = vehicleImu.x; double yey = vehicleImu.y; double zey = vehicleImu.z; double wey = vehicleImu.w; var imuData = new QuaternionMsg(xey, yey, zey, wey); double[] arg3 = { 0.00, 0.00, 0.00 }; var vect3 = new Vector3Msg(0.00, 0.00, 0.00); var imumessage = new ImuMessage(headerMessage, imuData, arg3, vect3, arg3, vect3, arg3); ros.Publish(imuPublisher.GetMessageTopic(), imumessage); ros.Render(); }
public void ReceiveTF_FrameWithParent_ReturnsSameTranslation() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; TFSystem system = TFSystem.GetOrCreateInstance(); TFSystem.TFTopicState topic = system.GetOrCreateTFTopic(); TFStream stream = system.GetOrCreateFrame(composite_frame_id); Assert.AreEqual(stream.Name, simple_frame_id); Assert.AreEqual(stream.Parent.Name, parent_frame_id); TimeMsg time = MakeTimeMsg(4567, 890); Vector3 unityPosition = new Vector3(1, 2, 3); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time, parent_frame_id), composite_frame_id, new TransformMsg(unityPosition.To <FLU>(), new QuaternionMsg() )) })); TFFrame frame = stream.GetWorldTF(time.ToLongTime()); Assert.AreEqual(frame.translation.x, unityPosition.x); Assert.AreEqual(frame.translation.y, unityPosition.y); Assert.AreEqual(frame.translation.z, unityPosition.z); Vector3 gameObjectPos = stream.GameObject.transform.position; Assert.AreEqual(gameObjectPos.x, unityPosition.x); Assert.AreEqual(gameObjectPos.y, unityPosition.y); Assert.AreEqual(gameObjectPos.z, unityPosition.z); }
public static DateTime ToDateTime(this TimeMsg message) { DateTime time = new DateTime(message.sec); time = time.AddMilliseconds(message.nanosec / 1E6); return(time); }
HeaderMsg MakeHeaderMsg(TimeMsg time, string frameID) { #if !ROS2 return(new HeaderMsg((uint)0, time, frameID)); #else return(new HeaderMsg(time, frameID)); #endif }
public MapMetaDataMsg(TimeMsg map_load_time, float resolution, uint width, uint height, PoseMsg origin) { _map_load_time = map_load_time; _resolution = resolution; _width = width; _height = height; _origin = origin; }
public MapMetaDataMsg(JSONNode msg) { _map_load_time = new TimeMsg(msg["map_load_time"]); _resolution = float.Parse(msg["resolution"], System.Globalization.CultureInfo.InvariantCulture); _width = uint.Parse(msg["width"], System.Globalization.CultureInfo.InvariantCulture); _height = uint.Parse(msg["height"], System.Globalization.CultureInfo.InvariantCulture); _origin = new PoseMsg(msg["origin"]); }
public HeaderMsg(JSONNode msg) { Debug.Log("HeaderMsg with " + msg.ToString()); _seq = int.Parse(msg["seq"]); _stamp = new TimeMsg(msg ["stamp"]); _frame_id = msg["frame_id"]; Debug.Log("HeaderMsg done "); Debug.Log(" and it looks like " + this.ToString()); }
public LookupTransformGoal() { this.target_frame = ""; this.source_frame = ""; this.source_time = new TimeMsg(); this.timeout = new DurationMsg(); this.target_time = new TimeMsg(); this.fixed_frame = ""; this.advanced = false; }
public LookupTransformGoal(string target_frame, string source_frame, TimeMsg source_time, DurationMsg timeout, TimeMsg target_time, string fixed_frame, bool advanced) { this.target_frame = target_frame; this.source_frame = source_frame; this.source_time = source_time; this.timeout = timeout; this.target_time = target_time; this.fixed_frame = fixed_frame; this.advanced = advanced; }
private LookupTransformGoal(MessageDeserializer deserializer) { deserializer.Read(out this.target_frame); deserializer.Read(out this.source_frame); this.source_time = TimeMsg.Deserialize(deserializer); this.timeout = DurationMsg.Deserialize(deserializer); this.target_time = TimeMsg.Deserialize(deserializer); deserializer.Read(out this.fixed_frame); deserializer.Read(out this.advanced); }
private static void GetArmsPosition() { TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); TFSubscriptionActionGoalMsg msg = new TFSubscriptionActionGoalMsg(new HeaderMsg(1, currentTime, ""), new GoalIDMsg(currentTime, ROSActionHelper.GenerateUniqueGoalID("robot_radius", ROSTimeHelper.ToSec(currentTime))), new TFSubscriptionGoalMsg(new List <string>() { leftArm.BaseLink, rightArm.BaseLink }, "marker", 0f, 0f, 1f)); ROSCommunicationManager.Instance.ros.Publish(TF2WebRepublisherGoalPublisher.GetMessageTopic(), msg); }
// Update is called once per frame in Unity void Update() { GameObject OVRtouchcontroller = GameObject.Find("OVR_touch_controller"); // OVRtouchcontroller. // fill in the body velocity request message: // carregar un msg tipus body velocity request amb els continugts de Global Variables double vx = Global_Variables.vel_linear_x; double vy = Global_Variables.vel_linear_y; double vz = Global_Variables.vel_linear_z; double yaw = (double)Global_Variables.yaw; // HeaderMsg header, GoalDescriptorMsg goal, TwistMsg twist, auv_msgs.Bool6AxisMsg disable_axis // twist for the boby velocity request: linear + angular speeds TwistMsg twist = new TwistMsg(new Vector3Msg(vx, vy, vz), new Vector3Msg(0.0, 0.0, Global_Variables.yaw)); Bool6AxisMsg dissable_axis = new Bool6AxisMsg(false, false, false, true, true, false); // dissable axis field for the body velocity request string requester = "/teleoperation"; GoalDescriptorMsg goal = new GoalDescriptorMsg(requester, 0, 60); // id= 0, priority=60 // header: int seq, TimeMsg stamp, string frame_id System.DateTime epochStart = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc); double cur_time = ((System.DateTime.UtcNow - epochStart).TotalSeconds); // in seconds //print("cur_time" + cur_time); double decimalPart = cur_time % 1; // get the decimal part int secs = (int)(cur_time - decimalPart); //take the integer part of the timestamp expressed in nanoseconds //print("seconds" + secs); int nsecs = (int)(decimalPart * 1000000000); // take the nanoseconds //print("nsencs"+ nsecs); TimeMsg stamp = new TimeMsg(secs, nsecs); HeaderMsg header = new HeaderMsg(0, stamp, "/map"); //frame id=/map. BodyVelocityRequestMsg msg = new BodyVelocityRequestMsg(header, goal, twist, dissable_axis); // create a clase of message to be published //print("Body request" + twist); if (Global_Variables.activate_control) //send velocity commands only if the Unity Control is active // print("publish body vel. request:" + msg.ToString()); // print requested vel.. { ros.Publish(RobotBodyVelocityRequest.GetMessageTopic(), msg); // descomentar !! -- 18/09/2017 } //ros.Publish(RobotBodyVelocityRequest.GetMessageTopic(), twist); // descomentar !! -- 18/09/2017 if (Global_Variables.stop_motors) // dissable motors if required with the "A" button { ros.CallService("/control/disable_thrusters"); Global_Variables.stop_motors = false; // avoids to send the service all the time } if (Global_Variables.start_motors) // enable motors if required with button "B" { ros.CallService("/control/enable_thrusters"); Global_Variables.start_motors = false; // avoids to send the service all the time } ros.Render(); }
public InterfaceStateMsg(string interface_id, SystemState system_state, TimeMsg timestamp, UInt16 program_id, UInt16 block_id, ProgramItemMsg program_current_item, List <KeyValueMsg> flags, bool edit_enabled, ErrorSeverity error_severity, ErrorCode error_code) { _interface_id = interface_id; _system_state = system_state; _timestamp = timestamp; _program_id = program_id; _block_id = block_id; _program_current_item = program_current_item; _flags = flags; _edit_enabled = edit_enabled; _error_severity = error_severity; _error_code = error_code; }
public InterfaceStateMsg(JSONNode msg) { _interface_id = msg["interface_id"]; _system_state = (SystemState)Int16.Parse(msg["system_state"]); _timestamp = new TimeMsg(msg["timestamp"]); _program_id = UInt16.Parse(msg["program_id"]); _block_id = UInt16.Parse(msg["block_id"]); _program_current_item = new ProgramItemMsg(msg["program_current_item"]); foreach (JSONNode item in msg["flags"].AsArray) { _flags.Add(new KeyValueMsg(item)); } _edit_enabled = bool.Parse(msg["edit_enabled"]); _error_severity = (ErrorSeverity)Int32.Parse(msg["error_severity"]); _error_code = (ErrorCode)Int32.Parse(msg["error_code"]); }
// Update is called once per frame public void UpdatePlacePose(Vector3 placePosition, Quaternion placeOrientation) { if (ROSUnityCoordSystemTransformer.AlmostEqual(currentPlacePosition, placePosition, 0.001f) && ROSUnityCoordSystemTransformer.AlmostEqual(currentPlaceOrientation, placeOrientation, 0.001f)) { return; } currentPlacePosition = placePosition; currentPlaceOrientation = placeOrientation; if (learning) { TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>(), new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(currentPlacePosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(currentPlaceOrientation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); } }
private TopicStatisticsMsg(MessageDeserializer deserializer) { deserializer.Read(out this.topic); deserializer.Read(out this.node_pub); deserializer.Read(out this.node_sub); this.window_start = TimeMsg.Deserialize(deserializer); this.window_stop = TimeMsg.Deserialize(deserializer); deserializer.Read(out this.delivered_msgs); deserializer.Read(out this.dropped_msgs); deserializer.Read(out this.traffic); this.period_mean = DurationMsg.Deserialize(deserializer); this.period_stddev = DurationMsg.Deserialize(deserializer); this.period_max = DurationMsg.Deserialize(deserializer); this.stamp_age_mean = DurationMsg.Deserialize(deserializer); this.stamp_age_stddev = DurationMsg.Deserialize(deserializer); this.stamp_age_max = DurationMsg.Deserialize(deserializer); }
public TopicStatisticsMsg(string topic, string node_pub, string node_sub, TimeMsg window_start, TimeMsg window_stop, int delivered_msgs, int dropped_msgs, int traffic, DurationMsg period_mean, DurationMsg period_stddev, DurationMsg period_max, DurationMsg stamp_age_mean, DurationMsg stamp_age_stddev, DurationMsg stamp_age_max) { this.topic = topic; this.node_pub = node_pub; this.node_sub = node_sub; this.window_start = window_start; this.window_stop = window_stop; this.delivered_msgs = delivered_msgs; this.dropped_msgs = dropped_msgs; this.traffic = traffic; this.period_mean = period_mean; this.period_stddev = period_stddev; this.period_max = period_max; this.stamp_age_mean = stamp_age_mean; this.stamp_age_stddev = stamp_age_stddev; this.stamp_age_max = stamp_age_max; }
public TopicStatisticsMsg() { this.topic = ""; this.node_pub = ""; this.node_sub = ""; this.window_start = new TimeMsg(); this.window_stop = new TimeMsg(); this.delivered_msgs = 0; this.dropped_msgs = 0; this.traffic = 0; this.period_mean = new DurationMsg(); this.period_stddev = new DurationMsg(); this.period_max = new DurationMsg(); this.stamp_age_mean = new DurationMsg(); this.stamp_age_stddev = new DurationMsg(); this.stamp_age_max = new DurationMsg(); }
public TFFrame GetTransform(string frame_id, TimeMsg time, string tfTopic = "/tf") { return(GetTransform(frame_id, time.ToLongTime(), tfTopic)); }
private ClockMsg(MessageDeserializer deserializer) { this.clock = TimeMsg.Deserialize(deserializer); }
public ClockMsg(TimeMsg clock) { this.clock = clock; }
public override void Deserialize(JSONNode msg) { _seq = int.Parse(msg["seq"]); _stamp = new TimeMsg(msg ["stamp"]); _frame_id = msg["frame_id"]; }
public HeaderMsg(JSONNode msg) { _seq = int.Parse(msg["seq"]); _stamp = new TimeMsg(msg ["stamp"]); _frame_id = msg["frame_id"]; }
public IEnumerator SaveToROS() { ROSActionHelper.OnLearningActionResult += OnLearningActionResult; //save instruction to ROS Debug.Log("PLACE TO POSE SAVED"); TimeMsg currentTime = ROSTimeHelper.GetCurrentTime(); InterfaceStateMsg msg = new InterfaceStateMsg( "HOLOLENS", InterfaceStateMsg.SystemState.STATE_LEARNING, currentTime, interfaceStateMsg.GetProgramID(), interfaceStateMsg.GetBlockID(), new ProgramItemMsg( programItemMsg.GetID(), programItemMsg.GetOnSuccess(), programItemMsg.GetOnFailure(), programItemMsg.GetIType(), programItemMsg.GetName(), new List <string>(), new List <PoseStampedMsg>() { new PoseStampedMsg(new HeaderMsg(0, currentTime, "marker"), new PoseMsg(new PointMsg(ROSUnityCoordSystemTransformer.ConvertVector(PlacePosition)), new QuaternionMsg(ROSUnityCoordSystemTransformer.ConvertQuaternion(PlaceOrientation)))) }, new List <PolygonStampedMsg>(), programItemMsg.GetRefID(), programItemMsg.GetFlags(), programItemMsg.GetDoNotClear(), programItemMsg.GetLabels()), interfaceStateMsg.GetFlags(), true, InterfaceStateMsg.ErrorSeverity.NONE, InterfaceStateMsg.ErrorCode.ERROR_UNKNOWN); ROSCommunicationManager.Instance.ros.Publish(InterfaceStatePublisher.GetMessageTopic(), msg); yield return(new WaitForSecondsRealtime(0.1f)); currentTime = ROSTimeHelper.GetCurrentTime(); currentRequestID = ROSActionHelper.GenerateUniqueGoalID((int)learning_request_goal.DONE, ROSTimeHelper.ToSec(currentTime)); LearningRequestActionGoalMsg requestMsg = new LearningRequestActionGoalMsg(new HeaderMsg(0, currentTime, ""), new GoalIDMsg(currentTime, currentRequestID), new LearningRequestGoalMsg(learning_request_goal.DONE)); ROSCommunicationManager.Instance.ros.Publish(LearningRequestActionGoalPublisher.GetMessageTopic(), requestMsg); waiting_for_action_response = true; //Wait for action server response yield return(new WaitWhile(() => waiting_for_action_response == true)); Debug.Log("SERVER RESPONDED!"); yield return(new WaitForSecondsRealtime(0.1f)); ROSActionHelper.OnLearningActionResult -= OnLearningActionResult; //if(InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride) { // InteractiveProgrammingManager.Instance.followedLearningPlacePoseOverride = false; // InteractiveProgrammingManager.Instance.CurrentState = InteractiveProgrammingManager.ProgrammingManagerState.place_to_pose_vis; // Visualize(); //} yield return(null); }
public static void GUI(this TimeMsg message) { GUILayout.Label(message.ToTimestampString()); }
public static string ToTimestampString(this TimeMsg message) { // G: format using short date and long time return(message.ToDateTime().ToString("G") + $"(+{message.nanosec})"); }
public static long ToLongTime(this TimeMsg message) { return((long)message.sec << 32 | message.nanosec); }
public HeaderMsg(int seq, TimeMsg stamp, string frame_id) { _seq = seq; _stamp = stamp; _frame_id = frame_id; }