// Update is called once per frame void Update() { Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage(); Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1]; arr[0] = new Messages.geometry_msgs.TransformStamped(); tfmsg.transforms = arr; Transform trans = trackedObject.transform; emTransform ta = new emTransform(trans, ROS.GetTime(), frame_id, child_frame_id); Messages.std_msgs.Header hdr = new Messages.std_msgs.Header(); hdr.frame_id = frame_id; hdr.stamp = ROS.GetTime(); if (!using_gazebo) { hdr.stamp.data.sec += 18000; } tfmsg.transforms[0].header = hdr; tfmsg.transforms[0].child_frame_id = child_frame_id; tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform(); tfmsg.transforms[0].transform.translation = ta.origin.ToMsg(); //tfmsg.transforms[0].transform.translation.z += 1.0; tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; tfPub.publish(tfmsg); }
void joyTwistCallback(Joy joy) { TwistStamped velocity = new TwistStamped(); velocity.header.Frame_id = baseStabilizedFrame; velocity.header.Stamp = ROS.GetTime(); velocity.twist.linear.x = getAxis(joy, sAxes.x); velocity.twist.linear.y = getAxis(joy, sAxes.y); velocity.twist.linear.z = getAxis(joy, sAxes.z); velocity.twist.angular.z = getAxis(joy, sAxes.yaw) * Math.PI / 180.0; if (getButton(joy, sButtons.slow)) { velocity.twist.linear.x *= slowFactor; velocity.twist.linear.y *= slowFactor; velocity.twist.linear.z *= slowFactor; velocity.twist.angular.z *= slowFactor; } velocityPublisher.publish(velocity); if (getButton(joy, sButtons.stop)) { enableMotors(false); } else if (getButton(joy, sButtons.go)) { enableMotors(true); } }
private void CancelCallback(GoalID goalId) { if (!started) { return; } ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new cancel request"); if (goalId.id == null) { var timeZero = DateTime.UtcNow; foreach (var valuePair in goalHandles) { var goalHandle = valuePair.Value; if ((ROS.GetTime(goalId.stamp) == timeZero) || (ROS.GetTime(goalHandle.GoalId.stamp) < ROS.GetTime(goalId.stamp))) { if (goalHandle.SetCancelRequested() && (cancelCallback != null)) { cancelCallback(goalHandle); } } } } else { ServerGoalHandle <TGoal, TResult, TFeedback> goalHandle; var foundGoalHandle = goalHandles.TryGetValue(goalId.id, out goalHandle); if (foundGoalHandle) { if (goalHandle.SetCancelRequested() && (cancelCallback != null)) { cancelCallback(goalHandle); } } else { // We have not received the goal yet, prepare to cancel goal when it is received var goalStatus = new GoalStatus { status = GoalStatus.RECALLING }; goalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, null) { DestructionTime = ROS.GetTime(goalId.stamp) }; //lock( lockGoalHandles ) //{ goalHandles[goalId.id] = goalHandle; //} } } // Make sure to set lastCancel based on the stamp associated with this cancel request if (ROS.GetTime(goalId.stamp) > lastCancel) { lastCancel = ROS.GetTime(goalId.stamp); } }
public static void RosoutCallback(Messages.rosgraph_msgs.Log msg) { string pfx = "[?]"; switch (msg.level) { case Log.DEBUG: pfx = "[DEBUG]"; break; case Log.ERROR: pfx = "[ERROR]"; break; case Log.FATAL: pfx = "[FATAL]"; break; case Log.INFO: pfx = "[INFO]"; break; case Log.WARN: pfx = "[WARN]"; break; } TimeData td = ROS.GetTime().data; Console.WriteLine("[" + td.sec + "." + td.nsec + "]: " + pfx + ": " + msg.msg + " (" + msg.file + " (" + msg.function + " @" + msg.line + "))"); pub.publish(msg); }
bool GetPathService(GetPath.Request req, ref GetPath.Response resp) { Debug.Log("getpath service called!"); Path path = new Path(); path.header = new Messages.std_msgs.Header(); path.header.Frame_id = "global"; path.header.Stamp = ROS.GetTime(); path.header.Seq = 0; Pathing.PathSample[] samples = PathPlanner.GetPath(); int count = samples.Length; path.poses = new PoseStamped[count]; Debug.Log("sending " + count + " samples"); for (int i = 0; i < count; i++) { PoseStamped pst = new PoseStamped(); pst.header = new Messages.std_msgs.Header(); pst.header.Frame_id = "local"; pst.header.Stamp = ROS.GetTime(); pst.header.Seq = (uint)i; pst.pose = new Messages.geometry_msgs.Pose(); pst.pose.position = new Messages.geometry_msgs.Point(samples [i].position.ToRos()); pst.pose.orientation = new Messages.geometry_msgs.Quaternion(samples [i].orientation.ToRos()); path.poses [i] = pst; } resp.path = path; return(true); }
public void reset() { lock ( mutex_ ) { propulsion_model_.x.Initialize(); propulsion_model_.x_pred.Initialize(); propulsion_model_.u.Initialize(); propulsion_model_.y.Initialize(); wrench_ = new Wrench(); motor_status_ = new MotorStatus(); motor_status_.voltage = new float[4]; motor_status_.frequency = new float[4]; motor_status_.current = new float[4]; supply_ = new Supply(); supply_.voltage = new float[1]; supply_.voltage[0] = (float)initial_voltage_; last_command_time_ = ROS.GetTime(); command_queue_ = new Queue <MotorPWM> (); } }
public ClientGoalHandle <ActionSpec> initGoal <TGoal> (TGoal goal, TransitionCallback <ActionSpec> transitionCallback, FeedbackCallback <ActionSpec> feedbackCallback) where TGoal : AGoal { AActionGoal actionGoal = new ActionSpec().NewActionGoal(); // AActionGoal actionGoal = new AActionGoal (); actionGoal.Header.Stamp = ROS.GetTime(); actionGoal.GoalID = idGenerator.generateID(); actionGoal.Goal = goal; // actionGoal.header.Stamp = ROS.GetTime (); // actionGoal.goal_id = idGenerator.generateID (); // actionGoal.goal = goal; CommStateMachine <ActionSpec> commStateMachine = new CommStateMachine <ActionSpec> (actionGoal, transitionCallback, feedbackCallback); lock ( lockObject ) { var handle = list.Add(commStateMachine); if (sendGoalDelegate != null) { sendGoalDelegate(actionGoal); } else { ROS.Warn("actionlib", "Possible coding error: sendGoalDelegate set to NULL. Not going to send goal"); } return(new ClientGoalHandle <ActionSpec> (this, handle, guard)); } }
public TransformerFacts(RosFixture rosFixture) { this.rosFixture = rosFixture; when = ROS.GetTime(); transformer = new tf.Transformer(); }
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 ); }
void Update() { if (publishtf) { _tfmsg.transforms[0].header.stamp = ROS.GetTime(); _tfmsg.transforms[0].header.stamp.data.sec += 18000; //windows time is dumb //Debug.Log("Current time" + ROS.GetTime().data.sec); tfPub.publish(_tfmsg); } if (hj == null) { hj = GameObject.Find(frame_id); } else if (!interactableItem.IsInteracting()) { if (!startedInteracting) { //Debug.Log("Not interacting!"); Vector3 t = hj.transform.position; Quaternion q = hj.transform.rotation; transform.position = t; transform.rotation = q; } if (startedInteracting) { Messages.tf.tfMessage tfmsg = new Messages.tf.tfMessage(); Messages.geometry_msgs.TransformStamped[] arr = new Messages.geometry_msgs.TransformStamped[1]; arr[0] = new Messages.geometry_msgs.TransformStamped(); tfmsg.transforms = arr; //Transform trans = trackedObj.transform; emTransform ta = new emTransform(transform, ROS.GetTime(), "/world", "/look_at_frame"); Messages.std_msgs.Header hdr = new Messages.std_msgs.Header(); hdr.frame_id = "/world"; hdr.stamp = ROS.GetTime(); hdr.stamp.data.sec += 18000; tfmsg.transforms[0].header = hdr; tfmsg.transforms[0].child_frame_id = "/look_at_frame"; tfmsg.transforms[0].transform = new Messages.geometry_msgs.Transform(); tfmsg.transforms[0].transform.translation = ta.origin.ToMsg(); tfmsg.transforms[0].transform.rotation = ta.basis.ToMsg(); tfmsg.Serialized = null; tfPub.publish(tfmsg); _tfmsg = tfmsg; publishtf = true; startedInteracting = false; } } else { startedInteracting = true; } }
/// <summary> /// get blank TransformStamped all member is filled (no null pointer) /// you can just fill members such as x,y,z /// </summary> /// <returns>Blank TransformStamped data for addTf</returns> public GM.TransformStamped BlankTf() { GM.TransformStamped tfdata = new GM.TransformStamped(); tfdata.header = new Messages.std_msgs.Header(); tfdata.transform = new GM.Transform(); tfdata.header.stamp = ROS.GetTime(); tfdata.transform = new GM.Transform(); tfdata.transform.translation = new GM.Vector3(); tfdata.transform.rotation = new GM.Quaternion(); return(tfdata); }
void thrustCommandCb (ThrustCommand command) { lock ( command_mutex_ ) { thrust_command_ = command; if ( thrust_command_.header.Stamp.data.toSec () == 0.0 ) { attitude_command_.header.Stamp = ROS.GetTime (); } } }
public void PublishFeedback(GoalStatus goalStatus, TFeedback feedback) { var newFeedback = new FeedbackActionMessage <TFeedback>(); newFeedback.Header = new Messages.std_msgs.Header(); newFeedback.Header.stamp = ROS.GetTime(); newFeedback.GoalStatus = goalStatus; newFeedback.Feedback = feedback; ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing feedback for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}"); feedbackPublisher.publish(newFeedback); }
void yawrateCommandCb (YawRateCommand command) { lock ( command_mutex_ ) { yawrate_command_ = command; if ( yawrate_command_.header.Stamp.data.toSec () == 0.0 ) { yawrate_command_.header.Stamp = ROS.GetTime (); } } }
/** * \brief Sends a cancel message for this specific goal to the ActionServer * * Also transitions the Communication State Machine to WAITING_FOR_CANCEL_ACK */ public void cancel() { if (!isActive) { ROS.Error("actionlib", "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle"); return; } UnityEngine.Debug.Assert(goalManager != null); DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(guard); if (!protector.isProtected()) { ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this call"); return; } lock ( lockObject ) { switch (listHandle.GetElement().getCommState().state) { case CommState.StateEnum.WAITING_FOR_GOAL_ACK: case CommState.StateEnum.PENDING: case CommState.StateEnum.ACTIVE: case CommState.StateEnum.WAITING_FOR_CANCEL_ACK: break; // Continue standard processing case CommState.StateEnum.WAITING_FOR_RESULT: case CommState.StateEnum.RECALLING: case CommState.StateEnum.PREEMPTING: case CommState.StateEnum.DONE: ROS.Debug("actionlib", "Got a cancel() request while in state [%s], so ignoring it", listHandle.GetElement().getCommState().toString()); return; default: ROS.Error("actionlib", "BUG: Unhandled CommState: %u", listHandle.GetElement().getCommState().state); return; } } AActionGoal actionGoal = listHandle.GetElement().getActionGoal(); Messages.actionlib_msgs.GoalID cancelMsg = new Messages.actionlib_msgs.GoalID(); cancelMsg.stamp = ROS.GetTime(); cancelMsg.id = listHandle.GetElement().getActionGoal().GoalID.id; // cancelMsg.id = listHandle.GetElement ().getActionGoal ()->goal_id.id; if (goalManager.cancelDelegate != null) { goalManager.cancelDelegate(cancelMsg); } listHandle.GetElement().transitionToState(this, CommState.StateEnum.WAITING_FOR_CANCEL_ACK); }
public bool waitForTransform(string target_frame, string source_frame, Time time, Duration timeout, out string error_msg, Duration pollingSleepDuration) { TimeSpan?ts = null; if (pollingSleepDuration != null) { ts = ROS.GetTime(pollingSleepDuration); } return(waitForTransform(target_frame, source_frame, time, ROS.GetTime(timeout), out error_msg, ts)); }
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); } }
public ClientGoalHandle <TGoal, TResult, TFeedback> SendGoal( TGoal goal, Action <ClientGoalHandle <TGoal, TResult, TFeedback> > OnTransistionCallback = null, Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > OnFeedbackCallback = null ) { // Create Goal Message; var goalId = new GoalID(); lock (lockId) { var now = ROS.GetTime(); // Create sortable goal id goalId.id = $"{ThisNode.Name}-{nextGoalId:x08}-{now.data.sec:x08}.{now.data.nsec:x08}"; goalId.stamp = now; nextGoalId = nextGoalId + 1; } // Prepare Goal Message var goalAction = new GoalActionMessage <TGoal> { Header = new Messages.std_msgs.Header { stamp = ROS.GetTime() }, GoalId = goalId, Goal = goal }; // Register goal message var goalHandle = new ClientGoalHandle <TGoal, TResult, TFeedback>( this, goalAction, OnTransistionCallback, OnFeedbackCallback ); lock (gate) { goalHandles[goalAction.GoalId.id] = goalHandle; } // Publish goal message GoalPublisher.Publish(goalAction); ROS.Debug()("Goal published: {0}", goalHandle.Id); return(goalHandle); }
private void callback(object o) { CompressedImage cm = new CompressedImage { format = "jpeg", header = new Header { stamp = ROS.GetTime() } }; using (MemoryStream ms = new MemoryStream()) { PInvoke.CaptureWindow(hwnd, window_left, window_top, ms, ImageFormat.Jpeg); cm.data = ms.GetBuffer(); } pub.publish(cm); }
public void PublishResult(GoalStatus goalStatus, TResult result) { var newResult = new ResultActionMessage <TResult>(); newResult.Header = new Messages.std_msgs.Header(); newResult.Header.stamp = ROS.GetTime(); newResult.GoalStatus = goalStatus; if (result != null) { newResult.Result = result; } ROS.Debug()($"[{ThisNode.Name}] [actionlib] Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: {new DateTimeOffset( ROS.GetTime( goalStatus.goal_id.stamp ) ).ToUnixTimeSeconds()}"); resultPublisher.publish(newResult); PublishStatus(); }
/** * \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); }
public ServerGoalHandle(IActionServer <TGoal, TResult, TFeedback> actionServer, GoalID goalId, GoalStatus goalStatus, TGoal goal) { this.actionServer = actionServer; GoalStatus = goalStatus; GoalId = goalId; GoalStatus.goal_id = goalId; if ((goalId.stamp == null) || (ROS.ToDateTime(goalId.stamp) == new DateTime(1970, 1, 1, 0, 0, 0))) { // If stamp is not initialized GoalStatus.goal_id.stamp = ROS.GetTime(); } GoalStatus = goalStatus; this.Goal = goal; }
public bool waitForActionServerToStart(duration timeout, NodeHandle nh) { if (timeout < new duration()) { ROS.Error("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.data.sec.ToString() + ":" + timeout.data.nsec.ToString()); } Messages.std_msgs.Time timeout_time = ROS.GetTime() + timeout; lock ( lockObject ) { if (isServerConnected()) { return(true); } // Hardcode how often we check for node.ok() duration loop_period = new duration(new Messages.TimeData(0, 500000000)); // ".fromSec(.5)" while (nh.ok && !isServerConnected()) { // 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(new Messages.TimeData(0, 0)) && time_left <= new duration(new Messages.TimeData(0, 0))) { break; } // Truncate the time left if (time_left > loop_period || timeout == new duration()) { time_left = loop_period; } uint msWait = time_left.data.sec * 1000 + time_left.data.nsec / 1000000; Monitor.Wait(lockObject, UnityEngine.Mathf.Max((int)msWait, 0)); } return(isServerConnected()); } }
/** * \brief Generates a unique ID * \return A unique GoalID for this action */ public GoalID generateID() { GoalID id = new GoalID(); Messages.std_msgs.Time t = ROS.GetTime(); Messages.TimeData td = ROS.GetTime().data; string s = name + "-"; lock ( lockObject ) { goalCount++; s += goalCount + "-"; } s += td.sec + "." + td.nsec; id.id = s; id.stamp = t; return(id); }
private void GoalCallback(GoalActionMessage <TGoal> goalAction) { if (!started) { return; } GoalID goalId = goalAction.GoalId; ROS.Debug()($"[{ThisNode.Name}] [actionlib] The action server has received a new goal request"); ServerGoalHandle <TGoal, TResult, TFeedback> observedGoalHandle = null; if (goalHandles.ContainsKey(goalId.id)) { observedGoalHandle = goalHandles[goalId.id]; } if (observedGoalHandle != null) { // The goal could already be in a recalling state if a cancel came in before the goal if (observedGoalHandle.GoalStatus.status == GoalStatus.RECALLING) { observedGoalHandle.GoalStatus.status = GoalStatus.RECALLED; PublishResult(observedGoalHandle.GoalStatus, null); // Empty result } } else { // Create and register new goal handle GoalStatus goalStatus = new GoalStatus(); goalStatus.status = GoalStatus.PENDING; var newGoalHandle = new ServerGoalHandle <TGoal, TResult, TFeedback>(this, goalId, goalStatus, goalAction.Goal ); newGoalHandle.DestructionTime = ROS.GetTime(goalId.stamp); //lock( lockGoalHandles ) //{ goalHandles[goalId.id] = newGoalHandle; //} goalCallback?.Invoke(newGoalHandle); } }
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 PublishStatus() { var now = DateTime.UtcNow; //if (this.statusArray == null) //{ // this.statusArray = new GoalStatusArray(); // this.statusArray.header = new Messages.std_msgs.Header(); //} var statusArray = new GoalStatusArray { header = new Messages.std_msgs.Header() }; statusArray.header.stamp = ROS.GetTime(now); goalStatuses.Clear(); idsToBeRemoved.Clear(); //lock( lockGoalHandles ) //{ foreach (var pair in goalHandles) { goalStatuses.Add(pair.Value.GoalStatus); if ((pair.Value.DestructionTime != null) && (pair.Value.DestructionTime + StatusListTimeout < now)) { ROS.Debug()($"[{ThisNode.Name}] [actionlib] Removing server goal handle for goal id: {pair.Value.GoalId.id}"); idsToBeRemoved.Add(pair.Value.GoalId.id); } } statusArray.status_list = goalStatuses.ToArray(); goalStatusPublisher.publish(statusArray); foreach (string id in idsToBeRemoved) { goalHandles.TryRemove(id, out var dummy); } //} }
public void joyAttitudeCallback(Joy joy) { AttitudeCommand attitude = new AttitudeCommand(); ThrustCommand thrust = new ThrustCommand(); YawRateCommand yawrate = new YawRateCommand(); attitude.header.Stamp = thrust.header.Stamp = yawrate.header.Stamp = ROS.GetTime(); attitude.header.Frame_id = yawrate.header.Frame_id = baseStabilizedFrame; thrust.header.Frame_id = baseLinkFrame; attitude.roll = (float)(-getAxis(joy, sAxes.y) * Math.PI / 180.0); attitude.pitch = (float)(getAxis(joy, sAxes.x) * Math.PI / 180.0); if (getButton(joy, sButtons.slow)) { attitude.roll *= (float)slowFactor; attitude.pitch *= (float)slowFactor; } attitudePublisher.publish(attitude); thrust.thrust = (float)getAxis(joy, sAxes.thrust); thrustPublisher.publish(thrust); yawrate.turnrate = (float)(getAxis(joy, sAxes.yaw) * Math.PI / 180.0); if (getButton(joy, sButtons.slow)) { yawrate.turnrate *= (float)slowFactor; } yawRatePublisher.publish(yawrate); if (getButton(joy, sButtons.stop)) { enableMotors(false); } else if (getButton(joy, sButtons.go)) { enableMotors(true); } }
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))); } }