C++ ArTime::setSec方法代码示例

AREXPORT ArTime ArSerialConnection::getTimeRead(int index)
{
  ArTime ret;
  struct timeval timeStamp;
  if (myPort <= 0)
  {
    ret.setToNow();
    return ret;
  }

  if (myTakingTimeStamps)
  {
    timeStamp.tv_sec = index;
    if (ioctl(myPort, TIOGETTIMESTAMP, &timeStamp) == 0)
    {
      ret.setSec(timeStamp.tv_sec);
      ret.setMSec(timeStamp.tv_usec / 1000);
    }
    else
      ret.setToNow();
  }
  else
    ret.setToNow();

  return ret;
}

ArPoseWithTime RosArnlNode::rosPoseStampedToArPoseWithTime(const geometry_msgs::PoseStamped& p)
{ 
  ArPoseWithTime arp(rosPoseToArPose(p));
  ArTime t;
  t.setSec(p.header.stamp.sec);
  t.setMSec(p.header.stamp.nsec * 1.0e-6);
  arp.setTime(t);
  return arp;
}

/// This should be its own thread here
void *ArCentralManager::runThread(void *arg)
{
  std::list<ArSocket *>::iterator sIt;
  std::list<std::string>::iterator nIt;
  std::list<ArCentralForwarder *>::iterator fIt;
  ArSocket *socket;
  std::string robotName;
  ArCentralForwarder *forwarder;

  ArNetPacket *packet;
  std::list<ArNetPacket *> addPackets;
  std::list<ArNetPacket *> remPackets;


  threadStarted();
  while (getRunning())
  {
    int numForwarders = 0;
    int numClients = 0;
    
    myDataMutex.lock();
    // this is where the original code to add forwarders was before we
    // changed the unique behavior to drop old ones...


    std::list<ArCentralForwarder *> connectedRemoveList;
    std::list<ArCentralForwarder *> unconnectedRemoveList;
    for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
    {
      forwarder = (*fIt);

      numForwarders++;
      if (forwarder->getServer() != NULL)
	numClients += forwarder->getServer()->getNumClients();
      bool connected = forwarder->isConnected();
      bool removed = false;
      if (!forwarder->callOnce(myHeartbeatTimeout, myUdpHeartbeatTimeout,
			       myRobotBackupTimeout, myClientBackupTimeout))
      {
	if (connected)
	{
	  ArLog::log(ArLog::Normal, "Will remove forwarder from %s", 
		     forwarder->getRobotName());
	  connectedRemoveList.push_back(forwarder);
	  removed = true;
	}
	else
	{
	  ArLog::log(ArLog::Normal, "Failed to connect to forwarder from %s", 
		     forwarder->getRobotName());
	  unconnectedRemoveList.push_back(forwarder);
	  removed = true;
	}
      }
      if (!connected && !removed && forwarder->isConnected())
      {
	ArLog::log(ArLog::Normal, "Adding forwarder %s", 
		   forwarder->getRobotName());
	ArTime *newTime = new ArTime;
	newTime->setSec(0);
	myUsedPorts[forwarder->getPort()] = newTime;
	
	std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
	for (it = myForwarderAddedCBList.begin();
	     it != myForwarderAddedCBList.end();
	     it++)
	{
	  if ((*it).second->getName() == NULL || 
	      (*it).second->getName()[0] == '\0')
	    ArLog::log(ArLog::Normal, "Calling unnamed add functor at %d",
		       -(*it).first);
	  else
	    ArLog::log(ArLog::Normal, "Calling %s add functor at %d",
		       (*it).second->getName(), -(*it).first);
	  (*it).second->invoke(forwarder);
	}
	ArLog::log(ArLog::Normal, "Added forwarder %s", 
		   forwarder->getRobotName());      
	ArNetPacket *sendPacket = new ArNetPacket;
	sendPacket->strToBuf("");
	sendPacket->uByte2ToBuf(forwarder->getPort());
	sendPacket->strToBuf(forwarder->getRobotName());
	sendPacket->strToBuf("");
	sendPacket->strToBuf(
		forwarder->getClient()->getTcpSocket()->getIPString());
	addPackets.push_back(sendPacket);
	// MPL added this at the same time as the changes for the deadlock that happened down below
	//myClientServer->broadcastPacketTcp(&sendPacket, "clientAdded");
      }
    }

    while ((fIt = connectedRemoveList.begin()) != connectedRemoveList.end())
    {
      forwarder = (*fIt);

      ArLog::log(ArLog::Normal, "Removing forwarder %s", 
		 forwarder->getRobotName());
      std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
      for (it = myForwarderRemovedCBList.begin();
//.........这里部分代码省略.........