void
Comm::handle_comm (struct sockaddr_in &addr, const char *msg, int len)
throw()
{
if (len == static_cast<int>(strlen(TOOL_REQUEST_MSG)) &&
memcmp(msg, TOOL_REQUEST_MSG, TOOL_REQUEST_LEN) == 0) {
std::string robotName = getRobotName();
const char *name = robotName.c_str();
int len = TOOL_ACCEPT_LEN + 3 + strlen(name);
char *response = (char *) malloc(len);
memcpy(&response[0], TOOL_ACCEPT_MSG, TOOL_ACCEPT_LEN);
response[TOOL_ACCEPT_LEN] = ':';
response[TOOL_ACCEPT_LEN+1] = (char)strlen(name);
response[TOOL_ACCEPT_LEN+2] = ':';
memcpy(&response[TOOL_ACCEPT_LEN+3], name, strlen(name));
struct sockaddr_in r_addr = addr;
r_addr.sin_port = htons(TOOL_PORT);
send(&response[0], len, r_addr);
free(response);
}else {
// validate packet format, check packet timestamp, and parse data
CommPacketHeader packet;
if (validate_packet(msg, len, packet) && timer.check_packet(packet))
parse_packet(packet, msg + sizeof(packet), len - sizeof(packet));
}
}
AREXPORT bool ArCentralForwarder::requestOnceUdp(const char *name,
ArNetPacket *packet,
bool quiet)
{
if (myClient != NULL)
{
unsigned int commandNum = myClient->findCommandFromName(name);
if (commandNum == 0)
{
if (!quiet)
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::requestOnce: Can't find commandNum for %s",
getRobotName(), name);
return false;
}
//return myClient->requestOnceUdp(name, packet, quiet);
if (packet != NULL)
{
packet->setCommand(commandNum);
packet->finalizePacket();
return internalRequestOnce(NULL, packet, false);
}
else
{
ArNetPacket tempPacket;
tempPacket.setCommand(commandNum);
tempPacket.finalizePacket();
return internalRequestOnce(NULL, &tempPacket, false);
}
}
else
{
if (!quiet)
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::requestOnce: Tried to call (for %s) while client was still NULL",
getRobotName(), name);
return false;
}
}
AREXPORT bool ArCentralForwarder::dataExists(const char *name)
{
if (myClient != NULL)
{
return myClient->dataExists(name);
}
else
{
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::dataExists: Tried to call while client was still NULL",
getRobotName());
return false;
}
}
AREXPORT bool ArCentralForwarder::remHandler(const char *name, ArFunctor1<ArNetPacket *> *functor)
{
if (myClient != NULL)
{
return myClient->remHandler(name, functor);
}
else
{
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::remHandler: Tried to call while client was still NULL",
getRobotName());
return false;
}
}
AREXPORT bool ArCentralForwarder::request(const char *name, long mSec)
{
if (myServer != NULL)
{
return myServer->internalSetDefaultFrequency(name, mSec);
}
else
{
ArLog::log(ArLog::Normal,
"ArCentralForwarder::%s::remHandler: Tried to call while server was still NULL",
getRobotName());
return false;
}
}
void RobotWidget::setRobotName(const std::string &name)
{
robot_name = name.c_str();
robot_name_label->setText(getRobotName());
}
/**
* This method is called when the RTB message "Collision [type] [angle]" occurs. This message is sent whan the robot has been hit by or hits another object,
* @param type The type of the hit object
* @param angle The angle the collision happened at in rad, relative to the robot front
*/
void BFServerRotateState::receiveRTBMessageCollision(object_type type,double angle) throw (StrategyException, bad_exception) {
BFServerBasicState::receiveRTBMessageCollision(type,angle);
_bfsharedssp->getLogger()->logMessage(1,getRobotName()+" was hit (recognized by BFServerRotateState), stop rotating ...");
sendServerTipString("STOPROTATION");
setCurrentServerState(DONOTROTATE);
}
