// 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)));
}
}
