void PublishAll()
{
int sleep = 1000 / 60;
while (ROS.ok && !ROS.shutting_down)
{
PoseStamped ps = new PoseStamped();
ps.pose = new Messages.geometry_msgs.Pose();
ps.pose.position = new Messages.geometry_msgs.Point(droneController.Position.ToRos());
ps.pose.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
posePub.publish(ps);
Imu imu = new Imu();
imu.angular_velocity_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.linear_acceleration_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.orientation_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.angular_velocity = new GVector3(droneController.AngularVelocity.ToRos());
imu.linear_acceleration = new GVector3(droneController.LinearAcceleration.ToRos());
imu.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
imuPub.publish(imu);
Thread.Sleep(sleep);
}
}
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;
}
private void imu_callback(sm.Imu i)
{
emQuaternion q = new emQuaternion(i.orientation);
emVector3 rot = q.getRPY();
//Console.WriteLine("" + euler.roll + " " + euler.pitch + " " + euler.yaw);
Dispatcher.BeginInvoke(new Action(() =>
{
if (abraCadabra.Visibility != Visibility.Visible)
{
abraCadabra.Visibility = Visibility.Visible;
}
rotate(rot.y);
translate(rot.x);
}));
}
void OldPublishAll()
{
// pose info
PoseStamped ps = new PoseStamped();
// ps.header = new Messages.std_msgs.Header ();
// ps.header.Stamp = ROS.GetTime ();
// ps.header.Frame_id = "";
ps.pose = new Messages.geometry_msgs.Pose();
Imu imu = new Imu();
// imu info
// imu.header = new Messages.std_msgs.Header ( ps.header );
imu.angular_velocity_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.linear_acceleration_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.orientation_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
// image info
Image img = new Image();
// img.header = new Messages.std_msgs.Header ( ps.header );
img.width = (uint)QuadMotor.ImageWidth;
img.height = (uint)QuadMotor.ImageHeight;
img.encoding = "mono16"; // "rgba8";
img.step = img.width * 2; // * 4
img.is_bigendian = 1;
// int sleep = 1000 / 30;
// int sleep = 1000 / 2;
int sleep = 1000 / 60;
// int sleep = 1000 / 120;
// int sleep = 1000 / 250;
#if TIMETEST
Queue <TimeData> tdq = new Queue <TimeData> ();
Queue <long> tq = new Queue <long> ();
#endif
while (ROS.ok && !ROS.shutting_down)
{
#if TIMETEST
tdq.Enqueue(ROS.GetTime().data);
tq.Enqueue(System.DateTime.Now.Ticks);
#endif
// publish pose
///*
// ps.header = new Messages.std_msgs.Header ();
// ps.header.Stamp = ROS.GetTime ();
// ps.header.Frame_id = ps.header.Stamp.data.toSec ().ToString ();
// ps.header.Frame_id = "";
// ps.header.Seq = frameSeq;
ps.pose.position = new Messages.geometry_msgs.Point(droneController.Position.ToRos());
ps.pose.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
posePub.publish(ps);
if (ps.header != null)
{
Debug.Log("Send Time " + ps.header.Stamp.data.toSec());
}
else
{
Debug.Log("Send time null header");
}
// */
// publish imu
///*
// imu.header = new Messages.std_msgs.Header ();
// imu.header.Frame_id = "";
// imu.header.Seq = frameSeq++;
// imu.header.Stamp = ROS.GetTime ();
// imu.header.Stamp = ps.header.Stamp;
imu.angular_velocity = new GVector3(droneController.AngularVelocity.ToRos());
imu.linear_acceleration = new GVector3(droneController.LinearAcceleration.ToRos());
imu.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
imuPub.publish(imu);
// Debug.Log ( "Send Imu " + imu.header.Seq );
// publish image
// img.data = droneController.GetImageData ();
// imgPub.publish ( img );
// */
Thread.Sleep(sleep);
}
#if TIMETEST
while (tdq.Count > 0)
{
TimeData td = tdq.Dequeue();
Debug.Log("t: " + td.sec + " " + td.nsec);
}
while (tq.Count > 0)
{
Debug.Log("ticks: " + tq.Dequeue());
}
#endif
}
public void UpdateImu (Imu imu)
{
lastImu = imu;
Update ();
}
