void ZoneGraph::updateConnections()
{
bool drawFuncRels = mDrawOptions.mDrawFunctionRelations;
ReverseIndexLookup indexLookup(getNodes());
mConnections.clear();
for(size_t nodeIndex=0; nodeIndex<getNodes().size(); nodeIndex++)
{
const ModelClassifier *classifier = ModelClassifier::getClass(
getNodes()[nodeIndex].mType);
if(classifier)
{
// Get attributes of classifier, and get the decl type
for(const auto &attr : classifier->getAttributes())
{
mConnections.insertConnection(nodeIndex,
ModelClassifier::getClass(attr->getDeclType()),
indexLookup, ZDD_FirstIsClient);
}
if(drawFuncRels)
{
ConstModelClassifierVector relatedClasses;
mModel->getRelatedFuncInterfaceClasses(*classifier, relatedClasses);
for(auto const &item : relatedClasses)
{
mConnections.insertConnection(nodeIndex, item,
indexLookup, ZDD_FirstIsClient);
}
ConstModelDeclClasses relatedDeclClasses;
mModel->getRelatedBodyVarClasses(*classifier, relatedDeclClasses);
for(auto const &item : relatedDeclClasses)
{
mConnections.insertConnection(nodeIndex, item.getClass(),
indexLookup, ZDD_FirstIsClient);
}
}
// Go through associations, and get related classes.
for(const auto &assoc : mModel->mAssociations)
{
size_t n1Index = NO_INDEX;
size_t n2Index = NO_INDEX;
if(assoc->getChild() == classifier)
{
n1Index = indexLookup.getClassIndex(assoc->getParent());
n2Index = indexLookup.getClassIndex(classifier);
}
else if(assoc->getParent() == classifier)
{
n2Index = indexLookup.getClassIndex(assoc->getChild());
n1Index = indexLookup.getClassIndex(classifier);
}
if(n1Index != NO_INDEX && n2Index != NO_INDEX)
{
mConnections.insertConnection(n1Index, n2Index, ZDD_SecondIsClient);
}
}
}
}
}
/**
* Set the reference of a joint to a position in radians
*
* @param joint The joint to set
* @param rad The position in radians
* @param mode The mode of joint operation
*/
void ReferenceChannel::setReference(string &joint, double rad, Mode mode){
if (errored) return;
int index = indexLookup(joint);
if (index != -1){
currentReference.ref[index] = rad;
currentReference.mode[index] = 1;
}
}
/**
* Reset a joint
* @param joint The joint to reset
* @return True on successs
*/
bool CommandChannel::reset(string &joint) {
BoardCommand command;
memset(&command, 0, sizeof(command));
int jointNum = 0;
if ((jointNum = indexLookup(joint)) == -1) {
return false;
} else {
command.type = D_ZERO_ENCODER;
command.joint = jointNum;
}
int r = ach_put(&huboBoardCommandChannel, &command, sizeof(command));
if (ACH_OK != r) {
cerr << "Error! Command reset failed with state " << r << endl;
return false;
}
return true;
}
/**
* Home a joint
* @param joint The joint to home
* @return True on success
*/
bool CommandChannel::home(string joint) {
BoardCommand command;
memset(&command, 0, sizeof(command));
int jointNum = 0;
if (strcmp(joint.c_str(), "all") == 0) {
command.type = D_GOTO_HOME_ALL;
} else if ((jointNum = indexLookup(joint)) == -1) {
return false;
} else {
command.type = D_GOTO_HOME;
command.joint = jointNum;
}
int r = ach_put(&huboBoardCommandChannel, &command, sizeof(command));
if (ACH_OK != r) {
cerr << "Error! Command home failed with state " << r << endl;
return false;
}
return true;
}
/**
* Enable a joint
* @param joint The joint to enable
* @return True on success
*/
bool CommandChannel::enable(string joint) {
BoardCommand command;
memset(&command, 0, sizeof(command));
int jointNum = 0;
if (strcmp(joint.c_str(), "all") == 0) {
command.type = D_CTRL_ON_OFF_ALL;
command.param[0] = D_ENABLE;
} else if ((jointNum = indexLookup(joint)) == -1) {
return false;
} else {
command.type = D_CTRL_ON_OFF;
command.joint = jointNum;
command.param[0] = D_ENABLE;
}
int r = ach_put(&huboBoardCommandChannel, &command, sizeof(command));
if (ACH_OK != r) {
cerr << "Error! Command enable failed with state " << r << endl;
return false;
}
return true;
}
