C++ NodeData类代码示例

std::shared_ptr<NodeData> EnvirePhysics::getInertialNode(const smurf::Inertial& inertial, const envire::core::FrameId& frame)
{
  NodeData * result = new NodeData;
  std::shared_ptr<NodeData> resultPtr(result);
  urdf::Inertial inertialUrdf = inertial.getUrdfInertial();
  result->init(inertial.getName());
  result->initPrimitive(mars::interfaces::NODE_TYPE_SPHERE, mars::utils::Vector(0.1, 0.1, 0.1), inertialUrdf.mass);
  result->groupID = inertial.getGroupId();
  result->movable = true;
  result->inertia[0][0] = inertialUrdf.ixx;
  result->inertia[0][1] = inertialUrdf.ixy;
  result->inertia[0][2] = inertialUrdf.ixz;
  result->inertia[1][0] = inertialUrdf.ixy;
  result->inertia[1][1] = inertialUrdf.iyy;
  result->inertia[1][2] = inertialUrdf.iyz;
  result->inertia[2][0] = inertialUrdf.ixz;
  result->inertia[2][1] = inertialUrdf.iyz;
  result->inertia[2][2] = inertialUrdf.izz;
  result->inertia_set = true;
  result->c_params.coll_bitmask = 0;
#ifdef DEBUG
  LOG_DEBUG("[EnvirePhysics::getInertialNode] Inertial object's mass: %f", result->mass);
#endif
  result->density = 0.0;
  setPos(frame, resultPtr);
  return resultPtr;
}

void MCTreeData::add(NodeSite *node) {
    node->initSum();
    if (_root->isFull()) {
        NodeData *newRoot = new NodeData(_width, _root->level() + 1);
        newRoot->add(_root);
        _root = newRoot;
    }
    _root->add(node);
}

bool NodeToVnfmEvent::
unknown(NodeData & a_rNodeData, DashBoard & a_board)
{
    gLog->WARNING("unknown");
    gLog->WARNING("%-24s| unknown - [%d] [%s]",
        "NodeToVnfmEvent",
        a_rNodeData.GetCommand(),
        a_rNodeData.GetBody().c_str());
    return true;
}

NodeData testConstructors() {
    NodeData A;
    assert(A.getToken() == "");
    assert(A.getCount() == 1);
    NodeData B("hello world");
    assert(B.getToken() == "hello world");
    NodeData C(B);
    assert(C.getToken() == "hello world");
    return B;
}

//-----------------------------------------------------------------------------
void IoHandler::handleData(QByteArray *pPacket)
{
    NodeData *pData = new NodeData;
	pData->UpdateData(pPacket);
	pData->timeStamp= QTime::currentTime();

    pNodeDb->addData(pData);

    emit dataReceived();
}

void OutputNodeData(NodeData & ndata)
{
	NodeData::iterator nit;
	int count(0);

	for(nit= ndata.begin(); nit!= ndata.end(); ++nit)
	{
		cout<<"         \""<<nit->first.c_str()<<"\" = \""<<nit->second.c_str()<<"\""<<endl;
	}

	cout<<"\n  NodeData size: "<<ndata.size()<<"\n\n\n\n"<<endl;
}

S_UINT GetNodeDataRawDataSize(NodeData & ndata)
{
	S_UINT len=0,tlen=sizeof(S_UINT);
	len+=tlen + tlen;	
	NodeData::iterator it;
	for(it=ndata.begin();it!=ndata.end();it++)
	{
		len+=(S_UINT)strlen(it->first.c_str())+1;
		len+=(S_UINT)strlen(it->second.c_str())+1;
	}

	return len;
}

std::shared_ptr<NodeData> EnvirePhysics::getCollidableNode(const smurf::Collidable& collidable, const envire::core::FrameId& frame) {
  NodeData * node = new NodeData;
  std::shared_ptr<NodeData> nodePtr(node);
  urdf::Collision collision = collidable.getCollision();
  node->init(collision.name);
  node->fromGeometry(collision.geometry);
  node->density = 0.0;
  node->mass = 0.00001;
  setPos(frame, nodePtr);
  node->movable = true;
  node->c_params.fromSmurfCP(collidable.getContactParams());
  node->groupID = collidable.getGroupId();
  return nodePtr;
}

 void EnvireSmurfLoader::addFloor(const vertex_descriptor &center)
 {
     NodeData data;
     data.init("floorData", Vector(0,0,0));
     data.initPrimitive(interfaces::NODE_TYPE_BOX, Vector(25, 25, 0.1), 0.0001);
     data.movable = false;
     mars::sim::PhysicsConfigMapItem::Ptr item(new mars::sim::PhysicsConfigMapItem);
     data.material.transparency = 0.5;
     //data.material.ambientFront = mars::utils::Color(0.0, 1.0, 0.0, 1.0);
     // TODO Fix the material data is lost in the conversion from/to configmap
     data.material.emissionFront = mars::utils::Color(1.0, 1.0, 1.0, 1.0);
     LOG_DEBUG("Color of the Item in the addFloor: %f , %f, %f, %f", data.material.emissionFront.a , data.material.emissionFront.b, data.material.emissionFront.g, data.material.emissionFront.r );
     data.toConfigMap(&(item.get()->getData()));
     control->graph->addItemToFrame(control->graph->getFrameId(center), item);
 }                                      

int LimitedNodeList::updateNodeWithDataFromPacket(QSharedPointer<NLPacket> packet, SharedNodePointer sendingNode) {
    QMutexLocker locker(&sendingNode->getMutex());

    NodeData* linkedData = sendingNode->getLinkedData();
    if (!linkedData && linkedDataCreateCallback) {
        linkedDataCreateCallback(sendingNode.data());
    }

    if (linkedData) {
        QMutexLocker linkedDataLocker(&linkedData->getMutex());
        return linkedData->parseData(*packet);
    }
    
    return 0;
}

NodeData testSetters(NodeData& B) {
    NodeData A("a");
    A.decreaseCount();
    assert(A.getCount() == 0);
    
    int bCount = 7;
    while(B.getCount() < bCount) {
        B.increaseCount();
    }
    assert(B.getCount() == bCount);

    assert(A.getToken() < B.getToken());

    A = B;
    assert(A.getToken() == B.getToken());    
    return B;
}

/** 
 * @brief copy constructor
 * @details creates a copy of the passed in TBST
 * 
 * @param other TBST tree to copy
 */
TBST::TBST(const TBST& other) {
    root = NULL;
    nodeCount = 0;
    if(other.root != NULL) {
        // handle root separately
        assert(root == NULL); // make sure that tree is empty
        // then add the root
        NodeData* rootData = other.root->getData();
        insert(rootData->getToken(), rootData->getCount());

        // recursively copy the left and right subtrees
        recursiveCopy(other.root->getLeft());
        recursiveCopy(other.root->getRight());
    }
    else {
        root = NULL;
    }
}

bool	CreateNodeDataByRawData(NodeData & ndata,const char *lpbuf,S_UINT bufsize)
{
	if(lpbuf==NULL)
		return false;

	if(bufsize<sizeof(S_UINT))
		return false;
		
	S_UINT len=0,tlen=sizeof(S_UINT);

	try{

		const char *pend=lpbuf+bufsize;
		const char *pt=lpbuf;
		memmove(&len,pt,tlen);
		pt+=tlen;
		if(pt>pend)
			return false;

		if(len!=bufsize)
		{
			puts("Raw data of NodeData error");
			return false;
		}

		S_UINT count;
		memmove(&count,pt,tlen);
		pt+=tlen;


		std::string str1,str2;
		S_UINT datalen;
		char * data;

		for(S_UINT i=0;i<count;i++)
		{
			str1=pt;
			pt+=strlen(str1.c_str())+1;
			if(pt>pend)
				return true;

			str2=pt;
			pt+=strlen(str2.c_str())+1;
			if(pt>pend)
				return true;

			ndata.insert(std::make_pair(str1,str2));
		}
	}catch(...)
	{
		cout<<"Exception to CreateNodeDataByRawData."<<endl;
		return false;
	}
    
	return true;
}

void EnvirePhysics::itemAdded(const TypedItemAddedEvent<Item<urdf::Collision>>& e)
{
#ifdef DEBUG
  LOG_DEBUG(("[EnvirePhysics::ItemAdded] urdf::Collision item received in frame *** " + e.frame + "***").c_str());
#endif
  urdf::Collision collision = e.item->getData();
  NodeData * node = new NodeData;
  node->init(collision.name);
  node->fromGeometry(collision.geometry);
  node->mass = 0.00001;
  node->density = 0.0;
  std::shared_ptr<NodeData> nodePtr(node);
  setPos(e.frame, nodePtr);
  node->movable = true;
  if (instantiateNode(nodePtr, e.frame))
  {
#ifdef DEBUG    
    LOG_DEBUG(("[EnvirePhysics::ItemAdded] Smurf::Collision - Instantiated and stored the nodeInterface in frame ***" + e.frame +"***").c_str());
#endif
  }
}

int LimitedNodeList::updateNodeWithDataFromPacket(const SharedNodePointer& matchingNode, const QByteArray &packet) {
    QMutexLocker locker(&matchingNode->getMutex());

    matchingNode->setLastHeardMicrostamp(usecTimestampNow());

    // if this was a sequence numbered packet we should store the last seq number for
    // a packet of this type for this node
    PacketType packetType = packetTypeForPacket(packet);
    if (SEQUENCE_NUMBERED_PACKETS.contains(packetType)) {
        matchingNode->setLastSequenceNumberForPacketType(sequenceNumberFromHeader(packet, packetType), packetType);
    }

    NodeData* linkedData = matchingNode->getLinkedData();
    if (!linkedData && linkedDataCreateCallback) {
        linkedDataCreateCallback(matchingNode.data());
    }

    if (linkedData) {
        QMutexLocker linkedDataLocker(&linkedData->getMutex());
        return linkedData->parseData(packet);
    }
    return 0;
}