void StatusMonitor::diagnosticStatus(const ros::TimerEvent& time)
{
if ((double)ros::Time::now().toSec() - (double)last_status_.toSec() > TIMEOUT){
diag_error("CONTROLLER TIMEOUT");
}
else if (status_ == 0){
diag_ok(info_);
}
else if (status_ == 1){
diag_warn(info_);
}
else if (status_ == 2){
diag_error(info_);
}
}
int diag_wait_agent(int n, const intptr_t *agents, int *code)
{
assert(n > 0);
assert(agents);
int status;
int i;
for (;;) {
pid_t pid = wait(&status);
if (pid == (pid_t)0) {
assert(0);
} else if (pid == (pid_t)-1) {
perror(NULL);
diag_error("failed to wait agent");
return -1;
}
if (!WIFEXITED(status)) continue;
if (code) {
*code = WEXITSTATUS(status);
}
for (i = 0; i < n; i++) {
if (pid == agents[i]) {
return i;
}
}
}
}
static void stop_client(void)
{
ssize_t ret;
uint8_t dummy = 0xff;
ret = write(fdpipes[1], &dummy , sizeof(dummy));
if (ret < 0)
diag_error("can't write in pipe, error:%s", strerror(errno));
}
static void sig_handler(int signum)
{
ssize_t ret;
uint8_t dummy = 0xff;
diag("signal %d(%s) received !", signum, strsignal(signum));
ret = write(fdpipes[1], &dummy , sizeof(dummy));
if (ret < 0)
diag_error("can't write in pipe, error:%s", strerror(errno));
}
/*
* TODO: n should exceed MAXIMUM_WAIT_OBJECTS
* http://msdn.microsoft.com/en-us/library/windows/desktop/ms687025(v=vs.85).aspx
*/
int diag_wait_agent(int n, const intptr_t *agents, int *code)
{
assert(n > 0);
assert(agents);
if (n > MAXIMUM_WAIT_OBJECTS) {
diag_error("number of agents exceeds MAXIMUM_WAIT_OBJECTS");
return -1;
}
DWORD r = WaitForMultipleObjects((DWORD)n,
(const HANDLE *)agents,
FALSE,
INFINITE);
assert(r != WAIT_TIMEOUT);
if (r == WAIT_FAILED) {
diag_error("failed to wait agent: %x",
(unsigned int)GetLastError());
return -1;
}
assert(WAIT_OBJECT_0 == 0);
if (WAIT_OBJECT_0 + (DWORD)n <= r) {
diag_fatal("failed to wait agent");
}
DWORD i = r - WAIT_OBJECT_0;
intptr_t p = agents[i];
if (code) {
DWORD c = 0;
if (GetExitCodeProcess((HANDLE)p, &c) == 0) {
diag_fatal("failed to get exit code: %x",
(unsigned int)GetLastError());
}
*code = (int)c;
}
if (CloseHandle((HANDLE)p) == 0) {
diag_error("failed to close handle");
}
return (int)i;
}
void diag_process_wait(struct diag_process *process)
{
assert(process);
HANDLE h = (HANDLE)process->pid;
WaitForSingleObject(h, INFINITE);
DWORD code = 0;
if (GetExitCodeProcess(h, &code) == 0) {
diag_error("failed to get exit code: %x",
(unsigned int)GetLastError());
}
process->status = (int)code;
CloseHandle(h);
}
/*
* References:
*
* How to spawn console processes with redirected standard handles
* http://support.microsoft.com/kb/190351
*/
struct diag_process *diag_run_program(struct diag_command *command)
{
assert(command);
if (!command->out) {
command->out = diag_malloc(PATH_LENGTH);
build_path(command->dir, local_pid, "out", command->out);
}
if (!command->err) {
command->err = diag_malloc(PATH_LENGTH);
build_path(command->dir, local_pid, "err", command->err);
}
local_pid++;
SECURITY_ATTRIBUTES sa;
sa.nLength = sizeof(sa);
sa.bInheritHandle = TRUE;
sa.lpSecurityDescriptor = NULL;
HANDLE ih = NULL;
if (command->in) {
ih = CreateFile(command->in,
GENERIC_READ,
FILE_SHARE_READ,
&sa,
OPEN_EXISTING,
0,
NULL);
if (ih == INVALID_HANDLE_VALUE) {
diag_error("failed to open file: %s: 0x%x",
command->in,
(unsigned int)GetLastError());
return NULL;
}
}
HANDLE oh = CreateFile(command->out,
GENERIC_WRITE,
FILE_SHARE_WRITE|FILE_SHARE_DELETE,
&sa,
CREATE_NEW,
FILE_ATTRIBUTE_NORMAL,
NULL);
if (oh == INVALID_HANDLE_VALUE) {
diag_error("failed to open file: %s: 0x%x",
command->out,
(unsigned int)GetLastError());
return NULL;
}
HANDLE eh = CreateFile(command->err,
GENERIC_WRITE,
FILE_SHARE_WRITE|FILE_SHARE_DELETE,
&sa,
CREATE_NEW,
FILE_ATTRIBUTE_NORMAL,
NULL);
if (eh == INVALID_HANDLE_VALUE) {
diag_error("failed to open file: %s: 0x%x",
command->err,
(unsigned int)GetLastError());
return NULL;
}
STARTUPINFO si;
GetStartupInfo(&si);
si.dwFlags |= STARTF_USESTDHANDLES;
si.hStdInput = ih;
si.hStdOutput = oh;
si.hStdError = eh;
PROCESS_INFORMATION pi;
ZeroMemory(&pi, sizeof(pi));
char *line = diag_get_command_line(command->argv);
BOOL b = CreateProcess(NULL,
line,
NULL,
NULL,
TRUE,
0,
NULL,
NULL,
&si,
&pi);
if (!b) {
diag_error("could not create process: %s: 0x%x",
line,
(unsigned int)GetLastError());
// should follow GetLastError()
diag_free(line);
if (ih) CloseHandle(ih);
CloseHandle(oh);
CloseHandle(eh);
return NULL;
}
/* We do not want WaitForInputIdle() because there is no interaction
between the parent process and this child */
diag_free(line);
if (ih) CloseHandle(ih);
CloseHandle(oh);
CloseHandle(eh);
CloseHandle(pi.hThread);
struct diag_process *p = diag_malloc(sizeof(*p));
p->pid = (intptr_t)pi.hProcess;
return p;
}
int main(int argc, char *argv[])
{
int ret, stop;
struct pollfd fds[2];
struct ps_process_provider process_provider = {
.add = ps_process_add,
.remove = ps_process_remove,
.event = ps_process_event,
};
struct ps_summary_provider summary_provider = {
.add = ps_summary_add,
.remove = ps_summary_remove,
.event = ps_summary_event,
};
if (argc < 2) {
diag_error("expected server address argument\n");
print_usage();
exit(EXIT_FAILURE);
}
/* open pipes */
ret = pipe(fdpipes);
if (ret < 0) {
ret = EXIT_FAILURE;
goto out;
}
/* create client */
client = obus_client_new("psclient", ps_bus_desc,
psclient_event, NULL);
if (!client) {
diag_error("can't create obus ps client");
ret = EXIT_FAILURE;
goto close_pipes;
}
/* subscribe to ps process objects */
ret = ps_process_subscribe(client, &process_provider, NULL);
if (ret < 0) {
diag_error("can't subscribe to ps process");
ret = EXIT_FAILURE;
goto destroy_client;
}
/* subscribe to ps summary objects */
ret = ps_summary_subscribe(client, &summary_provider, NULL);
if (ret < 0) {
diag_error("can't subscribe to ps process");
ret = EXIT_FAILURE;
goto destroy_client;
}
/* start client */
ret = obus_client_start(client, argv[1]);
if (ret < 0) {
diag_error("can't start obus ps client");
ret = EXIT_FAILURE;
goto unsubscribe_provider;
}
/* attach sig handler */
signal(SIGINT, &sig_handler);
/* build poll fds */
fds[0].fd = fdpipes[0];
fds[0].events = POLLIN;
fds[1].fd = obus_client_fd(client);
fds[1].events = POLLIN;
/* run main loop */
stop = 0;
do {
/* wait fd I/O occurs */
do {
ret = poll(fds, SIZEOF_ARRAY(fds), -1);
} while (ret == -1 && errno == EINTR);
if (ret < 0) {
diag_errno("poll");
stop = 1;
}
/* check read pipe events*/
if (fds[0].revents)
stop = 1;
/* process obus event */
if (!stop && fds[1].revents)
obus_client_process_fd(client);
} while (!stop);
ret = EXIT_SUCCESS;
unsubscribe_provider:
ps_process_unsubscribe(client, &process_provider);
ps_summary_unsubscribe(client, &summary_provider);
destroy_client:
obus_client_destroy(client);
close_pipes:
close(fdpipes[0]);
close(fdpipes[1]);
out:
return ret;
}
