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