public AttitudeControllerNode ()
{
prevTime = new RosTime ( new Messages.TimeData ( 0, 0 ) );
// Roll controller
double rollKp = 0;
double rollKi = 0;
double rollKIMax = 0;
double rollKd = 0;
rollController = new PIDController ( rollKp, rollKi, rollKd, rollKIMax, 0 );
// Pitch controller
double pitchKp = 0;
double pitchKi = 0;
double pitchKIMax = 0;
double pitchKd = 0;
pitchController = new PIDController ( pitchKp, pitchKi, pitchKd, pitchKIMax, 0 );
// Yaw controller
double yawKp = 0;
double yawKi = 0;
double yawKIMax = 0;
double yawKd = 0;
yawController = new PIDController ( yawKp, yawKi, yawKd, yawKIMax, 0 );
lastImu = new Imu ();
zThrust = 0;
}
/// <summary>
/// Turns a TimeSpan into a Time (not a Duration, although it sorta is)
/// </summary>
/// <param name="timestamp"> The timespan to convert to seconds/nanoseconds </param>
/// <returns> a time struct </returns>
public static m.Time GetTime(TimeSpan timestamp)
{
uint seconds = (((uint)Math.Floor(timestamp.TotalSeconds) & 0xFFFFFFFF));
uint nanoseconds = ((uint)Math.Floor(((timestamp.TotalSeconds - seconds) * 1000000000)));
m.Time stamp = new m.Time(new TimeData(seconds, nanoseconds));
return(stamp);
}
public TransformTest()
{
when = ROS.GetTime();
//
// TODO: Add constructor logic here
//
transformer = new Transformer();
}
public void Init ()
{
prevTime = ROS.GetTime ();
double t = prevTime.data.toSec ();
rollController.SetStartTime ( t );
pitchController.SetStartTime ( t );
yawController.SetStartTime ( t );
}
protected override void Callback(LaserScan argument)
{
if (lastStamp != null && ROS.GetTime(argument.header.stamp) < ROS.GetTime(lastStamp))
{
UnityEngine.Debug.LogError("TIME IS MOVING BACKWARDS");
}
lastStamp = argument.header.stamp;
lock (toDraw)
{
toDraw.Add(ROS.GetTime(argument.header.stamp), argument);
}
}
/**
* \brief Blocks until this goal finishes
* \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
* \return True if the goal finished. False if the goal didn't finish within the allocated timeout
*/
public bool waitForResult(Duration timeout)
{
if (goalHandle.isExpired())
{
ROS.Error("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
return(false);
}
if (timeout < new Duration(new TimeData(0, 0)))
{
ROS.Warn("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
}
Time timeout_time = ROS.GetTime() + timeout;
lock ( doneMutex )
{
// Hardcode how often we check for node.ok()
Duration loop_period = new Duration().fromSec(.1);
while (nodeHandle.ok)
{
// Determine how long we should wait
Duration time_left = timeout_time - ROS.GetTime();
// Check if we're past the timeout time
if (timeout > new Duration() && time_left <= new Duration())
{
break;
}
if (goalState == SimpleGoalState.StateEnum.DONE)
{
break;
}
// Truncate the time left
if (time_left > loop_period || timeout == new Duration())
{
time_left = loop_period;
}
}
// doneCondition.timed_wait(lock, boost.posix_time.milliseconds(time_left.toSec() * 1000.0f));
}
return(goalState == SimpleGoalState.StateEnum.DONE);
}
void joyPoseCallback(Joy joy)
{
Messages.std_msgs.Time now = ROS.GetTime();
double dt = 0.0;
if ((pose.header.Stamp.data.sec == 0 && pose.header.Stamp.data.nsec == 0))
{
TimeData td = (now - pose.header.Stamp).data;
double sec = td.toSec();
dt = Mathf.Max(0, Mathf.Min(1f, (float)sec));
}
if (getButton(joy, sButtons.go))
{
pose.header.Stamp = ROS.GetTime();
pose.header.Frame_id = worldFrame;
pose.pose.position.x += (Math.Cos(yaw) * getAxis(joy, sAxes.x) - Math.Sin(yaw) * getAxis(joy, sAxes.y)) * dt;
pose.pose.position.y += (Math.Cos(yaw) * getAxis(joy, sAxes.y) + Math.Sin(yaw) * getAxis(joy, sAxes.x)) * dt;
pose.pose.position.z += getAxis(joy, sAxes.z) * dt;
yaw += getAxis(joy, sAxes.yaw) * Math.PI / 180.0 * dt;
tf.net.emQuaternion q = tf.net.emQuaternion.FromRPY(new tf.net.emVector3(0, 0, yaw));
pose.pose.orientation = q.ToMsg();
PoseGoal goal = new PoseGoal();
goal.target_pose = pose;
poseClient.sendGoal(goal);
}
if (getButton(joy, sButtons.interrupt))
{
poseClient.cancelGoalsAtAndBeforeTime(ROS.GetTime());
}
if (getButton(joy, sButtons.stop))
{
landingClient.sendGoalAndWait(new LandingGoal(), new Messages.std_msgs.Duration(new TimeData(10, 0)), new Messages.std_msgs.Duration(new TimeData(10, 0)));
}
}
/// <summary>
/// Convert a std_msgs/Time mesage into a DateTime struct
/// </summary>
/// <param name="time"> std_msgs.Time to convert </param>
/// <returns> a DateTime </returns>
public static DateTime ToDateTime(this std_msgs.Time time)
{
return(new DateTime(1970, 1, 1, 0, 0, 0).Add(new TimeSpan(time.data.Ticks)));
}
/**
* \brief Cancel all goals that were stamped at and before the specified time
* \param time All goals stamped at or before `time` will be canceled
*/
public void cancelGoalsAtAndBeforeTime(Time time)
{
actionClient.cancelGoalsAtAndBeforeTime(time);
}
public Stamped(std_msgs.Time stamp, string frameId, T data)
{
this.Stamp = stamp;
this.FrameId = frameId;
this.Data = data;
}
/// <summary>
/// Turns a std_msgs.Time into a DateTime
/// </summary>
/// <param name="time"> std_msgs.Time to convert </param>
/// <returns> a DateTime </returns>
public static DateTime GetTime(m.Time time)
{
return(new DateTime(1970, 1, 1, 0, 0, 0).Add(new TimeSpan(ticksFromData(time.data))));
}
public emTransform(UnityEngine.Vector3 v, UnityEngine.Quaternion q, Time t = null, string fid = null, string cfi = null)
{
UnityRotation = q;
UnityPosition = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public emTransform(UnityEngine.Transform T, Time t = null, string fid = null, string cfi = null)
{
UnityRotation = T.rotation;
UnityPosition = T.position;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public emTransform(UnityEngine.GameObject go, Time t = null, string fid = null, string cfi = null)
: this(go.transform,t,fid,cfi)
{
}
public emTransform(emQuaternion q, emVector3 v, Time t = null, string fid = null, string cfi = null)
{
basis = q;
origin = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public Stamped(std_msgs.Time t, string f, T d)
{
stamp = t;
frame_id = f;
data = d;
}
/// <summary>
/// Turns a TimeSpan into a Time (not a Duration, although it sorta is)
/// </summary>
/// <param name="timestamp"> The timespan to convert to seconds/nanoseconds </param>
/// <returns> a time struct </returns>
public static m.Time GetTime(TimeSpan timestamp)
{
uint seconds = (((uint) Math.Floor(timestamp.TotalSeconds) & 0xFFFFFFFF));
uint nanoseconds = ((uint) Math.Floor(((timestamp.TotalSeconds - seconds)*1000000000)));
m.Time stamp = new m.Time(new TimeData(seconds, nanoseconds));
return stamp;
}