int
skin_inf_ask (void)
{
static int tick = 0;
sw_struct sw;
printf ("inferior ask\n");
sw_init (&sw);
sw_set_name (&sw, "MyEncoder");
sw.type = SW_SEN_ENCODER;
sw.op = SW_SEN_ENCODER_SET;
sw.data.encoder.resolution = 0.001;
sw.data.encoder.mount.x = 1;
sw.data.encoder.mount.y = 2;
sw.data.encoder.mount.z = -3;
sw.data.encoder.mount.roll = -0.1;
sw.data.encoder.mount.pitch = -0.2;
sw.data.encoder.mount.yaw = 0.3;
sup_tell (&sw);
ulapi_wait (500000000);
sw.op = SW_SEN_ENCODER_STAT;
sw.time = ulapi_time ();
sw.data.encoder.tick = tick++;
sup_tell (&sw);
ulapi_wait (500000000);
return 0;
}
int main(int argc, char **argv)
{
int ros_argc;
char **ros_argv;
char node_name[NODE_NAME_LEN] = NODE_NAME_DEFAULT;
int retval;
int option;
double dval;
nist_kitting::ws_stat ws_stat_buf;
ws_stat_buf.stat.period = PERIOD_DEFAULT;
opterr = 0;
while (true) {
option = getopt(argc, argv, ":n:p:hd");
if (option == -1)
break;
switch (option) {
case 'n':
// first check for valid name
if (optarg[0] == '-') {
fprintf(stderr, "invalid node name: %s\n", optarg);
return 1;
}
strncpy(node_name, optarg, NODE_NAME_LEN-1);
node_name[NODE_NAME_LEN-1] - 0;
break;
case 'p':
dval = atof(optarg);
if (dval < FLT_EPSILON) {
fprintf(stderr, "bad value for period: %s\n", optarg);
return 1;
}
ws_stat_buf.stat.period = dval;
break;
case 'h':
print_help();
break;
case 'd':
debug = 1;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
}
}
// pass everything after a '--' separator to ROS
ros_argc = argc - optind;
ros_argv = &argv[optind];
ros::init(ros_argc, ros_argv, node_name);
ros::NodeHandle nh;
ros::Subscriber ws_cmd_sub;
ros::Subscriber task_stat_sub;
ros::Publisher ws_stat_pub;
ros::Publisher task_cmd_pub;
ros::Rate loop_rate(1.0 / ws_stat_buf.stat.period);
ws_cmd_sub = nh.subscribe(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN, ws_cmd_callback);
task_stat_sub = nh.subscribe(KITTING_TASK_STAT_TOPIC, TOPIC_QUEUE_LEN, task_stat_callback);
ws_stat_pub = nh.advertise<nist_kitting::ws_stat>(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN);
task_cmd_pub = nh.advertise<nist_kitting::task_cmd>(KITTING_TASK_CMD_TOPIC, TOPIC_QUEUE_LEN);
// stuff a NOP command
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type = KITTING_NOP;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number = 0;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
ws_stat_buf.stat.heartbeat = 0;
signal(SIGINT, quit);
double start, end, last_start = ulapi_time() - ws_stat_buf.stat.period;
while (true) {
ros::spinOnce();
start = ulapi_time();
ws_stat_buf.stat.cycle = start - last_start;
last_start = start;
if (ws_stat_buf.stat.serial_number != ws_cmd_buf.cmd.serial_number) {
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
}
switch (ws_cmd_buf.cmd.type) {
case KITTING_NOP:
do_cmd_kitting_nop(ws_stat_buf);
break;
case KITTING_INIT:
do_cmd_kitting_init(ws_stat_buf, task_cmd_pub, task_stat_buf);
break;
case KITTING_HALT:
do_cmd_halt(ws_stat_buf);
break;
case KITTING_WS_ASSEMBLE_KIT:
do_cmd_kitting_ws_assemble_kit(ws_cmd_buf.assemble_kit, ws_stat_buf);
break;
default:
// unrecognized command -- FIXME
break;
}
ws_stat_buf.stat.heartbeat++;
end = ulapi_time();
ws_stat_buf.stat.duration = ulapi_time() - start;
ws_stat_pub.publish(ws_stat_buf);
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
int ros_argc;
char **ros_argv;
std::string node_name(NODE_NAME_DEFAULT);
int option;
int ival;
double dval;
nist_kitting::ws_stat ws_stat_buf;
ws_stat_buf.stat.period = PERIOD_DEFAULT;
opterr = 0;
while (true) {
option = getopt(argc, argv, ":i:n:t:p:f:hd");
if (option == -1)
break;
switch (option) {
case 'i':
inifile_name = std::string(optarg);
break;
case 'n':
// first check for valid name
if (optarg[0] == '-') {
fprintf(stderr, "invalid node name: %s\n", optarg);
return 1;
}
node_name = std::string(optarg);
break;
case 't':
dval = atof(optarg);
if (dval < FLT_EPSILON) {
fprintf(stderr, "bad value for period: %s\n", optarg);
return 1;
}
ws_stat_buf.stat.period = dval;
break;
case 'p':
planning_app = std::string(optarg);
break;
case 'f':
plan_file = std::string(optarg);
break;
case 'h':
print_help();
break;
case 'd':
debug = 1;
ulapi_set_debug(ULAPI_DEBUG_ALL);
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
}
}
if (ULAPI_OK != ulapi_init()) {
fprintf(stderr, "can't init ulapi\n");
return 1;
}
if (inifile_name.empty()) {
fprintf(stderr, "no ini file provided\n");
return 1;
}
if (0 != ini_load(inifile_name, planning_app, plan_file)) {
fprintf(stderr, "error reading ini file %s\n", inifile_name.c_str());
return 1;
}
if (planning_app.empty()) {
fprintf(stderr, "no planning application provided\n");
return 1;
}
if (plan_file.empty()) {
fprintf(stderr, "no plan file provided\n");
return 1;
}
if (debug) {
ROS_INFO("Using planning application '%s', plan file '%s'\n", planning_app.c_str(), plan_file.c_str());
}
// pass everything after a '--' separator to ROS
ros_argc = argc - optind;
ros_argv = &argv[optind];
ros::init(ros_argc, ros_argv, node_name);
ros::NodeHandle nh;
ros::Subscriber ws_cmd_sub;
ros::Subscriber emove_stat_sub;
ros::Publisher ws_stat_pub;
ros::Publisher emove_cmd_pub;
ros::Rate loop_rate(1.0 / ws_stat_buf.stat.period);
ws_cmd_sub = nh.subscribe(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN, ws_cmd_callback);
emove_stat_sub = nh.subscribe(KITTING_EMOVE_STAT_TOPIC, TOPIC_QUEUE_LEN, emove_stat_callback);
ws_stat_pub = nh.advertise<nist_kitting::ws_stat>(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN);
emove_cmd_pub = nh.advertise<nist_kitting::emove_cmd>(KITTING_EMOVE_CMD_TOPIC, TOPIC_QUEUE_LEN);
// stuff a NOP command
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type = KITTING_NOP;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number = 0;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
ws_stat_buf.stat.heartbeat = 0;
signal(SIGINT, quit);
double start, end, last_start = ulapi_time() - ws_stat_buf.stat.period;
while (true) {
ros::spinOnce();
start = ulapi_time();
ws_stat_buf.stat.cycle = start - last_start;
last_start = start;
if (ws_stat_buf.stat.serial_number != ws_cmd_buf.cmd.serial_number) {
ws_stat_buf.stat.type = ws_cmd_buf.cmd.type;
ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number;
ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND;
ws_stat_buf.stat.status = RCS_STATUS_EXEC;
if (NULL != planning_process) {
ulapi_process_stop(planning_process);
ulapi_process_delete(planning_process);
planning_process = NULL;
}
}
switch (ws_cmd_buf.cmd.type) {
case KITTING_NOP:
do_cmd_kitting_nop(ws_stat_buf);
break;
case KITTING_INIT:
do_cmd_kitting_init(ws_stat_buf, emove_cmd_pub, emove_stat_buf);
break;
case KITTING_HALT:
do_cmd_halt(ws_stat_buf);
break;
case KITTING_WS_ASSEMBLE_KIT:
do_cmd_kitting_ws_assemble_kit(ws_cmd_buf.assemble_kit, ws_stat_buf, emove_cmd_pub, emove_stat_buf);
break;
default:
// unrecognized command -- FIXME
break;
}
ws_stat_buf.stat.heartbeat++;
end = ulapi_time();
ws_stat_buf.stat.duration = ulapi_time() - start;
ws_stat_pub.publish(ws_stat_buf);
loop_rate.sleep();
}
return 0;
}
