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);
}
// ********* GoalStatus Connections *********
public void processStatus(gsa status, string curStatusCallerID)
{
lock ( lockObject )
{
if (statusReceived)
{
if (statusCallerID != curStatusCallerID)
{
ROS.Warn("processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?",
statusCallerID, curStatusCallerID);
statusCallerID = curStatusCallerID;
}
latestStatusTime = status.header.Stamp;
}
else
{
ROS.Debug("processStatus: Just got our first status message from the ActionServer at node [%s]", curStatusCallerID);
statusReceived = true;
statusCallerID = curStatusCallerID;
latestStatusTime = status.header.Stamp;
}
}
// check_connection_condition_.notify_all();
}
