void write()
{
if(!initted)
{
ROS_WARN_NAMED(APPLICATION_NAME, "hw interface was not initted yet (not able to connect to the robot), cannot send command to the robot\n");
return;
}
// write only meaningful values, if possible.
printf("out: - size of controlledJoints is %d\n", (int) controlledJoints.size());
if(controlledJoints.size() == 0)
return;
msg_clear();
for(int i=0; i<controlledJoints.size(); i++)
{
output_msg.names.push_back(controlledJoints[i].first);
output_msg.referenceType.push_back(controlledJoints[i].second);
output_msg.reference.push_back(outCmd[jointMap.at(controlledJoints[i].first)]);
printf("[%2d - %s] mode: %d; cmd: %f [%f]\n", i, controlledJoints[i].first.c_str(), output_msg.referenceType[i], output_msg.reference[i], outCmd[i]);
}
printf("\n"); fflush(stdout);
yarpRosCmd_publisher.publish(output_msg);
}
static void
set_ptn_header(menudesc *m, void *arg)
{
partinfo *p = arg;
int i;
int t;
msg_clear();
msg_table_add(MSG_edfspart, 'a' + (p - pm->bsdlabel));
/* Determine which of the properties can be changed */
for (i = PTN_MENU_START; i <= PTN_MENU_MOUNTPT; i++) {
/* Default to disabled... */
m->opts[i].opt_flags |= OPT_IGNORE;
t = p->pi_fstype;
if (i == PTN_MENU_END)
/* The 'end address' is calculated from the size */
continue;
if (t == FS_UNUSED || (t == FS_SWAP && i > PTN_MENU_END)) {
/* Nothing after 'size' can be set for swap/unused */
p->pi_flags &= ~(PIF_NEWFS | PIF_MOUNT);
p->pi_mount[0] = 0;
continue;
}
if (i == PTN_MENU_NEWFS && t != FS_BSDFFS && t != FS_BSDLFS
&& t != FS_APPLEUFS) {
/* Can only newfs UFS and LFS filesystems */
p->pi_flags &= ~PIF_NEWFS;
continue;
}
if (i >= PTN_MENU_ISIZE && i <= PTN_MENU_FSIZE) {
/* Parameters for newfs... */
if (!(p->pi_flags & PIF_NEWFS))
/* Not if we aren't going to run newfs */
continue;
if (t == FS_APPLEUFS && i != PTN_MENU_ISIZE)
/* Can only set # inodes for appleufs */
continue;
if (t == FS_BSDLFS && i != PTN_MENU_BSIZE)
/* LFS doesn't have fragments */
continue;
}
/* Ok: we want this one */
m->opts[i].opt_flags &= ~OPT_IGNORE;
}
}
int main(int argc, char *argv[])
{
int k;
struct coord c;
unsigned int i;
unsigned char turn_taken = 1, running = 0;
seed_rng();
display_init();
if(player_init() == -1) return 0;
level_init(&levels[0], 8);
player_set_level(&levels[0]);
while(1) {
main_clear();
player_see();
player_status();
turn_taken = 0;
if(!running) k = display_getch();
msg_clear();
switch(k) {
case '8':
case '9':
case '6':
case '3':
case '2':
case '1':
case '4':
case '7':
case KEY_UP:
case KEY_DOWN:
case KEY_LEFT:
case KEY_RIGHT:
c = key_to_direction(k);
if(!player_move(c.x, c.y)) running = 0;
turn_taken = 1;
break;
case 'c':
c = key_to_direction(ask_key("In which direction?"));
msg_clear();
player_close(c.x, c.y);
turn_taken = 1;
break;
case 'o':
c = key_to_direction(ask_key("In which direction?"));
msg_clear();
player_open(c.x, c.y);
turn_taken = 1;
break;
case 'k':
c = key_to_direction(ask_key("In which direction?"));
msg_clear();
player_kick(c.x, c.y);
turn_taken = 1;
break;
case 'g':
case 'G':
k = ask_key("In which direction?");
c = key_to_direction(k);
if((c.x || c.y) && player_move(c.x, c.y)) running = 1;
turn_taken = 1;
break;
case ':':
player_look();
break;
case ';':
msg_printf("Pick an object...");
c = player_select_square();
msg_clear();
player_remote_look(c.x, c.y);
break;
case '.':
turn_taken = 1;
break;
}
if(turn_taken) {
if(player_poll()) running = 0;
for(i = 0; i < levels[0].nmonst; i++)
if(monster_poll(levels[0].monsters[i]))
running = 0;
}
}
return 0;
}
void
msglooper_clear(void) {
msg_clear();
}
/*
* hook called after writing disklabel to new target disk.
*/
int
md_post_disklabel(void)
{
struct disklabel updated_label;
int fd, i, no_match;
char dev_name[100], buf[80];
const char *fst[] = {"free", "swap", " v6 ", " v7 ", "sysv", "v71k",
" v8 ", "ffs ", "dos ", "lfs ", "othr", "hpfs",
"9660", "boot", "ados", "hfs ", "fcor", "ex2f",
"ntfs", "raid", "ccd "};
snprintf(dev_name, sizeof(dev_name), "/dev/r%sc", diskdev);
/*
* Open the disk as a raw device
*/
if ((fd = open(dev_name, O_RDONLY, 0)) < 0)
return 0;
/*
* Get the "new" label to see if we were successful. If we aren't
* we'll return an error to keep from destroying the user's disk.
*/
ioctl(fd, DIOCGDINFO, &updated_label);
close(fd);
/*
* Make sure the in-core label matches the on-disk one
*/
no_match = 0;
for (i=0;i<MAXPARTITIONS;i++) {
if (i > updated_label.d_npartitions)
break;
if (bsdlabel[i].pi_size != updated_label.d_partitions[i].p_size)
no_match = 1;
if (bsdlabel[i].pi_size) {
if (bsdlabel[i].pi_offset != updated_label.d_partitions[i].p_offset)
no_match = 1;
if (bsdlabel[i].pi_fstype != updated_label.d_partitions[i].p_fstype)
no_match = 1;
}
if (no_match)
break;
}
/*
* If the labels don't match, tell the user why
*/
if (no_match) {
msg_clear();
msg_display(MSG_label_error);
msg_table_add(MSG_dump_line,
" in-core: offset size type on-disk: offset size type");
for (i=0;i<MAXPARTITIONS;i++) {
sprintf(buf, " %c:%13.8x%10.8x%5s%16.8x%10.8x%5s", i+'a',
bsdlabel[i].pi_offset, bsdlabel[i].pi_size,
fst[bsdlabel[i].pi_fstype],
updated_label.d_partitions[i].p_offset,
updated_label.d_partitions[i].p_size,
fst[updated_label.d_partitions[i].p_fstype]);
msg_table_add(MSG_dump_line, buf);
}
process_menu(MENU_ok2, NULL);
}
return no_match;
}
