AREXPORT void ArActionRobotJoydrive::setRobot(ArRobot *robot)
{
ArAction::setRobot(robot);
if (myRobot != NULL)
{
myRobot->addConnectCB(&myConnectCB);
myRobot->addPacketHandler(&myHandleJoystickPacketCB);
if (robot->isConnected())
connectCallback();
}
}
static void workCallback(asynUser *pasynUser)
{
cmdInfo *pcmdInfo = (cmdInfo *)pasynUser->userPvt;
threadInfo *pthreadInfo = pcmdInfo->pthreadInfo;
char *threadName = pthreadInfo->threadName;
switch(pcmdInfo->test) {
case connect: connectCallback(pasynUser); break;
case testBlock: blockCallback(pasynUser); break;
case testCancelRequest: cancelCallback(pasynUser); break;
default:
fprintf(pcmdInfo->file,"%s workCallback illegal test %d\n",threadName,pcmdInfo->test);
}
}
bool detectFiducialsServiceCallback(cob_object_detection_msgs::DetectObjects::Request &req,
cob_object_detection_msgs::DetectObjects::Response &res)
{
ROS_DEBUG("[fiducials] Service Callback");
// Connect to image topics
bool result = false;
synchronizer_received_ = false;
connectCallback();
// Wait for data
{
boost::mutex::scoped_lock lock( mutexQ_);
boost::system_time const timeout=boost::get_system_time()+ boost::posix_time::milliseconds(5000);
ROS_INFO("[fiducials] Waiting for image data");
if (condQ_.timed_wait(lock, timeout))
ROS_INFO("[fiducials] Waiting for image data [OK]");
else
{
ROS_WARN("[fiducials] Could not receive image data from ApproximateTime synchronizer");
return false;
}
// Wait for data (at least 5 seconds)
//int nSecPassed = 0;
//float nSecIncrement = 0.5;
//while (!synchronizer_received_ && nSecPassed < 10)
//{
// ros::Duration(nSecIncrement).sleep();
// nSecPassed += nSecIncrement;
// ROS_INFO("[fiducials] Waiting");
//}
//if (!synchronizer_received_)
//{
// ROS_WARN("[fiducials] Could not receive image data");
// return false;
//}
result = detectFiducials(res.object_list, color_mat_8U3_);
}
disconnectCallback();
return result;
}
AREXPORT ArRobotJoyHandler::ArRobotJoyHandler(ArRobot *robot) :
myHandleJoystickPacketCB(this, &ArRobotJoyHandler::handleJoystickPacket),
myConnectCB(this, &ArRobotJoyHandler::connectCallback)
{
myRobot = robot;
myHandleJoystickPacketCB.setName("ArRobotJoyHandler");
myRobot->addConnectCB(&myConnectCB);
myRobot->addPacketHandler(&myHandleJoystickPacketCB, ArListPos::FIRST);
if (myRobot->isConnected())
connectCallback();
myStarted.setToNow();
myButton1 = 0;
myButton2 = 0;
myJoyX = 0;
myJoyY = 0;
myThrottle = 1;
myGotData = false;
}
AREXPORT ArRobotConfig::ArRobotConfig(ArRobot *robot) :
myConnectCB(this, &ArRobotConfig::connectCallback),
myProcessFileCB(this, &ArRobotConfig::processFile)
{
myRobot = robot;
myAddedMovementParams = false;
myTransVelMax = 0;
myTransAccel = 0;
myTransDecel = 0;
myRotVelMax = 0;
myRotAccel= 0;
myRotDecel = 0;
mySavedOriginalMovementParameters = false;
myOriginalTransVelMax = 0;
myOriginalTransAccel = 0;
myOriginalTransDecel = 0;
myOriginalRotVelMax = 0;
myOriginalRotAccel= 0;
myOriginalRotDecel = 0;
myAnalogGyro = NULL;
myUseGyro = true;
myAddedGyro = false;
myConnectCB.setName("ArRobotConfig");
myProcessFileCB.setName("ArRobotConfig");
myRobot->addConnectCB(&myConnectCB, ArListPos::FIRST);
Aria::getConfig()->addProcessFileCB(&myProcessFileCB, 98);
if (myRobot->isConnected())
connectCallback();
}
AREXPORT void ArDataLogger::addToConfig(ArConfig *config)
{
if (config == NULL || myAddedToConfig)
return;
myConfig = config;
if (!myRobot->isConnected())
{
myAddToConfigAtConnect = true;
myRobot->addConnectCB(&myConnectCB);
return;
}
else
{
connectCallback();
}
myAddedToConfig = true;
ArLog::log(ArLog::Verbose, "ArDataLogger::addToConfig");
std::string section;
char name[512];
char desc[512];
int i;
section = "Data logging";
// add everything to the config
myConfig->addParam(
ArConfigArg("DataLog", &myConfigLogging, "True to log data, false not to"),
section.c_str(), ArPriority::NORMAL);
myConfig->addParam(
ArConfigArg("DataLogInterval", &myConfigLogInterval, "Seconds between logs", 0),
section.c_str(), ArPriority::NORMAL);
if (myPermanentFileName.size() == 0)
myConfig->addParam(
ArConfigArg("DataLogFileName", myConfigFileName,
"File to log data into", sizeof(myConfigFileName)),
section.c_str(), ArPriority::NORMAL);
for (i = 0; i < myStringsCount; i++)
{
snprintf(name, sizeof(name), "DataLog%s", myStrings[i]->getName());
snprintf(desc, sizeof(desc), "Logs %s", myStrings[i]->getName());
myConfig->addParam(
ArConfigArg(name, myStringsEnabled[i], desc),
"Custom data logging", ArPriority::NORMAL);
}
myConfig->addParam(
ArConfigArg("DataLogBatteryVoltage", &myLogVoltage, "True to log battery voltage"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogChargeState", &myLogChargeState,
"True to log charge state"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogPose", &myLogPose, "True to log robot's pose"),
section.c_str(), ArPriority::NORMAL);
myConfig->addParam(
ArConfigArg("DataLogEncoderPose", &myLogEncoderPose, "True to log robot's raw encoder pose"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogCorrectedEncoderPose", &myLogCorrectedEncoderPose, "True to log robot's corrected (by gyro, etc) encoder pose"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogEncoders", &myLogEncoders, "True to log the raw encoder readings"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogLeftVel", &myLogLeftVel, "True to log left wheel velocity"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogRightVel", &myLogRightVel, "True to log right wheel velocity"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogTransVel", &myLogTransVel, "True to log translational wheel velocity"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogRotVel", &myLogRotVel, "True to log rotational wheel velocity"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogLeftStalled", &myLogLeftStalled, "True to log if the left wheel is stalled"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogRightStalled", &myLogRightStalled, "True to log if the right wheel is stalled"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogStallBits", &myLogStallBits, "True to log all the stall bits is stalled"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogFlags", &myLogFlags, "True to log all the flags"),
section.c_str(), ArPriority::DETAILED);
myConfig->addParam(
ArConfigArg("DataLogFaultFlags", &myLogFaultFlags, "True to log all the fault flags"),
section.c_str(), ArPriority::DETAILED);
for (i = 0; i < myAnalogCount; i++)
{
snprintf(name, sizeof(name), "DataLogAnalog%d", i);
snprintf(desc, sizeof(desc),
"Logs the value of analog %d as a 10 bit (0-1024) value",
i);
myConfig->addParam(
ArConfigArg(name, &myAnalogEnabled[i], desc),
section.c_str(), ArPriority::DETAILED);
}
for (i = 0; i < myAnalogVoltageCount; i++)
{
snprintf(name, sizeof(name), "DataLogAnalogVoltage%d", i);
snprintf(desc, sizeof(desc),
"Logs the value of analog %d as voltage from 0 to 5",
i);
myConfig->addParam(
ArConfigArg(name, &myAnalogVoltageEnabled[i], desc),
section.c_str(), ArPriority::DETAILED);
}
for (i = 0; i < myDigInCount; i++)
{
snprintf(name, sizeof(name), "DataLogDigIn%d", i);
snprintf(desc, sizeof(desc), "Logs digital in %d", i);
myConfig->addParam(
ArConfigArg(name, &myDigInEnabled[i], desc),
section.c_str(), ArPriority::DETAILED);
}
for (i = 0; i < myDigOutCount; i++)
{
snprintf(name, sizeof(name), "DataLogDigOut%d", i);
snprintf(desc, sizeof(desc), "Logs digital out %d", i);
myConfig->addParam(
ArConfigArg(name, &myDigOutEnabled[i], desc),
section.c_str(), ArPriority::DETAILED);
}
myProcessFileCB.setName("ArDataLogger");
myConfig->addProcessFileWithErrorCB(&myProcessFileCB, 100);
}
AREXPORT void ArRobotConfig::addAnalogGyro(ArAnalogGyro *gyro)
{
myAnalogGyro = gyro;
if (myRobot->isConnected())
connectCallback();
}