public emTransform transformFrame(string source, string target, out gm.Vector3 vec, out gm.Quaternion quat)
{
emTransform trans = new emTransform();
try
{
transformer.lookupTransform(target, source, new Time(new TimeData()), out trans);
}
catch (Exception e)
{
ROS.Error(e.ToString());
trans = null;
}
if (trans != null)
{
vec = trans.translation != null?trans.translation.ToMsg() : new emVector3().ToMsg();
quat = trans.rotation != null?trans.rotation.ToMsg() : new emQuaternion().ToMsg();
}
else
{
vec = null;
quat = null;
}
return(trans);
}
/// <summary>
/// Publishes the example geometry_msgs/Vector3 message
/// </summary>
void PublishVector3()
{
Messages.geometry_msgs.Vector3 msg = new Messages.geometry_msgs.Vector3();
msg.x = 1.0f;
msg.y = 2.0f;
msg.z = 3.0f;
// Publish
pubVector3.publish(msg);
}
void Update ()
{
Quaternion qTemp = lastImu.orientation.ToUnity ();
Vector3 euler = qTemp.eulerAngles;
double rollCmd = rollController.Update ( euler.x, ROS.GetTime ().data.toSec () );
double pitchCmd = pitchController.Update ( euler.y, ROS.GetTime ().data.toSec () );
double yawCmd = yawController.Update ( euler.z, ROS.GetTime ().data.toSec () );
string s = string.Format ( "pry: {0},{1},{2} orientation: {3},{4},{5},{6}", euler.x, euler.y, euler.z, qTemp.x, qTemp.y, qTemp.z, qTemp.w );
Debug.Log ( s );
s = string.Format ( "pry in degrees: {0},{1},{2}", euler.x * Mathf.Rad2Deg, euler.y * Mathf.Rad2Deg, euler.z * Mathf.Rad2Deg );
Debug.Log ( s );
Vec3 v = new Vec3 ();
v.x = rollCmd;
v.y = pitchCmd;
v.z = yawCmd;
torqueCallback ( v );
thrustCallback ( zThrust );
}
public void UpdatePose(PoseStamped ps)
{
if (!firstPoseReceived)
{
firstPoseReceived = true;
SetGoal(ps.pose.position);
}
Vec3 rpyCmd = new Vec3();
double radian = System.Math.PI / 180;
double t = ps.header.Stamp.data.toSec();
// Control Roll to to move along Y
double rollCmd = xController.Update(ps.pose.position.x, t);
rollCmd = Clamp(rollCmd, -10.0 * radian, 10.0 * radian);
// Control Pitch to move along X
double pitchCmd = yController.Update(ps.pose.position.y, t);
pitchCmd = Clamp(pitchCmd, -10.0 * radian, 10.0 * radian);
// Control Thrust to move along Z
double thrust = zController.Update(ps.pose.position.z, t);
rpyCmd.x = rollCmd;
rpyCmd.y = pitchCmd;
string s = string.Format("r: {0}, p: {1}, thrust: {2}", rollCmd, pitchCmd, thrust);
Debug.Log(s);
// publish
torqueCallback(rpyCmd);
thrustCallback(thrust);
}
public emVector3(gm.Vector3 shallow) : this(shallow.x, shallow.y, shallow.z)
{
}
void UpdateAttitude (Vec3 attVector)
{
rollController.SetTarget ( attVector.x );
pitchController.SetTarget ( attVector.y );
yawController.SetTarget ( attVector.z );
}
/// <summary>
/// Callback fired when the pubVector3 Topic receives a message
/// </summary>
/// <param name="data">Received message</param>
void OnReceiveVector3(Messages.geometry_msgs.Vector3 data)
{
Debug.LogFormat("Received Vector3 message: ({0}, {1}, {2})", data.x.ToString("F4"), data.y.ToString("F4"), data.z.ToString("F4"));
}
