public void SetAccepted(string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] [actionlib] Accepting goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if (GoalStatus.status == GoalStatus.PENDING)
{
SetGoalStatus(GoalStatus.ACTIVE, text);
}
else if (GoalStatus.status == GoalStatus.RECALLING)
{
SetGoalStatus(GoalStatus.PREEMPTING, text);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: {GoalStatus.status}");
}
}
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()("actionlib", $"Publishing result for goal with id: {goalStatus.goal_id.id} and stamp: " +
$"{new DateTimeOffset(ROS.ToDateTime(goalStatus.goal_id.stamp)).ToUnixTimeSeconds()}"
);
resultPublisher.publish(newResult);
PublishStatus();
}
public void listElemDeleter(List <CommStateMachine <ActionSpec> > .Enumerator it)
{
UnityEngine.Debug.Assert(guard != 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. Not going to try delete the CommStateMachine associated with this goal");
return;
}
ROS.Debug("actionlib", "About to erase CommStateMachine");
lock ( lockObject )
{
list.Remove(it.Current);
}
ROS.Debug("actionlib", "Done erasing CommStateMachine");
}
static void Main(string[] args)
{
// Setup the logging system
var loggerFactory = new LoggerFactory();
loggerFactory.AddProvider(
new ConsoleLoggerProvider(
(string text, LogLevel logLevel) => { return(logLevel >= LogLevel.Information); }, true)
);
Logger = ApplicationLogging.CreateLogger(nameof(Main));
ROS.SetLoggerFactory(loggerFactory);
NodeHandle nodeHandle;
string NODE_NAME = "ServiceServerTest";
ServiceServer server;
try
{
ROS.Init(new string[0], NODE_NAME);
}
catch (RosException e)
{
Logger.LogCritical("ROS.Init failed, shutting down: {0}", e.Message);
ROS.shutdown();
ROS.waitForShutdown();
return;
}
try
{
nodeHandle = new NodeHandle();
server = nodeHandle.advertiseService <TwoInts.Request, TwoInts.Response>("/add_two_ints", addition);
while (ROS.ok && server.IsValid)
{
Thread.Sleep(10);
}
}
catch (RosException e)
{
Logger.LogCritical("Shutting down: {0}", e.Message);
}
ROS.shutdown();
ROS.waitForShutdown();
}
private bool CheckActionServerStatusAndLog()
{
lock (gate)
{
if (IsServerConnected())
{
return(true);
}
ROS.Warn()($"ActionServer {this.Name} is not ready (status received: {statusReceived}; callerId: {statusCallerId}; "
+ $"goal: {statusCallerId != null && goalSubscriberCount.ContainsKey(statusCallerId)} ({goalSubscriberCount.Count}); "
+ $"cancel: {statusCallerId != null && cancelSubscriberCount.ContainsKey(statusCallerId)} ({cancelSubscriberCount.Count}); "
+ $"feedback: {feedbackSubscriber.NumPublishers}; result: {resultSubscriber.NumPublishers}).");
}
return(false);
}
public static void Start()
{
ROS.Init(new string[0], "rosout");
nh = new NodeHandle();
sub = nh.subscribe <Messages.rosgraph_msgs.Log>("/rosout", 10, RosoutCallback);
pub = nh.advertise <Messages.rosgraph_msgs.Log>("/rosout_agg", 1000, true);
new Thread(() =>
{
while (!ROS.ok)
{
Thread.Sleep(10);
}
}).Start();
}
public void SetCanceled(TResult result, string text)
{
text = text ?? "";
ROS.Debug()($"[{ThisNode.Name}] Setting status to canceled on goal, id: {GoalId.id}, stamp: {GoalId.stamp}");
if ((GoalStatus.status == GoalStatus.PENDING) || (GoalStatus.status == GoalStatus.RECALLING))
{
SetGoalResult(GoalStatus.RECALLED, text, result);
}
else if ((GoalStatus.status == GoalStatus.ACTIVE) || (GoalStatus.status == GoalStatus.PREEMPTING))
{
SetGoalResult(GoalStatus.PREEMPTED, text, result);
}
else
{
ROS.Error()($"[{ThisNode.Name}] [actionlib] To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: {GoalStatus.status}");
}
}
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.GetTime(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 stop (CommandHandle handle)
{
if ( handle == null )
return false;
string resource = handle.getName ();
if ( enabled_.ContainsKey ( resource ) )
{
if ( enabled_ [ resource ] == handle )
{
enabled_.Remove ( resource );
ROS.Info ( "quadrotor_interface", "Disabled %s control", resource );
return true;
}
}
return false;
}
private void waitThenSubscribe()
{
while (true)
{
Thread.Sleep(100);
lock (this)
if (ROS.shutting_down || ROS.isStarted())
{
break;
}
}
lock (this)
if (ROS.shutting_down)
{
return;
}
SubscribeToImage(__topic);
}
ActionClient <ActionSpec> actionClient; // Action client depends on callback_queue, so it must be destroyed before callback_queue
// ***** Private Funcs *****
void initSimpleClient(NodeHandle n, string name, bool spin_thread)
{
if (spin_thread)
{
ROS.Debug("actionlib", "Spinning up a thread for the SimpleActionClient");
needToTerminate = false;
spinThread = new Thread(DoSpin);
// spinThread = new boost.thread(boost.bind(&SimpleActionClient<ActionSpec>.spinThread, this));
actionClient = new ActionClient <ActionSpec> (n, name, callback_queue);
// actionClient.reset(new ActionClient<T>(n, name, &callback_queue));
}
else
{
spinThread = null;
actionClient = new ActionClient <ActionSpec> (n, name);
// actionClient.reset(new ActionClient<T>(n, name));
}
}
public bool SetCancelRequested()
{
ROS.Debug()("actionlib", $"Transisitoning to a cancel requested state on goal id: {GoalId.id}, stamp: {GoalId.stamp}");
bool result = false;
if (GoalStatus.status == GoalStatus.PENDING)
{
SetGoalStatus(GoalStatus.RECALLING, "RECALLING");
result = true;
}
if (GoalStatus.status == GoalStatus.ACTIVE)
{
SetGoalStatus(GoalStatus.PREEMPTING, "PREEMPTING");
result = true;
}
return(result);
}
public void cancelConnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
if (cancelSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Warn("cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name]++;
}
else
{
ROS.Debug("cancelConnectCallback: Adding [%s] to cancelSubscribers", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name] = 1;
}
ROS.Debug("%s", cancelSubscribersString());
}
// check_connection_condition_.notify_all();
}
public double getAxis(Joy joy, SAxis axis)
{
if (axis.axis == 0 || Math.Abs(axis.axis) > joy.axes.Length)
{
ROS.Error("Axis " + axis.axis + " out of range, joy has " + joy.axes.Length + " axes");
return(0);
}
double output = Math.Abs(axis.axis) / axis.axis * joy.axes[Math.Abs(axis.axis) - 1] * axis.factor + axis.offset;
// TODO keep or remove deadzone? may not be needed
// if (Math.Abs(output) < axis.max_ * 0.2)
// {
// output = 0.0;
// }
return(output);
}
static void Main(string[] args)
{
Console.WriteLine("Start roscore and ActionServerTesbed.lua and press any key");
while (!Console.KeyAvailable)
{
Thread.Sleep(1);
}
Console.WriteLine("Start ROS");
ROS.ROS_MASTER_URI = "http://rosvita:11311";
ROS.Init(new string[0], "ActionClient");
(new Program()).Start(1000);
//(new TestActionServerKill()).Start(5);
Thread.Sleep(10000);
ROS.Shutdown();
}
public CROS(string host_name, string master_url)
{
if (OnInitProcess != null)
{
OnInitProcess(0);
}
ROS.ROS_HOSTNAME = host_name;
ROS.ROS_MASTER_URI = master_url;
if (OnInitProcess != null)
{
OnInitProcess(20);
}
ROS.Init(new string[0], "Windows_node");
if (OnInitProcess != null)
{
OnInitProcess(40);
}
}
static ROSMonoBehavior()
{
rosmanager = new ROSManager();
#if UNITY_EDITOR
UnityEditor.EditorApplication.playmodeStateChanged = () =>
{
string state = "";
if (EditorApplication.isPlaying)
{
state += "playing";
}
if (EditorApplication.isPaused)
{
state += " paused";
}
if (EditorApplication.isCompiling)
{
state += " compiling";
}
state = state.Trim(' ');
Debug.LogWarning("PlayMode == " + state);
if (!EditorApplication.isPlaying && !EditorApplication.isPaused)
{
ROS.Unfreeze();
if (ROS.ok || ROS.isStarted())
{
ROSManager.StopROS();
}
}
else if (EditorApplication.isPlaying)
{
if (!EditorApplication.isPaused)
{
ROS.Unfreeze();
rosmanager.StartROS(null, null);
}
else
{
ROS.Freeze();
}
}
};
#endif
}
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 Init()
{
if (!ROS.isStarted())
{
if (waitforinit == null)
{
string workaround = Topic;
waitforinit = new Thread(() => waitfunc(workaround));
}
if (!waitforinit.IsAlive)
{
waitforinit.Start();
}
}
else
{
SetupTopic(Topic);
}
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher <m.String> Talker = node.advertise <m.String>("/Chatter", 1);
//Subscriber<m.String> Subscriber = node.subscribe<m.String>("/Chatter", 1, chatterCallback);
int count = 0;
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
ROS.shutdown();
}
/**
* \brief Stops goal handle from tracking a goal
*
* Useful if you want to stop tracking the progress of a goal, but it is inconvenient to force
* the goal handle to go out of scope. Has pretty much the same semantics as boost::shared_ptr::reset()
*/
public void reset()
{
if (isActive)
{
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 reset() call");
return;
}
lock ( lockObject )
{
// listHandle.reset(); // not sure what to replace this stuff with
isActive = false;
goalManager = null;
}
}
}
private void DrawMap()
{
if (!ROS.isStarted())
{
if (waitingThread == null)
{
string topicString = Topic;
waitingThread = new Thread(() => waitThenSubscribe(topicString));
}
if (!waitingThread.IsAlive)
{
waitingThread.Start();
}
}
else
{
SubscribeToMap(Topic);
}
}
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);
}
}
/* public void updateFeedback<TFeedback> (ClientGoalHandle<ActionSpec> gh, TFeedback actionFeedback) where TFeedback : AActionFeedback, new()
* {
* // Check if this feedback is for us
* if ( actionGoal.GoalID.id != actionFeedback.GoalStatus.goal_id.id )
* // if ( actionGoal.goal_id.id != actionFeedback.status.goal_id.id )
* return;
*
* if ( feedbackCallback != null )
* {
* // EnclosureDeleter<const ActionFeedback> d(actionFeedback);
* // FeedbackConstPtr feedback(&(actionFeedback->feedback), d);
* AFeedback feedback = actionFeedback.Feedback.Clone ();
* feedbackCallback ( gh, feedback );
* }
* }*/
public void updateResult(ClientGoalHandle <ActionSpec> gh, AActionResult actionResult)
{
// Check if this feedback is for us
if (actionGoal.GoalID.id != actionResult.GoalStatus.goal_id.id)
{
// if ( actionGoal.goal_id.id != actionResult.status.goal_id.id )
return;
}
latest_goal_status_ = actionResult.GoalStatus;
// latest_goal_status_ = actionResult.status;
latestResult = actionResult;
switch (state.state)
{
case CommState.StateEnum.WAITING_FOR_GOAL_ACK:
case CommState.StateEnum.PENDING:
case CommState.StateEnum.ACTIVE:
case CommState.StateEnum.WAITING_FOR_RESULT:
case CommState.StateEnum.WAITING_FOR_CANCEL_ACK:
case CommState.StateEnum.RECALLING:
case CommState.StateEnum.PREEMPTING:
{
// A little bit of hackery to call all the right state transitions before processing result
gsarray statusArray = new gsarray();
List <gstat> list = new List <gstat> (statusArray.status_list);
list.Add(actionResult.GoalStatus);
// list.Add ( actionResult.status );
statusArray.status_list = list.ToArray();
updateStatus(gh, statusArray);
transitionToState(gh, CommState.StateEnum.DONE);
break;
}
case CommState.StateEnum.DONE:
ROS.Error("actionlib", "Got a result when we were already in the DONE state");
break;
default:
ROS.Error("actionlib", "In a funny comm state: %u", state.state);
break;
}
}
public void cancelDisconnectCallback(SingleSubscriberPublisher pub)
{
lock ( lockObject )
{
if (cancelSubscribers.ContainsKey(pub.subscriber_name))
{
ROS.Debug("cancelDisconnectCallback: Removing [%s] from cancelSubscribers", pub.subscriber_name);
cancelSubscribers [pub.subscriber_name]--;
if (cancelSubscribers[pub.subscriber_name] == 0)
{
cancelSubscribers.Remove(pub.subscriber_name);
}
}
else
{
ROS.Warn("cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list", pub.subscriber_name);
}
}
ROS.Debug("%s", cancelSubscribersString());
}
public bool start (CommandHandle handle)
{
if ( handle == null || !handle.connected () )
return false;
string resource = handle.getName ();
if ( enabled_.ContainsKey ( resource ) )
{
if ( enabled_ [ resource ] == handle )
return true;
} else
{
enabled_.Add ( resource, handle );
ROS.Info ( "quadrotor_interface", "Enabled %s output", resource );
return true;
}
return false;
}
public async Task <TResult> SendGoalAsync(
TGoal goal,
Action <ClientGoalHandle <TGoal, TResult, TFeedback> > onTransistionCallback = null,
Action <ClientGoalHandle <TGoal, TResult, TFeedback>, FeedbackActionMessage <TFeedback> > onFeedbackCallback = null,
CancellationToken cancel = default(CancellationToken)
)
{
if (!await this.WaitForActionServerToStartAsync(this.ActionServerWaitTimeout, cancel).ConfigureAwait(false))
{
ROS.Info()($"Action server {this.Name} is not available.");
throw new TimeoutException($"Action server {this.Name} is not available.");
}
var gh = this.SendGoal(goal, onTransistionCallback, onFeedbackCallback);
using (cancel.Register(gh.Cancel))
{
return(await gh.GoalTask.ConfigureAwait(false));
}
}
private void WaitForLatchedMessage(TimeSpan timeOut)
{
var receivedMessage = false;
var s = ROS.GlobalNodeHandle.subscribe <Messages.std_msgs.String>("/PubSubTestbed", 10, (message) =>
{
ROS.Info()("--- Received message");
receivedMessage = true;
});
var startTime = DateTime.UtcNow;
while (!receivedMessage)
{
Thread.Sleep(1);
if (DateTime.UtcNow - startTime > timeOut)
{
throw new TimeoutException($"Now message retrieved withhin {timeOut}");
}
}
}
/**
* \brief Check if two goal handles point to the same goal
* \return TRUE if both point to the same goal. Also returns TRUE if both handles are inactive.
*/
public static bool operator ==(ClientGoalHandle <ActionSpec> lhs, ClientGoalHandle <ActionSpec> rhs)
{
if (object.ReferenceEquals(lhs, rhs))
{
return(true);
}
if (!(lhs.isActive && rhs.isActive))
{
return(false);
}
DestructionGuard.ScopedProtector protector = new DestructionGuard.ScopedProtector(lhs.guard);
if (!protector.isProtected())
{
ROS.Error("actionlib", "This action client associated with the goal handle has already been destructed. Ignoring this operator==() call");
return(false);
}
return(lhs.listHandle == rhs.listHandle);
}
void _listener_MessageReceived(ROS.Utils.Messenger.Client.Msg msg) {
DispatchSvc.Invoke(() => RosLogs.Insert(0, string.Format("[{0} {1}] {2}", DateTime.Now.ToString("HH:mm"), msg.from.ToUpper(), msg.msgText)));
}
