AREXPORT bool ArUrg::asyncConnect(void)
{
myStartConnect = true;
if (!getRunning())
runAsync();
return true;
}
int eThread::run(int prio, int policy)
{
if (runAsync(prio, policy))
return -1;
sync();
return 0;
}
TEST_F(BoomStickTest, SingleTargetSingleShooterAsync) {
BoomStick stick{mAddress};
MockSkelleton target{mAddress};
ASSERT_TRUE(target.Initialize());
ASSERT_TRUE(stick.Initialize());
target.BeginListenAndRepeat();
runAsync(stick, 100);
target.EndListendAndRepeat();
}
void Thread::startAsync()
{
int r = uv_thread_create(&_handle, [](void* arg) {
auto& ptr = *reinterpret_cast<Runner::Context::ptr*>(arg);
ptr->tid = 0;
ptr->exit = false;
do {
runAsync(ptr.get());
scy::sleep(1); // TODO: uv_thread_yield when available
} while (ptr->repeating && !ptr->cancelled());
ptr->running = false;
ptr->started = false;
delete &ptr;
}, new Runner::Context::ptr(pContext));
if (r < 0) throw std::runtime_error("System error: Cannot initialize thread");
}
void SyncContext::startAsync()
{
// assert(!_handle.active()); // active() can be unreliable when called inside thread
_handle.ptr()->data = new async::Runner::Context::ptr(pContext);
int r = uv_async_init(_handle.loop(), _handle.ptr<uv_async_t>(), [](uv_async_t* req) {
assert(req->data != nullptr); // catch late callbacks, may need to
// make uv handle a context member
auto ctx = reinterpret_cast<async::Runner::Context::ptr*>(req->data);
if (ctx->get()->cancelled()) {
delete ctx; // delete the context and free memory
req->data = nullptr;
return;
}
runAsync(ctx->get());
});
if (r < 0) _handle.setAndThrowError("Cannot initialize async", r);
}
AREXPORT bool ArUrg::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
AREXPORT bool ArUrg_2_0::blockingConnect(void)
{
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
// if we have a starting baud and are a serial port, then change the
// baud rate... not by default this will set it to 0 baud which'll
// cause the serial stuff not to touch it
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
ArUtil::sleep(100);
bool connected = false;
if (internalConnect())
connected = true;
if (connected)
{
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else
{
failedToConnect();
return false;
}
}
ArCentralManager::ArCentralManager(ArServerBase *robotServer,
ArServerBase *clientServer) :
myNetSwitchCB(this, &ArCentralManager::netServerSwitch),
myNetClientListCB(this, &ArCentralManager::netClientList),
myAriaExitCB(this, &ArCentralManager::close),
myProcessFileCB(this, &ArCentralManager::processFile),
myForwarderServerClientRemovedCB(
this, &ArCentralManager::forwarderServerClientRemovedCallback),
myMainServerClientRemovedCB(
this, &ArCentralManager::mainServerClientRemovedCallback)
{
myMutex.setLogName("ArCentralManager::myCallbackMutex");
myDataMutex.setLogName("ArCentralManager::myDataMutex");
setThreadName("ArCentralManager");
myRobotServer = robotServer;
myClientServer = clientServer;
myAriaExitCB.setName("ArCentralManager");
Aria::addExitCallback(&myAriaExitCB, 25);
myEnforceType = ArServerCommands::TYPE_UNSPECIFIED;
myClientBackupTimeout = 2;
Aria::getConfig()->addParam(
ArConfigArg("CentralServerToClientTimeoutInMins",
&myClientBackupTimeout,
"The amount of time the central server can go without sending a packet to the robot successfully (when there are packets to send). A number less than 0 means this won't happen. The time is in minutes but takes doubles (ie .5) (5 seconds is used if the value is positive, but less than that amount)", -1),
"Connection timeouts", ArPriority::DETAILED);
myRobotBackupTimeout = 2;
Aria::getConfig()->addParam(
ArConfigArg("CentralServerToRobotTimeoutInMins",
&myRobotBackupTimeout,
"The amount of time the central server can go without sending a packet to the robot successfully (when there are packets to send). A number less than 0 means this won't happen. The time is in minutes but takes doubles (ie .5) (5 seconds is used if the value is positive, but less than that amount)", -1),
"Connection timeouts", ArPriority::DETAILED);
myHeartbeatTimeout = 2;
Aria::getConfig()->addParam(
ArConfigArg("CentralServerFromRobotTimeoutInMins",
&myHeartbeatTimeout,
"The amount of time the central server can go without hearing a robot's heartbeat without disconnecting it. A number less than 0 means that the robots will never timeout. The time is in minutes but takes doubles (ie .5) (5 seconds is used if the value is positive, but less than that amount)", -1),
"Connection timeouts", ArPriority::DETAILED);
myUdpHeartbeatTimeout = 2;
Aria::getConfig()->addParam(
ArConfigArg("CentralServerFromRobotUdpTimeoutInMins",
&myUdpHeartbeatTimeout,
"The amount of time the central server can go without hearing a robot's udp heartbeat without disconnecting it (this fails it over to tcp only). A number less than 0 means that the robots will never timeout. The time is in minutes but takes doubles (ie .5) (5 seconds is used if the value is positive, but less than that amount)", -1),
"Connection timeouts", ArPriority::DETAILED);
myProcessFileCB.setName("ArCentralManager");
Aria::getConfig()->addProcessFileCB(&myProcessFileCB, -999);
myRobotServer->addData("switch", "switches the direction of the connection, after this is requested it sends an empty packet denoting acceptance of the switch, then switches this to a client connection",
&myNetSwitchCB, "string: robotName", "empty packet", "RobotInfo",
"RETURN_SINGLE");
myRobotServer->addData("centralServerHeartbeat", "Just a data to let the robot's know that this server has the centralServerHeartbeat feature (nothing is actually done with this command)",
NULL, "none", "none", "RobotInfo",
"RETURN_NONE");
myClientServer->addData("clientList", "Lists the clients that are connected",
&myNetClientListCB, "none",
"ubyte2: numClients; repeating for <numClients> [string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address]",
"RobotInfo", "RETURN_SINGLE");
myClientServer->addData("clientAdded", "Broadcast when a client is added",
NULL, "none",
"string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address",
"RobotInfo", "RETURN_SINGLE");
myClientServer->addData("clientRemoved", "Broadcast when a client is removed",
NULL, "none",
"string: hostname (empty means this host); ubyte2: port; string: robot name; string: flags; string: robot ip address",
"RobotInfo", "RETURN_SINGLE");
myClientServer = clientServer;
myClosingConnectionID = 0;
myMostForwarders = 0;
myMostClients = 0;
myForwarderServerClientRemovedCB.setName("ArCentralManager");
myMainServerClientRemovedCB.setName("ArCentralManager");
myClientServer->addClientRemovedCallback(&myMainServerClientRemovedCB);
runAsync();
}
AREXPORT bool ArSZSeries::blockingConnect(void) {
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL) {
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN
&& !myConn->openSimple()) {
ArLog::log(
ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
AREXPORT bool ArLMS1XX::blockingConnect(void)
{
char buf[1024];
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it", getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
int size = (270 / .25 + 1);
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
timeDone.addMSec(60 * 1000);
else
timeDone.addMSec(30 * 1000);
ArLMS1XXPacket *packet;
ArLMS1XXPacket sendPacket;
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("SetAccessMode");
sendPacket.uByteToBuf(0x3); // level
sendPacket.strToBuf("F4724744"); // hashed password
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "SetAccessMode")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Changed access mode (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not change access mode (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("mLMPsetscancfg");
sendPacket.byte4ToBuf(5000); // scanning freq
sendPacket.byte2ToBuf(1); // number segments
sendPacket.byte4ToBuf(getIncrementChoiceDouble() * 10000); // angle resolution
//sendPacket.byte4ToBuf((getStartDegrees() + 90) * 10000); // starting angle
sendPacket.byte4ToBuf(-45 * 10000); // can't change starting angle
//sendPacket.byte4ToBuf((getEndDegrees() + 90) * 10000); // ending angle
sendPacket.byte4ToBuf(225 * 10000); // can't change ending angle
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: setscancfg: %s", getName(),
sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "mLMPsetscancfg")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 0)
{
ArLog::log(myLogLevel, "%s: setscancfg succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Setscancfg failed (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sWN");
sendPacket.strToBuf("LMDscandatacfg");
sendPacket.uByte2ToBuf(0x1); // output channel
sendPacket.uByteToBuf(0x0); // remission
sendPacket.uByteToBuf(0x0); // remission resolution
sendPacket.uByteToBuf(0x0); // unit
sendPacket.uByte2ToBuf(0x0); // encoder
sendPacket.uByteToBuf(0x0); // position
sendPacket.uByteToBuf(0x0); // device name
sendPacket.uByteToBuf(0x0); // comment
sendPacket.uByteToBuf(0x0); // time
sendPacket.byteToBuf(5); // which scan
//sendPacket.byteToBuf(1); // which scan
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: scandatacfg: %s", getName(), sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandatacfg")) != NULL)
{
ArLog::log(myLogLevel, "%s: scandatacfg succeeded", getName());
delete packet;
packet = NULL;
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("Run");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "Run")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Run succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not run (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
/* when asking one at a time
sendPacket.empty();
sendPacket.strToBuf("sRN");
sendPacket.strToBuf("LMDscandata");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandata")) != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s scan data %d", getName(),
packet->getCommandType(), packet->getLength());
myPacketsMutex.lock();
myPackets.push_back(packet);
myPacketsMutex.unlock();
sensorInterp();
ArLog::log(myLogLevel, "%s: Processed scan data", getName());
}
else
{
failedToConnect();
return false;
}
*/
sendPacket.empty();
sendPacket.strToBuf("sEN");
sendPacket.strToBuf("LMDscandata");
sendPacket.uByteToBuf(1);
sendPacket.finalizePacket();
//printf("(%s)\n", sendPacket.getBuf());
// just ask for continuous data
if (!myConn->write(sendPacket.getBuf(), sendPacket.getLength()))
{
ArLog::log(ArLog::Terse,
"%s: Could not send %s to laser", getName(), "LMDscandata");
failedToConnect();
return false;
}
while (timeDone.mSecTo())
{
packet = myReceiver.receivePacket(1000);
if (packet != NULL &&
strcasecmp(packet->getCommandType(), "sSN") == 0 &&
strcasecmp(packet->getCommandName(), "LMDscandata") == 0)
{
delete packet;
packet = NULL;
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else if (packet != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s %s (%d long)", getName(),
packet->getCommandType(), packet->getCommandName(),
packet->getLength());
delete packet;
packet = NULL;
}
}
ArLog::log(ArLog::Terse,
"%s: Did not get scandata back from laser", getName());
failedToConnect();
return false;
}
