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;
	}
Пример #3
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 ();
	}