void timerCallback(const ros::TimerEvent& e)
{
// Saturate commands to stop crazy thing from happening
if (cmd.angular.z > 2.0)
cmd.angular.z = 2.0;
else if (cmd.angular.z < -2.0)
cmd.angular.z = -2.0;
/* if (cmd.angular.z > 2.0)
cmd.angular.z = 2.0;
else if (cmd.angular.z < -2.0)
cmd.angular.z = -2.0;*/
// Map control inputs and outputs and fix sending stuff out
double x_cmd = (cmd.linear.x*12.0/0.6271) - (cmd.angular.z*12.0/0.92);
double y_cmd = (cmd.linear.x*12.0/0.6271) + (cmd.angular.z*12.0/0.92);
write_commands(-x_cmd, -y_cmd);
//Health check
static int ticks = 0;
ticks++;
if(ticks > 50) {
//ROS_INFO("ALIVE");
ticks = 0;
if( (ros::Time::now() - last_arduino_message).toSec() > 2)
ROS_INFO("Arduino appears to be dead");
}
}
/* Reads contents of the filename file as an info file and updates it with the
* state of current instance. */
static void
update_info_file(const char filename[])
{
/* TODO: refactor this function update_info_file() */
FILE *fp;
char **cmds_list;
int ncmds_list = -1;
char **ft = NULL, **fx = NULL, **fv = NULL, **cmds = NULL, **marks = NULL;
char **lh = NULL, **rh = NULL, **cmdh = NULL, **srch = NULL, **regs = NULL;
int *lhp = NULL, *rhp = NULL, *bt = NULL, *bmt = NULL;
char **prompt = NULL, **filter = NULL, **trash = NULL;
char **bmarks = NULL;
int nft = 0, nfx = 0, nfv = 0, ncmds = 0, nmarks = 0, nlh = 0, nrh = 0;
int ncmdh = 0, nsrch = 0, nregs = 0, nprompt = 0, nfilter = 0, ntrash = 0;
int nbmarks = 0;
char **dir_stack = NULL;
int ndir_stack = 0;
char *non_conflicting_marks;
if(cfg.vifm_info == 0)
return;
cmds_list = list_udf();
while(cmds_list[++ncmds_list] != NULL);
non_conflicting_marks = strdup(valid_marks);
if((fp = os_fopen(filename, "r")) != NULL)
{
size_t nlhp = 0UL, nrhp = 0UL, nbt = 0UL, nbmt = 0UL;
char *line = NULL, *line2 = NULL, *line3 = NULL, *line4 = NULL;
while((line = read_vifminfo_line(fp, line)) != NULL)
{
const char type = line[0];
const char *const line_val = line + 1;
if(type == LINE_TYPE_COMMENT || type == '\0')
continue;
if(type == LINE_TYPE_FILETYPE)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if(!ft_assoc_exists(&filetypes, line_val, line2))
{
nft = add_to_string_array(&ft, nft, 2, line_val, line2);
}
}
}
else if(type == LINE_TYPE_XFILETYPE)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if(!ft_assoc_exists(&xfiletypes, line_val, line2))
{
nfx = add_to_string_array(&fx, nfx, 2, line_val, line2);
}
}
}
else if(type == LINE_TYPE_FILEVIEWER)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if(!ft_assoc_exists(&fileviewers, line_val, line2))
{
nfv = add_to_string_array(&fv, nfv, 2, line_val, line2);
}
}
}
else if(type == LINE_TYPE_COMMAND)
{
if(line_val[0] == '\0')
continue;
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
int i;
const char *p = line_val;
for(i = 0; i < ncmds_list; i += 2)
{
int cmp = strcmp(cmds_list[i], p);
if(cmp < 0)
continue;
if(cmp == 0)
p = NULL;
break;
}
if(p == NULL)
continue;
ncmds = add_to_string_array(&cmds, ncmds, 2, line_val, line2);
}
}
else if(type == LINE_TYPE_LWIN_HIST || type == LINE_TYPE_RWIN_HIST)
{
if(line_val[0] == '\0')
continue;
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
const int pos = read_optional_number(fp);
if(type == LINE_TYPE_LWIN_HIST)
{
process_hist_entry(&lwin, line_val, line2, pos, &lh, &nlh, &lhp,
&nlhp);
}
else
{
process_hist_entry(&rwin, line_val, line2, pos, &rh, &nrh, &rhp,
&nrhp);
}
}
}
else if(type == LINE_TYPE_MARK)
{
const char mark = line_val[0];
if(line_val[1] != '\0')
{
LOG_ERROR_MSG("Expected end of line, but got: %s", line_val + 1);
}
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if((line3 = read_vifminfo_line(fp, line3)) != NULL)
{
const int timestamp = read_optional_number(fp);
const char mark_str[] = { mark, '\0' };
if(!char_is_one_of(valid_marks, mark))
{
continue;
}
if(is_mark_older(mark, timestamp))
{
char *const pos = strchr(non_conflicting_marks, mark);
if(pos != NULL)
{
nmarks = add_to_string_array(&marks, nmarks, 3, mark_str, line2,
line3);
nbt = add_to_int_array(&bt, nbt, timestamp);
*pos = '\xff';
}
}
}
}
}
else if(type == LINE_TYPE_BOOKMARK)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if((line3 = read_vifminfo_line(fp, line3)) != NULL)
{
long timestamp;
if(read_number(line3, ×tamp) &&
bmark_is_older(line_val, timestamp))
{
nbmarks = add_to_string_array(&bmarks, nbmarks, 2, line_val,
line2);
nbmt = add_to_int_array(&bmt, nbmt, timestamp);
}
}
}
}
else if(type == LINE_TYPE_TRASH)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
char *const trash_name = convert_old_trash_path(line_val);
if(exists_in_trash(trash_name) && !is_in_trash(trash_name))
{
ntrash = add_to_string_array(&trash, ntrash, 2, trash_name, line2);
}
free(trash_name);
}
}
else if(type == LINE_TYPE_CMDLINE_HIST)
{
if(!hist_contains(&cfg.cmd_hist, line_val))
{
ncmdh = add_to_string_array(&cmdh, ncmdh, 1, line_val);
}
}
else if(type == LINE_TYPE_SEARCH_HIST)
{
if(!hist_contains(&cfg.search_hist, line_val))
{
nsrch = add_to_string_array(&srch, nsrch, 1, line_val);
}
}
else if(type == LINE_TYPE_PROMPT_HIST)
{
if(!hist_contains(&cfg.prompt_hist, line_val))
{
nprompt = add_to_string_array(&prompt, nprompt, 1, line_val);
}
}
else if(type == LINE_TYPE_FILTER_HIST)
{
if(!hist_contains(&cfg.filter_hist, line_val))
{
nfilter = add_to_string_array(&filter, nfilter, 1, line_val);
}
}
else if(type == LINE_TYPE_DIR_STACK)
{
if((line2 = read_vifminfo_line(fp, line2)) != NULL)
{
if((line3 = read_vifminfo_line(fp, line3)) != NULL)
{
if((line4 = read_vifminfo_line(fp, line4)) != NULL)
{
ndir_stack = add_to_string_array(&dir_stack, ndir_stack, 4,
line_val, line2, line3 + 1, line4);
}
}
}
}
else if(type == LINE_TYPE_REG)
{
if(regs_exists(line_val[0]))
{
continue;
}
nregs = add_to_string_array(®s, nregs, 1, line);
}
}
free(line);
free(line2);
free(line3);
free(line4);
fclose(fp);
}
if((fp = os_fopen(filename, "w")) != NULL)
{
fprintf(fp, "# You can edit this file by hand, but it's recommended not to "
"do that.\n");
if(cfg.vifm_info & VIFMINFO_OPTIONS)
{
write_options(fp);
}
if(cfg.vifm_info & VIFMINFO_FILETYPES)
{
write_assocs(fp, "Filetypes", LINE_TYPE_FILETYPE, &filetypes, nft, ft);
write_assocs(fp, "X Filetypes", LINE_TYPE_XFILETYPE, &xfiletypes, nfx,
fx);
write_assocs(fp, "Fileviewers", LINE_TYPE_FILEVIEWER, &fileviewers, nfv,
fv);
}
if(cfg.vifm_info & VIFMINFO_COMMANDS)
{
write_commands(fp, cmds_list, cmds, ncmds);
}
if(cfg.vifm_info & VIFMINFO_MARKS)
{
write_marks(fp, non_conflicting_marks, marks, bt, nmarks);
}
if(cfg.vifm_info & VIFMINFO_BOOKMARKS)
{
write_bmarks(fp, bmarks, bmt, nbmarks);
}
if(cfg.vifm_info & VIFMINFO_TUI)
{
write_tui_state(fp);
}
if((cfg.vifm_info & VIFMINFO_DHISTORY) && cfg.history_len > 0)
{
write_view_history(fp, &lwin, "Left", LINE_TYPE_LWIN_HIST, nlh, lh, lhp);
write_view_history(fp, &rwin, "Right", LINE_TYPE_RWIN_HIST, nrh, rh, rhp);
}
if(cfg.vifm_info & VIFMINFO_CHISTORY)
{
write_history(fp, "Command line", LINE_TYPE_CMDLINE_HIST,
MIN(ncmdh, cfg.history_len - cfg.cmd_hist.pos), cmdh, &cfg.cmd_hist);
}
if(cfg.vifm_info & VIFMINFO_SHISTORY)
{
write_history(fp, "Search", LINE_TYPE_SEARCH_HIST, nsrch, srch,
&cfg.search_hist);
}
if(cfg.vifm_info & VIFMINFO_PHISTORY)
{
write_history(fp, "Prompt", LINE_TYPE_PROMPT_HIST, nprompt, prompt,
&cfg.prompt_hist);
}
if(cfg.vifm_info & VIFMINFO_FHISTORY)
{
write_history(fp, "Local filter", LINE_TYPE_FILTER_HIST, nfilter, filter,
&cfg.filter_hist);
}
if(cfg.vifm_info & VIFMINFO_REGISTERS)
{
write_registers(fp, regs, nregs);
}
if(cfg.vifm_info & VIFMINFO_DIRSTACK)
{
write_dir_stack(fp, dir_stack, ndir_stack);
}
write_trash(fp, trash, ntrash);
if(cfg.vifm_info & VIFMINFO_STATE)
{
write_general_state(fp);
}
if(cfg.vifm_info & VIFMINFO_CS)
{
fputs("\n# Color scheme:\n", fp);
fprintf(fp, "c%s\n", cfg.cs.name);
}
fclose(fp);
}
free_string_array(ft, nft);
free_string_array(fv, nfv);
free_string_array(fx, nfx);
free_string_array(cmds, ncmds);
free_string_array(marks, nmarks);
free_string_array(cmds_list, ncmds_list);
free_string_array(lh, nlh);
free_string_array(rh, nrh);
free(lhp);
free(rhp);
free(bt);
free(bmt);
free_string_array(cmdh, ncmdh);
free_string_array(srch, nsrch);
free_string_array(regs, nregs);
free_string_array(prompt, nprompt);
free_string_array(filter, nfilter);
free_string_array(trash, ntrash);
free_string_array(bmarks, nbmarks);
free_string_array(dir_stack, ndir_stack);
free(non_conflicting_marks);
}
static void command_find_frame_props_cache_client(void * x) {
CommandFindFrameInfo * args = (CommandFindFrameInfo *)x;
Channel * c = cache_channel();
Context * ctx = NULL;
StackTracingInfo * info = NULL;
int err = 0;
ctx = id2ctx(args->id);
if (ctx == NULL) err = ERR_INV_CONTEXT;
else if (get_stack_tracing_info(ctx, args->addr, &info) < 0) err = errno;
cache_exit();
write_stringz(&c->out, "R");
write_stringz(&c->out, args->token);
write_errno(&c->out, err);
if (args->props) {
unsigned cnt = 0;
write_stream(&c->out, '{');
if (info != NULL && info->size) {
json_write_string(&c->out, "CodeAddr");
write_stream(&c->out, ':');
json_write_uint64(&c->out, info->addr);
write_stream(&c->out, ',');
json_write_string(&c->out, "CodeSize");
write_stream(&c->out, ':');
json_write_uint64(&c->out, info->size);
cnt++;
}
if (info != NULL && info->fp != NULL) {
if (cnt++ > 0) write_stream(&c->out, ',');
json_write_string(&c->out, "FP");
write_stream(&c->out, ':');
write_commands(&c->out, ctx, info->fp->cmds, info->fp->cmds_cnt);
}
if (info != NULL && info->regs != NULL) {
int i;
if (cnt++ > 0) write_stream(&c->out, ',');
json_write_string(&c->out, "Regs");
write_stream(&c->out, ':');
write_stream(&c->out, '{');
for (i = 0; i < info->reg_cnt; i++) {
if (i > 0) write_stream(&c->out, ',');
json_write_string(&c->out, register2id(ctx, STACK_NO_FRAME, info->regs[i]->reg));
write_stream(&c->out, ':');
write_commands(&c->out, ctx, info->regs[i]->cmds, info->regs[i]->cmds_cnt);
}
write_stream(&c->out, '}');
}
if (info != NULL && info->subs != NULL) {
int i;
if (cnt++ > 0) write_stream(&c->out, ',');
json_write_string(&c->out, "Inlined");
write_stream(&c->out, ':');
write_stream(&c->out, '[');
for (i = 0; i < info->sub_cnt; i++) {
if (i > 0) write_stream(&c->out, ',');
write_inlined_subroutine_info(&c->out, info->subs[i]);
}
write_stream(&c->out, ']');
}
write_stream(&c->out, '}');
write_stream(&c->out, 0);
}
else {
/* Deprecated, use findFrameProps */
json_write_uint64(&c->out, info ? info->addr : 0);
write_stream(&c->out, 0);
json_write_uint64(&c->out, info ? info->size : 0);
write_stream(&c->out, 0);
if (info == NULL || info->fp == NULL) write_string(&c->out, "null");
else write_commands(&c->out, ctx, info->fp->cmds, info->fp->cmds_cnt);
write_stream(&c->out, 0);
if (info != NULL && info->regs != NULL) {
int i;
write_stream(&c->out, '{');
for (i = 0; i < info->reg_cnt; i++) {
if (i > 0) write_stream(&c->out, ',');
json_write_string(&c->out, register2id(ctx, STACK_NO_FRAME, info->regs[i]->reg));
write_stream(&c->out, ':');
write_commands(&c->out, ctx, info->regs[i]->cmds, info->regs[i]->cmds_cnt);
}
write_stream(&c->out, '}');
}
else {
write_string(&c->out, "null");
}
write_stream(&c->out, 0);
}
write_stream(&c->out, MARKER_EOM);
}
static void command_get_location_info_cache_client(void * x) {
CommandGetLocationInfo * args = (CommandGetLocationInfo *)x;
Channel * c = cache_channel();
LocationInfo * info = NULL;
Context * ctx = NULL;
int frame = STACK_NO_FRAME;
Symbol * sym = NULL;
int err = 0;
if (id2symbol(args->id, &sym) < 0) err = errno;
else if (get_location_info(sym, &info) < 0) err = errno;
else if (get_symbol_frame(sym, &ctx, &frame) < 0) err = errno;
cache_exit();
write_stringz(&c->out, "R");
write_stringz(&c->out, args->token);
write_errno(&c->out, err);
if (info == NULL) {
write_stringz(&c->out, "null");
}
else {
write_stream(&c->out, '{');
json_write_string(&c->out, "BigEndian");
write_stream(&c->out, ':');
json_write_boolean(&c->out, info->big_endian);
write_stream(&c->out, ',');
json_write_string(&c->out, "ValueCmds");
write_stream(&c->out, ':');
write_commands(&c->out, ctx, info->value_cmds.cmds, info->value_cmds.cnt);
if (info->args_cnt) {
write_stream(&c->out, ',');
json_write_string(&c->out, "ArgCnt");
write_stream(&c->out, ':');
json_write_ulong(&c->out, info->args_cnt);
}
if (info->code_size) {
write_stream(&c->out, ',');
json_write_string(&c->out, "CodeAddr");
write_stream(&c->out, ':');
json_write_uint64(&c->out, info->code_addr);
write_stream(&c->out, ',');
json_write_string(&c->out, "CodeSize");
write_stream(&c->out, ':');
json_write_uint64(&c->out, info->code_size);
}
if (info->discr_cnt > 0) {
unsigned i;
write_stream(&c->out, ',');
json_write_string(&c->out, "Discriminant");
write_stream(&c->out, ':');
write_stream(&c->out, '[');
for (i = 0; i < info->discr_cnt; i++) {
DiscriminantRange * r = info->discr_lst + i;
if (i > 0) write_stream(&c->out, ',');
if (r->x == r->y) {
json_write_int64(&c->out, r->x);
}
else {
write_stream(&c->out, '{');
json_write_string(&c->out, "X");
write_stream(&c->out, ':');
json_write_int64(&c->out, r->x);
write_stream(&c->out, ',');
json_write_string(&c->out, "Y");
write_stream(&c->out, ':');
json_write_int64(&c->out, r->y);
write_stream(&c->out, '}');
}
}
write_stream(&c->out, ']');
}
write_stream(&c->out, '}');
write_stream(&c->out, 0);
}
write_stream(&c->out, MARKER_EOM);
}