ArServerInfoDrawings.cpp

Go to the documentation of this file.

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "Aria.h"
#include "ArExport.h"
#include "ArServerInfoDrawings.h"

AREXPORT ArServerInfoDrawings::ArServerInfoDrawings(ArServerBase *server) :
  myNetListDrawingsCB(this, &ArServerInfoDrawings::netListDrawings),
  myNetGetDrawingListCB(this, &ArServerInfoDrawings::netGetDrawingList)
{
  myServer = server;

  if (myServer != NULL)
  {
    myServer->addData("listDrawings", 
                      "Deprecated; use getDrawingList instead. Gets a list of things that can be drawn",
                      &myNetListDrawingsCB, 
                      "none", 
                      "byte4: numDrawings, <repeats numDrawings> string: name, string: shape, byte4: primaryColor(0RGB), byte4: size, byte4: layer, byte4: defaultRefreshTime(0 == don't request refresh), byte4: secondaryColor(0RGB)", "SensorInfo", "RETURN_SINGLE");
     myServer->addData("getDrawingList", 
                      "Gets a list of items to be drawn on the map. Each packet contains one drawing.  Terminates on empty packet.",
                      &myNetGetDrawingListCB, 
                      "none", 
          "For each drawing, a packet that contains: string: name, string: shape, byte4: primaryColor(0RGB), byte4: size, byte4: layer, byte4: defaultRefreshTime(0 == don't request refresh), byte4: secondaryColor(0RGB), string: visibility", 
          "SensorInfo", "RETURN_UNTIL_EMPTY");
 }

}

AREXPORT ArServerInfoDrawings::~ArServerInfoDrawings() 
{

}

AREXPORT bool ArServerInfoDrawings::addDrawing(ArDrawingData *drawingData, 
                                           const char *name,
                         ArFunctor2<ArServerClient *, ArNetPacket *> *functor)
{
  if (myDrawingDatas.find(name) != myDrawingDatas.end() || 
      myDrawingCallbacks.find(name) != myDrawingCallbacks.end())
  {
    ArLog::log(ArLog::Normal, "ArServerInfoDrawings::addDrawing: Already a drawing of name '%s'", name);
    return false;
  }
  if (myServer == NULL || 
      !myServer->addData(name, "", functor, "none", 
                        "depends on shape, see MobileEyes docs", "SensorInfo",
                         "RETURN_SINGLE"))
  {
    ArLog::log(ArLog::Normal, 
               "ArServerInfoDrawings::addDrawing: Could not add data of name '%s' to server", 
               name);
    return false;
  }
  myDrawingDatas[name] = drawingData;
  myDrawingCallbacks[name] = functor;
  return true;
}

AREXPORT void ArServerInfoDrawings::netListDrawings(ArServerClient *client, 
                                                    ArNetPacket *packet)
{
  ArNetPacket sendingPacket;
  std::map<std::string, ArDrawingData *, ArStrCaseCmpOp>::iterator it;
  
  sendingPacket.byte4ToBuf(myDrawingDatas.size());
  for (it = myDrawingDatas.begin(); it != myDrawingDatas.end(); it++)
  {
    sendingPacket.strToBuf((*it).first.c_str());
    sendingPacket.strToBuf((*it).second->getShape());
    sendingPacket.byte4ToBuf((*it).second->getPrimaryColor().colorToByte4());
    sendingPacket.byte4ToBuf((*it).second->getSize());
    sendingPacket.byte4ToBuf((*it).second->getLayer());
    sendingPacket.uByte4ToBuf((*it).second->getDefaultRefreshTime());
    sendingPacket.byte4ToBuf((*it).second->getSecondaryColor().colorToByte4());
  }
  client->sendPacketTcp(&sendingPacket);
}

AREXPORT void ArServerInfoDrawings::netGetDrawingList(ArServerClient *client, 
                                                    ArNetPacket *packet)
{
  ArNetPacket sendingPacket;
  
  // TODO: Any need to protect the map by a mutex?

  for (std::map<std::string, ArDrawingData *, ArStrCaseCmpOp>::iterator it = 
                myDrawingDatas.begin(); 
       it != myDrawingDatas.end(); 
       it++)
  {
    sendingPacket.empty();

    sendingPacket.strToBuf((*it).first.c_str());
    sendingPacket.strToBuf((*it).second->getShape());
    sendingPacket.byte4ToBuf((*it).second->getPrimaryColor().colorToByte4());
    sendingPacket.byte4ToBuf((*it).second->getSize());
    sendingPacket.byte4ToBuf((*it).second->getLayer());
    sendingPacket.uByte4ToBuf((*it).second->getDefaultRefreshTime());
    sendingPacket.byte4ToBuf((*it).second->getSecondaryColor().colorToByte4());
    sendingPacket.strToBuf((*it).second->getVisibility());

    client->sendPacketTcp(&sendingPacket);

  } // end for each drawing

  sendingPacket.empty();
  client->sendPacketTcp(&sendingPacket);
}

AREXPORT bool ArServerInfoDrawings::addRangeDevice(ArRangeDevice *rangeDevice)
{
  bool ret = true;
  char name[512];
  if (myServer == NULL)
  {
    ArLog::log(ArLog::Normal, "ArServerInfoDrawings::addRangeDeviceForDrawing: srever is NULL");
    return false;
  }
  sprintf(name, "%sCurrent", rangeDevice->getName());
  // LEAK a little when called, shouldn't be called more than a few
  // times and isn't a leak unless its called more than once
  if (rangeDevice->getCurrentDrawingData() != NULL && 
      !addDrawing(rangeDevice->getCurrentDrawingData(), name, 
                  new ArFunctor3C<ArServerInfoDrawings, 
                  ArServerClient *, ArNetPacket *, 
                  ArRangeDevice *>(this, &ArServerInfoDrawings::netRangeDeviceCurrent, NULL, NULL, rangeDevice)))
  {
    ArLog::log(ArLog::Normal, 
               "ArServerInfoDrawings::addRangeDevice: Could not add data for range device '%s' to server ('%s')", rangeDevice->getName(), name);
    ret = false;
  }
  sprintf(name, "%sCumulative", rangeDevice->getName());
  if (rangeDevice->getCumulativeDrawingData() != NULL && 
      !addDrawing(rangeDevice->getCumulativeDrawingData(), name, 
                  new ArFunctor3C<ArServerInfoDrawings, 
                  ArServerClient *, ArNetPacket *, 
                  ArRangeDevice *>(this, &ArServerInfoDrawings::netRangeDeviceCumulative, NULL, NULL, rangeDevice)))
  {
    ArLog::log(ArLog::Normal, 
               "ArServerInfoDrawings::addRangeDevice: Could not add data for range device '%s' to server ('%s')", rangeDevice->getName(), name);
    ret = false;
  }
  return ret;
}

AREXPORT bool ArServerInfoDrawings::addRobotsRangeDevices(ArRobot *robot)
{
  std::list<ArRangeDevice *>::iterator it;
  bool ret = true;
  ArRangeDevice *device;
  if (robot == NULL || robot->getRangeDeviceList() == NULL)
  {
    ArLog::log(ArLog::Terse, "InfoDrawings::addRobotsRangeDevices: Robot or robot's range device list is NULL");
    return false;
  }
  for (it = robot->getRangeDeviceList()->begin();
       it != robot->getRangeDeviceList()->end();
       it++)
  {
    device = (*it);
    device->lockDevice();
    if (!addRangeDevice(device))
      ret = false;
    device->unlockDevice();
  }
  return ret;
}

AREXPORT void ArServerInfoDrawings::netRangeDeviceCurrent(
        ArServerClient *client, ArNetPacket *packet, ArRangeDevice *device)
{
  ArNetPacket sendPacket;
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;

  device->lockDevice();
  readings = device->getCurrentBuffer();
  if (readings == NULL)
  {
    ArLog::log(ArLog::Verbose, "ArServerInfoDrawing::netRangeDeviceCurrent: No current buffer for %s", device->getName());
    device->unlockDevice();
    sendPacket.byte4ToBuf(0);
    client->sendPacketUdp(&sendPacket);
    return;
  } 
  
  sendPacket.byte4ToBuf(readings->size());
  for (it = readings->begin(); it != readings->end(); it++)
  {
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getX()));
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getY()));
  }
  device->unlockDevice();
  client->sendPacketUdp(&sendPacket);

}


AREXPORT void ArServerInfoDrawings::netRangeDeviceCumulative(
        ArServerClient *client, ArNetPacket *packet, ArRangeDevice *device)
{
  ArNetPacket sendPacket;
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;

  device->lockDevice();
  readings = device->getCumulativeBuffer();
  if (readings == NULL)
  {
    ArLog::log(ArLog::Verbose, "ArServerInfoDrawing::netRangeDeviceCumulative: No cumulative buffer for %s", device->getName());
    device->unlockDevice();
    sendPacket.byte4ToBuf(0);
    client->sendPacketUdp(&sendPacket);
    return;
  } 
  
  sendPacket.byte4ToBuf(readings->size());
  for (it = readings->begin(); it != readings->end(); it++)
  {
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getX()));
    sendPacket.byte4ToBuf(ArMath::roundInt((*it)->getY()));
  }
  device->unlockDevice();
  client->sendPacketUdp(&sendPacket);

}

---