void Simulator::receiveUpdate()
{
// Update connection timer and status
simTimer->setInterval(simTimeout);
simTimer->stop();
simTimer->start();
if ( !simConnectionStatus )
{
simConnectionStatus = true;
emit simulatorConnected();
}
// Process data
while(inSocket->hasPendingDatagrams()) {
// Receive datagram
QByteArray datagram;
datagram.resize(inSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
inSocket->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
//QString datastr(datagram);
// Process incomming data
processUpdate(datagram);
}
}
bool
aBackup::zipArchive(const QString& archName, const QString& dir)
{
#ifndef _Windows
Q3Process processUpdate( QString("zip") );
processUpdate.setWorkingDirectory(dir);
processUpdate.addArgument( "-r" ); // recurce into subdirectories
processUpdate.addArgument( "-0" ); // store only
processUpdate.addArgument( archName ); // backup name
processUpdate.addArgument(".");
#else
Q3Process processUpdate( QString("7z") );
processUpdate.setWorkingDirectory(dir);
processUpdate.addArgument( "a" );
processUpdate.addArgument( "-tzip" );
processUpdate.addArgument( archName );
processUpdate.addArgument( "-r" );
processUpdate.addArgument(".");
#endif
if( !processUpdate.start() )
{
setLastError(tr("Unable to start zip"));
aLog::print(aLog::Error, tr("aBackup zip start error"));
return true;
}
while( processUpdate.isRunning() );
if( !processUpdate.normalExit() )
{
setLastError(tr("Zip ended with error"));
aLog::print(aLog::Error, tr("aBackup zip dead"));
return true;
}
if( processUpdate.exitStatus() )
{
setLastError(tr("Zip ended with code %1").arg(processUpdate.exitStatus()));
return true;
}
return false;
}
void mog::network::NetworkManager::update(float dt)
{
elapsedTime += dt;
if (elapsedTime >= getFrameLenght())
{
processUpdate();
elapsedTime = 0;
}
}
/**
* FUNCTION NAME: checkMessages
*
* DESCRIPTION: This function is the message handler of this node.
* This function does the following:
* 1) Pops messages from the queue
* 2) Handles the messages according to message types
*/
void MP2Node::checkMessages() {
/*
* Implement this. Parts of it are already implemented
*/
char * data;
int size;
/*
* Declare your local variables here
*/
// dequeue all messages and handle them
while ( !memberNode->mp2q.empty() ) {
/*
* Pop a message from the queue
*/
data = (char *)memberNode->mp2q.front().elt;
size = memberNode->mp2q.front().size;
memberNode->mp2q.pop();
string message(data, data + size);
/*
* Handle the message types here
*/
cout << "Got message = " << message << endl;
Message incoming_msg(message); //convert the message in string form to Message type
//check for message type and call their handlers accordingly.
switch(incoming_msg.type) {
case CREATE : {processCreate(incoming_msg); break;}
case READ : {processRead(incoming_msg); break;}
case REPLY : {processNodesReply(incoming_msg); break;}
case READREPLY : {processReadReply(incoming_msg);break;}
case DELETE : {processDelete(incoming_msg); break;}
case UPDATE : {processUpdate(incoming_msg); break;}
default : {}
}
}
// At the end check the acks or nacks that this node may have received from the reply messages from the nodes to
// whom this node may sent the client CRUD message
checkReplyMessages();
/*
* This function should also ensure all READ and UPDATE operation
* get QUORUM replies
*/
}
void CoaxVisionControl::StateCallback(const coax_msgs::CoaxState::ConstPtr & msg) {
static int initTime = 200;
static int initCounter = 0;
static Eigen::Vector3f init_acc(0,0,0);
static Eigen::Vector3f init_gyr(0,0,0);
battery_voltage = msg->battery;
coax_nav_mode = msg->mode.navigation;
// rpy << msg->roll, msg->pitch, msg->yaw;
// std::cout << "RPY: \n" << rpy << std::endl;
accel << msg->accel[0], msg->accel[1], msg->accel[2];
gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
rpyt_rc << msg->rcChannel[4], msg->rcChannel[6], msg->rcChannel[2], msg->rcChannel[0];
rpyt_rc_trim << msg->rcChannel[5], msg->rcChannel[7], msg->rcChannel[3], msg->rcChannel[1];
range_al = msg->zfiltered;
global_z = msg->zfiltered;
if (FIRST_STATE) {
last_state_time = ros::Time::now().toSec();
FIRST_STATE = false;
ROS_INFO("First Time Stamp: %f",last_state_time);
return;
}
if ((battery_voltage < 10.50) && !LOW_POWER_DETECTED) {
ROS_INFO("Battery Low!!! (%fV) Landing initialized",battery_voltage);
LOW_POWER_DETECTED = true;
}
if (initCounter < initTime) {
init_acc += accel;
init_gyr += gyro;
initCounter++;
}
else if (initCounter == initTime) {
init_acc /= initTime;
gravity = init_acc.norm();
setGravity(gravity);
ROS_INFO("IMU Calibration Done! Gravity: %f", gravity);
initCounter++;
setInit(msg->header.stamp);
}
else {
processUpdate(accel, gyro, msg->header.stamp);
measureUpdate(range_al);
}
return;
}
void BodyModel::update(Odometry *odometry,
const SensorValues &sensors) {
// Various Center of Pressure calcs to help stabilise walk (ZMP)
stepCounter++;
// Update maximum sensed foot-sensor readings
float temp = sensors.sensors[RSSensors::LFoot_FSR_FrontLeft]; if(fsLfl<temp and fsLfl<5.0) fsLfl = temp;
temp = sensors.sensors[RSSensors::LFoot_FSR_FrontLeft]; if(fsLfl<temp and fsLfl<5.0) fsLfl = temp;
temp = sensors.sensors[RSSensors::LFoot_FSR_FrontRight]; if(fsLfr<temp and fsLfr<5.0) fsLfr = temp;
temp = sensors.sensors[RSSensors::LFoot_FSR_RearLeft]; if(fsLrl<temp and fsLrl<5.0) fsLrl = temp;
temp = sensors.sensors[RSSensors::LFoot_FSR_RearRight]; if(fsLrr<temp and fsLrr<5.0) fsLrr = temp;
temp = sensors.sensors[RSSensors::RFoot_FSR_FrontLeft]; if(fsRfl<temp and fsRfl<5.0) fsRfl = temp;
temp = sensors.sensors[RSSensors::RFoot_FSR_FrontRight]; if(fsRfr<temp and fsRfr<5.0) fsRfr = temp;
temp = sensors.sensors[RSSensors::RFoot_FSR_RearLeft]; if(fsRrl<temp and fsRrl<5.0) fsRrl = temp;
temp = sensors.sensors[RSSensors::RFoot_FSR_RearRight]; if(fsRrr<temp and fsRrr<5.0) fsRrr = temp;
lastZMPL = ZMPL;
ZMPL = 0;
// Calculate ZMPL (left-right) used to eg switch support foot in Walk2014
float pressureL =
+sensors.sensors[RSSensors::LFoot_FSR_FrontLeft]/fsLfl
+ sensors.sensors[RSSensors::LFoot_FSR_FrontRight]/fsLfr
+ sensors.sensors[RSSensors::LFoot_FSR_RearLeft]/fsLrl
+ sensors.sensors[RSSensors::LFoot_FSR_RearRight]/fsLrr;
float pressureR =
+sensors.sensors[RSSensors::RFoot_FSR_FrontLeft]/fsRfl
+ sensors.sensors[RSSensors::RFoot_FSR_FrontRight]/fsRfr
+ sensors.sensors[RSSensors::RFoot_FSR_RearLeft]/fsRrl
+ sensors.sensors[RSSensors::RFoot_FSR_RearRight]/fsRrr;
float totalPressure = pressureL + pressureR;
if (ABS(totalPressure) > 0.000001f) {
ZMPL =
( .080 * sensors.sensors[RSSensors::LFoot_FSR_FrontLeft]/fsLfl
+ .030 * sensors.sensors[RSSensors::LFoot_FSR_FrontRight]/fsLfr
+ .080 * sensors.sensors[RSSensors::LFoot_FSR_RearLeft]/fsLrl
+ .030 * sensors.sensors[RSSensors::LFoot_FSR_RearRight]/fsLrr
- .030 * sensors.sensors[RSSensors::RFoot_FSR_FrontLeft]/fsRfl
- .080 * sensors.sensors[RSSensors::RFoot_FSR_FrontRight]/fsRfr
- .030 * sensors.sensors[RSSensors::RFoot_FSR_RearLeft]/fsRrl
- .080 * sensors.sensors[RSSensors::RFoot_FSR_RearRight]/fsRrr) / totalPressure;
}
isOnFront = isOnFrontOfFoot(sensors);
processUpdate(odometry, sensors);
observationUpdate(odometry, sensors);
}
VoiceCall::VoiceCall(VoiceCallUpdaterIF* updater) : m_updater(updater)
{
// connect the signal to get update information
connect(m_updater, SIGNAL(update()), this, SLOT(processUpdate()));
}
void CTestDispatcher::testProcessUpdate(const TLUpdate &update)
{
return processUpdate(update);
}
bool CmdBreak::onClient(DebuggerClient *client) {
if (DebuggerCommand::onClient(client)) return true;
bool regex = false;
BreakPointInfo::State state = BreakPointInfo::Always;
int index = 1;
if (client->arg(1, "regex")) {
regex = true;
index++;
} else if (client->arg(1, "once")) {
state = BreakPointInfo::Once;
index++;
} else if (client->arg(1, "list")) {
return processList(client);
} else if (hasUpdateArg(client)) {
return processUpdate(client);
}
string currentFile;
int currentLine = 0;
BreakPointInfoPtr loc = client->getCurrentLocation();
if (loc) {
currentFile = loc->m_file;
currentLine = loc->m_line1;
}
BreakPointInfoPtr bpi;
InterruptType interrupt = (InterruptType)(-1);
if (client->argCount() == 0) {
if (currentFile.empty() || currentLine == 0) {
client->error("There is no current file or line to set breakpoint. "
"You will have to specify source file location yourself.");
return true;
}
bpi = BreakPointInfoPtr(new BreakPointInfo(regex, state,
currentFile.c_str(),
currentLine));
} else if (client->arg(index, "start")) {
interrupt = RequestStarted;
} else if (client->arg(index, "end")) {
interrupt = RequestEnded;
} else if (client->arg(index, "psp")) {
interrupt = PSPEnded;
} else {
bpi = BreakPointInfoPtr(new BreakPointInfo(regex, state, BreakPointReached,
client->argValue(index),
currentFile));
}
if (interrupt >= 0) {
string url;
if (!client->arg(index + 1, "if") && !client->arg(index + 1, "&&")) {
url = client->argValue(++index);
}
bpi = BreakPointInfoPtr(new BreakPointInfo(regex, state, interrupt, url));
}
if (!validate(client, bpi, index)) {
client->tutorial(
"This is the order of different arguments:\n"
"\n"
"\t[b]reak [o]nce {exp} if|&& {php}\n"
"\n"
"These are the components in a breakpoint {exp}:\n"
"\n"
"\tfile location: {file}:{line}\n"
"\tfunction call: {func}()\n"
"\tmethod invoke: {cls}::{method}()\n"
);
}
return true;
}
