void
run(void)
{
while (!isStopping())
{
if (!m_queue.waitForItems(1.0))
continue;
if (m_queue.closed())
break;
TCPSocket* sock = m_queue.pop();
if (!sock)
continue;
try
{
m_handler.handleRequest(sock);
}
catch (std::runtime_error& e)
{
DUNE_ERR("Handler", e.what());
}
delete sock;
}
}
int
waitForDaemon(pid_t pid)
{
int stat = 0;
while (1)
{
int rv = waitpid(pid, &stat, 0);
if (rv != -1)
{
if (WIFSIGNALED(stat))
return WTERMSIG(stat);
return 0;
}
if (errno == EINTR)
continue;
if (errno == ECHILD)
DUNE_ERR("Launcher", DTR("daemon process no longer exists"));
}
return 0;
}
Fragments::Fragments(IMC::Message* msg, int mtu)
{
m_uid = s_uid++;
m_num_frags = 0;
int frag_size = mtu - sizeof(IMC::Header) - 5;
if (frag_size <= 0)
{
DUNE_ERR("Fragments", "MTU is too small");
return;
}
Utils::ByteBuffer buff;
int size = IMC::Packet::serialize(msg, buff);
uint8_t* buffer = buff.getBuffer();
int part = 0, pos = 0;
m_num_frags = (int)std::ceil((float)size / (float)mtu);
while (pos < size)
{
int remaining = size - pos;
int cur_size = std::min(remaining, mtu);
IMC::MessagePart* mpart = new IMC::MessagePart();
mpart->frag_number = part++;
mpart->num_frags = m_num_frags;
mpart->uid = m_uid;
mpart->data.assign(buffer + pos, buffer + pos + cur_size);
pos += cur_size;
m_fragments.push_back(mpart);
}
}
void
RequestHandler::sendFile(TCPSocket* sock, const std::string& file, HeaderFieldsMap& hdr_fields, int64_t off_beg, int64_t off_end)
{
int64_t size = FileSystem::Path(file).size();
// File doesn't exist or isn't accessible.
if (size < 0)
{
sendResponse404(sock);
return;
}
// Requested end offset is larger than file size.
if (off_end > size)
{
sendResponse416(sock);
return;
}
// Send full file.
if ((off_beg < 0) && (off_end < 0))
{
sendHeader(sock, STATUS_LINE_200, size, &hdr_fields);
if (!sock->writeFile(file.c_str(), size - 1))
DUNE_ERR("HTTPHandle", "failed to send file: " << System::Error::getLastMessage());
return;
}
// Send partial content.
if (off_end < 0)
off_end = size - 1;
if (off_beg < 0)
off_beg = 0;
std::ostringstream os;
os << "bytes "
<< off_beg << "-" << off_end
<< "/" << size;
hdr_fields.insert(std::make_pair("Content-Range", os.str()));
sendHeader(sock, STATUS_LINE_206, off_end - off_beg + 1, &hdr_fields);
if (!sock->writeFile(file.c_str(), off_end, off_beg))
DUNE_ERR("HTTPHandle", "failed to send file: " << System::Error::getLastMessage());
}
void
run(void)
{
Frame* frame = NULL;
IOMultiplexing iom;
m_socket.addToPoll(iom);
while (!isStopping())
{
if (!iom.poll(1.0))
continue;
int rv = m_socket.read((char*)m_buffer, m_buffer_capacity);
if (rv == c_header_size)
{
m_count = 0;
frame = dequeueClean();
if (frame == NULL)
{
DUNE_ERR("GVSP", "buffer overrun");
break;
}
frame->setTimeStamp();
}
else if (rv == c_footer_size)
{
enqueueDirty(frame);
frame = NULL;
}
else
{
if (frame != NULL)
{
uint16_t packet_number = 0;
ByteCopy::fromBE(packet_number, m_buffer + 6);
m_count += frame->writePacket(packet_number, m_buffer + 8, rv - 8);
}
else
{
DUNE_ERR("GVSP", "null frame");
}
}
}
}
int
runDaemon(DUNE::Daemon& daemon)
{
setDaemonSignalHandlers();
bool call_abort = false;
try
{
daemon.start();
while (!s_stop)
{
if (!daemon.isRunning())
{
call_abort = true;
break;
}
Delay::wait(1.0);
}
DUNE_WRN("Daemon", DTR("stopping tasks"));
daemon.stopAndJoin();
}
catch (std::exception& e)
{
DUNE_ERR("Daemon", e.what());
return 1;
}
catch (...)
{
DUNE_ERR("Daemon", DTR("unhandled exception"));
return 1;
}
if (call_abort)
std::abort();
return 0;
}
void
fromDDT(const char* file)
{
try
{
m_loader.load(file);
m_creator = castUnsafe<task_creator_t, void*>(m_loader.getSymbol("dune_task_create"));
}
catch (System::Error& e)
{
DUNE_ERR("Task Creator", e.what());
}
}
IMC::Message*
FragmentedMessage::setFragment(const IMC::MessagePart* part)
{
// is this the first fragment?
if (m_num_frags < 0)
{
m_num_frags = part->num_frags;
m_uid = part->uid;
m_src = part->getSource();
m_creation_time = Time::Clock::get();
}
// Check if this is a valid fragment
if (part->uid != m_uid || part->getSource() != m_src ||
part->frag_number >= m_num_frags)
{
if (m_parent == NULL)
DUNE_ERR("FragmentedMessage", "Invalid fragment received and it won't be processed.");
else
m_parent->err("Invalid fragment received and it won't be processed.");
return NULL;
}
m_fragments[part->frag_number] = *part;
// Message is complete. Let's reassemble and return it.
if (getFragmentsMissing() == 0)
{
int i;
int total_length = 0;
// concatenate all parts into a single array
std::vector<char> data;
for (i = 0; i < m_num_frags; i++)
{
total_length += m_fragments[i].data.size();
data.insert(data.end(), m_fragments[i].data.begin(),
m_fragments[i].data.end());
}
return IMC::Packet::deserialize((uint8_t*)&data[0], total_length);
}
else
{
return 0;
}
}
void
Server::poll(double timeout)
{
if (m_poll.poll(timeout))
{
if (m_poll.wasTriggered(m_sock))
{
try
{
TCPSocket* nc = m_sock.accept();
m_queue.push(nc);
}
catch (std::runtime_error& e)
{
DUNE_ERR("Server", e.what());
}
}
}
}
int
main(int argc, char** argv)
{
DUNE::Tasks::Context ctx;
Path bin = ctx.dir_app / "dune";
#if defined(DUNE_OS_POSIX)
setLauncherSignalHandlers();
Counter<double> delta(c_restart_period);
while (!s_stop)
{
pid_t pid = fork();
if (pid == 0)
{
char name[PATH_MAX] = {0};
std::strcpy(name, bin.c_str());
argv[0] = name;
execv(argv[0], argv);
}
int rv = waitForDaemon(pid);
if (rv == 0)
break;
if (rv == SIGABRT)
DUNE_WRN("Launcher", DTR("execution aborted"));
else
DUNE_ERR("Launcher", DTR("daemon crashed with signal ") << rv);
Delay::wait(2.0);
if (delta.overflow())
delta.reset();
}
#endif
return 0;
}
void
RequestHandler::sendData(TCPSocket* sock, const char* data, int size, HeaderFieldsMap* hdr_fields)
{
sendHeader(sock, STATUS_LINE_200, size, hdr_fields);
int remaining = size;
int rv = 0;
while (remaining > 0)
{
rv = sock->write(data, size);
if (rv < 0)
{
DUNE_ERR("RequestHandler", "error sending data: " << rv);
return;
}
remaining -= rv;
}
}
void
PlanConfigParser::parse(Parsers::Config& cfg, IMC::PlanSpecification& plan)
{
cfg.get("Plan Configuration", "Plan ID", "", plan.plan_id);
// Parse plan actions
std::vector<std::string> plan_start_actions;
std::vector<std::string> plan_end_actions;
cfg.get("Plan Configuration", "Start Actions", "", plan_start_actions);
cfg.get("Plan Configuration", "End Actions", "", plan_end_actions);
parseActions(cfg, plan_start_actions, plan.start_actions);
parseActions(cfg, plan_end_actions, plan.end_actions);
std::vector<std::string> ids;
cfg.get("Plan Configuration", "Maneuvers", "", ids);
for (unsigned i = 0; i < ids.size(); i++)
{
std::string id = ids[i];
IMC::PlanManeuver pman;
pman.maneuver_id = id;
std::string type;
cfg.get(id, "Type", "!!unknown!!", type);
#ifdef DUNE_IMC_POPUP
if (type == "PopUp")
{
IMC::PopUp popup;
parse(cfg, id, popup);
pman.data.set(popup);
}
else
#endif
#ifdef DUNE_IMC_GOTO
if (type == "Goto")
{
IMC::Goto goto_man;
parse(cfg, id, goto_man);
pman.data.set(goto_man);
}
else
#endif
#ifdef DUNE_IMC_STATIONKEEPING
if (type == "StationKeeping")
{
IMC::StationKeeping sk_man;
parse(cfg, id, sk_man);
pman.data.set(sk_man);
}
else
#endif
#ifdef DUNE_IMC_IDLEMANEUVER
if (type == "Idle")
{
IMC::IdleManeuver idle;
parse(cfg, id, idle);
pman.data.set(idle);
}
else
#endif
#ifdef DUNE_IMC_LOITER
if (type == "Loiter")
{
IMC::Loiter loiter;
parse(cfg, id, loiter);
pman.data.set(loiter);
}
else
#endif
#ifdef DUNE_IMC_FOLLOWPATH
if (type == "FollowPath")
{
IMC::FollowPath fp;
parse(cfg, id, fp);
pman.data.set(fp);
}
else
#endif
#ifdef DUNE_IMC_ROWS
if (type == "Rows")
{
IMC::Rows r;
parse(cfg, id, r);
pman.data.set(r);
r.toText(std::cerr);
}
else
#endif
#ifdef DUNE_IMC_ELEMENTALMANEUVER
if (type == "ElementalManeuver")
{
IMC::ElementalManeuver eman;
parse(cfg, id, eman);
pman.data.set(eman);
}
else
#endif
#ifdef DUNE_IMC_YOYO
if (type == "YoYo")
{
IMC::YoYo yoyo;
parse(cfg, id, yoyo);
pman.data.set(yoyo);
}
else
#endif
#ifdef DUNE_IMC_ELEVATOR
if (type == "Elevator")
{
IMC::Elevator elev;
parse(cfg, id, elev);
pman.data.set(elev);
elev.toText(std::cerr);
}
else
#endif
#ifdef DUNE_IMC_DUBIN
if (type == "Dubin")
{
IMC::Dubin dub;
parse(cfg, id, dub);
pman.data.set(dub);
}
else
#endif
#ifdef DUNE_IMC_COMPASSCALIBRATION
if (type == "CompassCalibration")
{
IMC::CompassCalibration ccalib;
parse(cfg, id, ccalib);
pman.data.set(ccalib);
}
else
#endif
{
DUNE_ERR
("Plan Load", "Unknown or unsupported maneuver type: '" << type << '\'');
return;
}
// Parse maneuver actions
std::vector<std::string> man_start_actions;
std::vector<std::string> man_end_actions;
cfg.get(id, "Start Actions", "", man_start_actions);
cfg.get(id, "End Actions", "", man_end_actions);
parseActions(cfg, man_start_actions, pman.start_actions);
parseActions(cfg, man_end_actions, pman.end_actions);
plan.maneuvers.push_back(pman);
if (plan.maneuvers.size() == 1)
{
plan.start_man_id = id;
}
else
{
// See maneuver sequence in graph terms
IMC::PlanTransition ptransition;
ptransition.source_man = ids[plan.maneuvers.size() - 2];
ptransition.dest_man = id;
plan.transitions.push_back(ptransition);
}
}
}
