CRCLStatus::CRCLStatus()
{
ulapi_mutex_init(&statusMutex, 1);
ulapi_mutex_give(&statusMutex);
ulapi_mutex_take(&statusMutex);
currentCmd.cmd = CRCL_NOOP;
currentCmd.status = CRCL_DONE;
currentState = CRCL_UNINITIALIZED;
cycleTime = DEFAULT_LOOP_CYCLE;
maxAccel[MOVE_DEFAULT] = 0;
maxVel[MOVE_DEFAULT] = 0;
maxAccel[MOVE_CARTESIAN] = KUKA_DEFAULT_CART_MAX_ACCEL;
maxVel[MOVE_CARTESIAN] = KUKA_DEFAULT_CART_MAX_VEL;
maxAccel[MOVE_JOINT] = KUKA_DEFAULT_JOINT_MAX_ACCEL;
maxVel[MOVE_JOINT] = KUKA_DEFAULT_JOINT_MAX_VEL;
ulapi_mutex_give(&statusMutex);
}
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;
int ws_port = WS_PORT_DEFAULT;
int emove_port = EMOVE_PORT_DEFAULT;
double period = 1;
int ws_server_id;
int ws_connection_id;
ulapi_task_struct *ws_client_read_task;
ws_client_read_task_args *ws_client_read_args;
ulapi_task_struct *ws_client_write_task;
ws_client_write_task_args *ws_client_write_args;
int emove_server_id;
ulapi_task_struct emove_server_task;
emove_server_task_args emove_server_args;
enum {INBUF_LEN = 1024};
char inbuf[INBUF_LEN];
opterr = 0;
while (true) {
option = getopt(argc, argv, ":i:n:p:t:Whd");
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 'w':
ival = atoi(optarg);
ws_port = ival;
break;
case 'e':
ival = atoi(optarg);
emove_port = ival;
break;
case 't':
dval = atof(optarg);
if (dval < FLT_EPSILON) {
fprintf(stderr, "bad value for period: %s\n", optarg);
return 1;
}
period = dval;
break;
// FIXME -- ipad
case 'W':
ws_all = true;
break;
case 'h':
print_help();
break;
case 'd':
debug = true;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
} // switch (option)
} // while (true) for getopt
if (ULAPI_OK != ulapi_init()) {
fprintf(stderr, "can't init ulapi\n");
return 1;
}
if (! inifile_name.empty()) {
if (0 != ini_load(inifile_name, &ws_port, &emove_port)) {
fprintf(stderr, "error reading ini file %s\n", inifile_name.c_str());
return 1;
}
}
ulapi_mutex_init(&ws_stat_mutex, 1);
ulapi_mutex_init(&emove_stat_mutex, 1);
// 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 wssub;
ros::Subscriber emovesub;
wssub = nh.subscribe(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN, ws_stat_callback);
emovesub = nh.subscribe(KITTING_EMOVE_STAT_TOPIC, TOPIC_QUEUE_LEN, emove_stat_callback);
/*
Run two processes, one that serves up the Workstation-level commands
and status, and the other that serves up the Emove status. This main
thread will handle the Workstation level.
*/
ws_server_id = ulapi_socket_get_server_id(ws_port);
if (ws_server_id < 0) {
fprintf(stderr, "can't serve WS port %d\n", ws_port);
return 1;
}
emove_server_id = ulapi_socket_get_server_id(emove_port);
if (emove_server_id < 0) {
fprintf(stderr, "can't serve Emove port %d\n", emove_port);
return 1;
}
ulapi_task_init(&emove_server_task);
emove_server_args.task = &emove_server_task;
emove_server_args.id = emove_server_id;
emove_server_args.period_nsecs = (int) (period * 1.0e9);
ulapi_task_start(&emove_server_task, reinterpret_cast<ulapi_task_code>(emove_server_task_code), reinterpret_cast<void *>(&emove_server_args), ulapi_prio_highest(), 0);
bool done = false;
while (! done) {
if (debug) printf("waiting for a WS HMI connection on %d...\n", ws_server_id);
ws_connection_id = ulapi_socket_get_connection_id(ws_server_id);
if (ws_connection_id < 0) {
fprintf(stderr, "can't get a WS HMI connection\n");
break;
}
if (debug) printf("got a WS HMI connection on id %d\n", ws_connection_id);
// spawn connection tasks for reading and writing
ws_client_read_task = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*ws_client_read_task)));
ws_client_read_args = reinterpret_cast<ws_client_read_task_args *>(malloc(sizeof(*ws_client_read_args)));
ulapi_task_init(ws_client_read_task);
ws_client_read_args->task = ws_client_read_task;
ws_client_read_args->id = ws_connection_id;
ulapi_task_start(ws_client_read_task, reinterpret_cast<ulapi_task_code>(ws_client_read_task_code), reinterpret_cast<void *>(ws_client_read_args), ulapi_prio_highest(), 0);
ws_client_write_task = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*ws_client_write_task)));
ws_client_write_args = reinterpret_cast<ws_client_write_task_args *>(malloc(sizeof(*ws_client_write_args)));
ulapi_task_init(ws_client_write_task);
ws_client_write_args->task = ws_client_write_task;
ws_client_write_args->id = ws_connection_id;
ws_client_write_args->period_nsecs = (int) (period * 1.0e9);
ulapi_task_start(ws_client_write_task, reinterpret_cast<ulapi_task_code>(ws_client_write_task_code), reinterpret_cast<void *>(ws_client_write_args), ulapi_prio_highest(), 0);
} // while (true)
ulapi_socket_close(ws_server_id);
if (debug) printf("server on %d done\n", ws_server_id);
return 0;
}
rtapi_result rtapi_mutex_init(rtapi_mutex_struct *mutex, rtapi_id key)
{
return ulapi_mutex_init(mutex, key);
}
int main(int argc, char *argv[])
{
int option;
int ival;
double dval;
int message_port = MESSAGE_PORT_DEFAULT;
int state_port = STATE_PORT_DEFAULT;
int request_server_id;
int state_server_id;
ulapi_task_struct request_server_thread;
ulapi_task_struct state_server_thread;
request_server_thread_args request_server_args;
state_server_thread_args state_server_args;
double period = 0.2; // ROS recommends 2 Hz at least
enum {INBUF_LEN = 1024};
char inbuf[INBUF_LEN];
opterr = 0;
while (true) {
option = getopt(argc, argv, ":m:s:t:d?");
if (option == -1) break;
switch (option) {
case 'm':
ival = atoi(optarg);
message_port = ival;
break;
case 's':
ival = atoi(optarg);
state_port = ival;
break;
case 't':
dval = atof(optarg);
period = dval;
break;
case 'd':
debug = true;
break;
case '?':
print_help();
return 0;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
} // switch (option)
} // while (true) for getopt
ulapi_mutex_init(&robot_mutex, 1);
the_robot.print_robot_status();
request_server_id = ulapi_socket_get_server_id(message_port);
if (request_server_id < 0) {
fprintf(stderr, "can't serve port %d\n", message_port);
return 1;
}
ulapi_task_init(&request_server_thread);
request_server_args.id = request_server_id;
ulapi_task_start(&request_server_thread, reinterpret_cast<ulapi_task_code>(request_server_thread_code), reinterpret_cast<void *>(&request_server_args), ulapi_prio_highest(), 0);
state_server_id = ulapi_socket_get_server_id(state_port);
if (state_server_id < 0) {
fprintf(stderr, "can't serve port %d\n", state_port);
return 1;
}
ulapi_task_init(&state_server_thread);
state_server_args.id = state_server_id;
state_server_args.period = period;
ulapi_task_start(&state_server_thread, reinterpret_cast<ulapi_task_code>(state_server_thread_code), reinterpret_cast<void *>(&state_server_args), ulapi_prio_highest(), 0);
bool done = false;
bool print_robot_info = true;
while (! done) {
char *ptr;
char *endptr;
int ival;
float fval;
int ret;
int joint;
// print prompt
printf("> ");
fflush(stdout);
// get input line
if (NULL == fgets(inbuf, sizeof(inbuf), stdin)) break;
// skip leading whitespace
ptr = inbuf;
while (isspace(*ptr)) ptr++;
// strip off trailing whitespace
endptr = inbuf + strlen(inbuf);
while ((isspace(*endptr) || 0 == *endptr) && endptr >= ptr) *endptr-- = 0;
while (endptr >= ptr) *endptr = tolower(*endptr--);
// now 'ptr' is the stripped input string
do {
if (0 == *ptr) { // blank line, print state
ulapi_mutex_take(&robot_mutex);
if (print_robot_info) {
the_robot.print_robot_info();
} else {
the_robot.print_robot_status();
}
ulapi_mutex_give(&robot_mutex);
break;
}
if ('q' == *ptr) { // quit
done = true;
break;
}
if ('d' == *ptr) { // quit
debug = ! debug;
break;
}
if ('i' == *ptr) {
print_robot_info = true;
}
if ('s' == *ptr) {
print_robot_info = false;
}
if ('?' == *ptr) { // help
print_help();
break;
}
if (! strncmp(ptr, "set", strlen("set"))) {
ptr += strlen("set");
if (! isspace(*ptr)) {
printf("need a joint and value to set\n");
break;
}
ival = strtol(ptr, &endptr, 0);
if (endptr == ptr || isgraph(*endptr)) {
printf("need a joint to set\n");
break;
}
joint = ival;
ptr = endptr;
while (isspace(*ptr)) ptr++;
if (! strncmp(ptr, "pos", strlen("pos"))) {
ptr += strlen("pos");
if (! isspace(*ptr)) {
printf("missing position value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad position value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_pos(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d position to %f\n", joint, dval);
}
break;
}
if (! strncmp(ptr, "pmin", strlen("pmin"))) {
ptr += strlen("pmin");
if (! isspace(*ptr)) {
printf("missing min value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad min value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_min(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d min value to %f\n", joint, dval);
}
break;
}
if (! strncmp(ptr, "pmax", strlen("pmax"))) {
ptr += strlen("pmax");
if (! isspace(*ptr)) {
printf("missing max value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad max value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_max(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d max value to %f\n", joint, dval);
}
break;
}
printf("need 'pos', 'pmin', or 'pmax' to set\n");
break;
} // matches "set"
printf("?\n");
} while (false); // do ... wrapper
} // while (! done)
return 0;
}
