// it is called every frame update
void Update()
{
// render the ros communication
ros.Render();
// get the player object position
double playerPosX = player.transform.position.x;
double playerPosY = player.transform.position.y;
double playerPosZ = player.transform.position.z;
// get the player position vector
Vector3Msg playerPositionVector = new Vector3Msg(playerPosX, playerPosY, playerPosZ);
// get the player rotation vector
Vector3Msg playerRotationVector = new Vector3Msg(0, 0, 0);
// get the twist message for the player pose
TwistMsg playerPoseMsg = new TwistMsg(playerPositionVector, playerRotationVector);
// publish the message
ros.Publish(BallPosePublisher.GetMessageTopic(), playerPoseMsg);
// get player speed
float playerSpeed = (float)BallSpeedSubscriber.playerSpeed;
// update player speed
playerController.speed = (playerSpeed == 0)? 1 : playerSpeed;
}
void Update()
{
ros.Render();
if (!_running)
{
return;
}
timer -= Time.deltaTime;
if (timer <= 0)
{
timer = pollRate;
PointMsg point = new PointMsg(1, 2, 3);
QuaternionMsg quat = new QuaternionMsg(1, 2, 3, 4);
PoseMsg pose = new PoseMsg(point, quat);
PoseWithCovarianceMsg posec = new PoseWithCovarianceMsg(pose);
Vector3Msg vec3 = new Vector3Msg(1, 2, 3);
TwistMsg twist = new TwistMsg(vec3, vec3);
TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
HeaderMsg header = new HeaderMsg(1, new TimeMsg(1, 1), "0");
PoseStampedMsg ps = new PoseStampedMsg(header, pose);
PathMsg msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });
BoolMsg boolmsg = new BoolMsg(true);
StringMsg str = new StringMsg("This is a test");
RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 1, 2, 3, true);
CameraInfoMsg caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);
_genericPub.PublishData(caminfo);
}
}
public static Vector3 From(this Vector3Msg self, CoordinateSpaceSelection selection)
{
switch (selection)
{
case CoordinateSpaceSelection.RUF:
return(self.From <RUF>());
case CoordinateSpaceSelection.FLU:
return(self.From <FLU>());
case CoordinateSpaceSelection.FRD:
return(self.From <FRD>());
case CoordinateSpaceSelection.ENU:
return(self.From <ENU>());
case CoordinateSpaceSelection.NED:
return(self.From <NED>());
case CoordinateSpaceSelection.ENULocal:
return(self.From <ENULocal>());
case CoordinateSpaceSelection.NEDLocal:
return(self.From <NEDLocal>());
default:
Debug.LogError("Invalid coordinate space " + selection);
return(self.From <RUF>());
}
}
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();
}
/// <summary>
/// Publish velocity to turtlesim through the ROSBridge
/// </summary>
/// <param name="linearInput"></param>
/// <param name="angularInput"></param>
public void SendVelocity(float linearInput, float angularInput)
{
Vector3Msg linearVelocity = new Vector3Msg(linearInput, 0f, 0f);
Vector3Msg angularVelocity = new Vector3Msg(0f, 0f, -angularInput);
TwistMsg msg = new TwistMsg(linearVelocity, angularVelocity);
ROSBridge.Instance.Publish(TurtleVelocityPublisher.GetMessageTopic(), msg);
}
public SemanticObjectMsg(string id, ObjectHypothesisMsg[] scores, PoseMsg pose, int ndetections, String roomId, String roomType, Vector3Msg size)
{
_id = id;
_scores = scores;
_pose = pose;
_detections = ndetections;
_roomId = roomId;
_roomType = roomType;
_size = size;
}
public ImuMessage(HeaderMsg header, QuaternionMsg quaternionIMU, double[] orientation_c, Vector3Msg angularVelocity, double[] angular_velocity_c, Vector3Msg linearAcceleration, double[] linear_acceleration_c)
{
_header = header;
_quaternionIMU = quaternionIMU;
_orientation_c = orientation_c;
_angularVelocity = angularVelocity;
_angular_velocity_c = angular_velocity_c;
_linearAcceleration = linearAcceleration;
_linear_acceleration_c = linear_acceleration_c;
}
public SemanticObjectMsg(String id, String idRoom, String type, float score, PointCloudMsg pointCloud, PoseMsg position, Vector3Msg scale)
{
_id = id;
_idRoom = idRoom;
_type = type;
_score = score;
_pointCloud = pointCloud;
_pose = position;
_scale = scale;
}
public SemanticObjectMsg(JSONNode msg)
{
_id = msg["id"];
_idRoom = msg["idRoom"];
_type = msg["type"];
_score = double.Parse(msg["score"], System.Globalization.CultureInfo.InvariantCulture);
_pointCloud = new PointCloudMsg(msg["pointCloud"]);
_pose = new PoseMsg(msg["pose"]);
_scale = new Vector3Msg(msg["scale"]);
}
public static void GUI(this Vector3Msg message, string name)
{
string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]";
if (name == null || name == "")
{
GUILayout.Label(body);
}
else
{
GUILayout.Label($"{name}: {body}");
}
}
public SemanticObjectMsg(JSONNode msg)
{
_id = msg["id"];
_scores = new ObjectHypothesisMsg[msg["scores"].Count];
for (int i = 0; i < _scores.Length; i++)
{
_scores[i] = new ObjectHypothesisMsg(msg["scores"][i]);
}
_pose = new PoseMsg(msg["pose"]);
_detections = int.Parse(msg["detections"]);
_roomId = msg["roomId"];
_roomType = msg["roomType"];
_size = new Vector3Msg(msg["size"]);
}
private void Updatemess()
{
float deltatime = Time.realtimeSinceStartup - pretime;
Vector3 linacc = (game.transform.position - patpos) / (deltatime);
Vector3 angularvel = (game.transform.rotation.eulerAngles - qn.eulerAngles) / deltatime;
Vector3 rota = game.transform.rotation.eulerAngles;
pretime = Time.realtimeSinceStartup;
patpos = game.transform.position;
qn = game.transform.rotation;
geometryVector3 = new Vector3Msg(linacc.x, linacc.y, linacc.z);
geomv = new Vector3Msg(angularvel.x, angularvel.y, angularvel.z);
geonm = new Vector3Msg(rota.x, rota.y, rota.z);
msg = new Info(geometryVector3, geomv, geonm);
GameObject.Find("Cube").GetComponent <ROSInitialize>().ros.Publish(contInfo.GetMessageTopic(), msg);
}
public ImuMessage(JSONNode msg)
{
_header = new HeaderMsg(msg ["header"]);
_quaternionIMU = new QuaternionMsg(msg ["orientation"]);
/*
* for (int i = 0; i < 8; i++) {
* _orientation_c [i] = double.Parse (msg ["orientation_covariance"] [i]);
* _angular_velocity_c [i]= double.Parse (msg ["angular_velocity_covariance"][i]);
* _linear_acceleration_c [i]= double.Parse (msg ["linear_acceleration_covariance"][i]);
* }
*/
_angularVelocity = new Vector3Msg(msg ["angular_velocity"]);
_linearAcceleration = new Vector3Msg(msg ["linear_acceleration"]);
//Debug.Log ("IMU Message Done");
}
public static void DrawAxisVectors <C>(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new()
{
Vector3 unityPosition = position.From <C>();
Quaternion unityRotation = rotation.From <C>();
Vector3 x, y, z;
if (drawUnityAxes)
{
x = unityRotation * new Vector3(1, 0, 0) * size;
y = unityRotation * new Vector3(0, 1, 0) * size;
z = unityRotation * new Vector3(0, 0, 1) * size;
}
else
{
x = unityRotation * new Vector3 <C>(1, 0, 0).toUnity *size;
y = unityRotation * new Vector3 <C>(0, 1, 0).toUnity *size;
z = unityRotation * new Vector3 <C>(0, 0, 1).toUnity *size;
}
drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f);
}
public TwistMsg(JSONNode msg)
{
_linear = new Vector3Msg(msg["linear"]);
_angular = new Vector3Msg(msg["angular"]);
}
public void setAngular(Vector3Msg rosAngular)
{
OdomAngular = rosAngular;
}
public static void GUI(this Vector3Msg message)
{
GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]");
}
public void setLinear(Vector3Msg rosLinear)
{
OdomLinear = rosLinear;
}
public TransformMsg(JSONNode msg)
{
_translation = new Vector3Msg(msg["translation"]);
_rotation = new QuaternionMsg(msg["rotation"]);
}
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
ROSBridgeMsg result = null;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/dji_sdk/attitude":
QuaternionMsg attitudeMsg = (parsed == null) ? new QuaternionMsg(raw_msg["quaternion"]) : (QuaternionMsg)parsed;
attitude = offset * (new Quaternion(attitudeMsg.GetX(), attitudeMsg.GetY(), attitudeMsg.GetZ(), attitudeMsg.GetW()));
result = attitudeMsg;
break;
case "/dji_sdk/battery_state":
battery_state = (parsed == null) ? new BatteryStateMsg(raw_msg) : (BatteryStateMsg)parsed;
result = battery_state;
break;
case "/dji_sdk/flight_status":
if (isM100)
{
m100_flight_status = (FlightStatusM100)(new UInt8Msg(raw_msg)).GetData();
}
else
{
flight_status = (FlightStatus)(new UInt8Msg(raw_msg)).GetData();
}
break;
case "/dji_sdk/gimbal_angle":
Vector3Msg gimbleAngleMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
gimble_joint_angles = new Vector3((float)gimbleAngleMsg.GetX(), (float)gimbleAngleMsg.GetY(), (float)gimbleAngleMsg.GetZ());
result = gimbleAngleMsg;
break;
case "/dji_sdk/gps_health":
gps_health = (parsed == null) ? (new UInt8Msg(raw_msg)).GetData() : ((UInt8Msg)parsed).GetData();
break;
case "/dji_sdk/gps_position":
gps_position = (parsed == null) ? new NavSatFixMsg(raw_msg) : (NavSatFixMsg)parsed;
result = gps_position;
break;
case "/dji_sdk/imu":
imu = (parsed == null) ? new IMUMsg(raw_msg) : (IMUMsg)parsed;
result = imu;
break;
case "/dji_sdk/rc":
remote_controller_msg = (parsed == null) ? new JoyMsg(raw_msg) : (JoyMsg)parsed;
result = remote_controller_msg;
break;
case "/dji_sdk/velocity":
Vector3Msg velocityMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
velocity = new Vector3((float)velocityMsg.GetX(), (float)velocityMsg.GetY(), (float)velocityMsg.GetZ());
result = velocityMsg;
break;
case "/dji_sdk/height_above_takeoff":
relative_altitude = (parsed == null) ? (new Float32Msg(raw_msg)).GetData() : ((Float32Msg)parsed).GetData();
break;
case "/dji_sdk/local_position":
PointMsg pointMsg = (parsed == null) ? new PointMsg(raw_msg["point"]) : (PointMsg)parsed;
local_position = new Vector3(pointMsg.GetX(), pointMsg.GetY(), pointMsg.GetZ());
result = pointMsg;
Debug.Log(result);
break;
default:
Debug.LogError("Topic not implemented: " + topic);
break;
}
return(result);
}
public Info(JSONNode msg)
{
_linear = new Vector3Msg(msg["linear"]);
_angular = new Vector3Msg(msg["angular"]);
_rota = new Vector3Msg(msg["rota"]);
}
public static Vector3 From <C>(this Vector3Msg self) where C : ICoordinateSpace, new()
{
return(new Vector3 <C>((float)self.x, (float)self.y, (float)self.z).toUnity);
}
public Info(Vector3Msg linear, Vector3Msg angular, Vector3Msg rota)
{
_linear = linear;
_angular = angular;
_rota = rota;
}
public TwistMsg(Vector3Msg linear, Vector3Msg angular)
{
_linear = linear;
_angular = angular;
}
public TransformMsg(Transform tf)
{
_translation = new Vector3Msg(tf.localPosition, true);
_rotation = new QuaternionMsg(tf.localRotation, true);
}
public TransformMsg(Vector3Msg translation, QuaternionMsg rotation)
{
_translation = translation;
_rotation = rotation;
}
public override void Deserialize(JSONNode msg)
{
_linear = new Vector3Msg(msg["linear"]);
_angular = new Vector3Msg(msg["angular"]);
}
