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;
	}
示例#2
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);
        }
示例#3
0
 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);
        }
示例#7
0
    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)));
        }
    }
示例#8
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);
 }
示例#10
0
 public Stamped(std_msgs.Time stamp, string frameId, T data)
 {
     this.Stamp   = stamp;
     this.FrameId = frameId;
     this.Data    = data;
 }
示例#11
0
 /// <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))));
 }
示例#12
0
 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;
 }
示例#13
0
 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;
 }
示例#14
0
 public emTransform(UnityEngine.GameObject go, Time t = null, string fid = null, string cfi = null)
         : this(go.transform,t,fid,cfi)
 {
 }
示例#15
0
 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;
 }
示例#16
0
 public Stamped(std_msgs.Time t, string f, T d)
 {
     stamp    = t;
     frame_id = f;
     data     = d;
 }
示例#17
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;
 }