int
main (int argc, char **argv)
{
ServoInf *servo; // servo level interface
UsarsimInf *usarsim; // usarsim interface
void *rosTask = NULL;
// init ros
ros::init (argc, argv, "usarsim");
// ros::Rate r(60);
servo = new ServoInf ();
usarsim = new UsarsimInf ();
// this code uses the ULAPI library to provide portability
// between different operating systems and architectures
if (ULAPI_OK != ulapi_init (UL_USE_DEFAULT))
{
ROS_FATAL ("can't initialize ulapi");
return 1;
}
// initialize the ROS interface wrapper
servo->init (usarsim);
// initialize the USARSim interface wrapper
usarsim->init (servo);
rosTask = ulapi_task_new ();
ulapi_task_start (rosTask, rosThread, (void *) servo, ulapi_prio_lowest (),
1);
// main loop
while ((usarsim->getNH ())->ok ())
{
if (usarsim->msgIn () != 1)
{
ROS_ERROR ("Error from usarsimInf, exiting");
break;
}
}
ulapi_exit ();
}
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;
}
int main(int argc, char *argv[])
{
int option;
ulapi_integer port = SAMPLE_APP_DEFAULT_PORT;
enum {BUFFERLEN = 256};
char host[BUFFERLEN] = "localhost";
ulapi_integer debug = 0;
ulapi_integer client_id;
ulapi_task_struct *client_task;
client_args *client_args_ptr;
client_db_struct client_db;
char buffer[BUFFERLEN];
char *ptr;
int number;
ulapi_opterr = 0;
for (;;) {
option = ulapi_getopt(argc, argv, ":p:h:d");
if (option == -1)
break;
switch (option) {
case 'p':
port = atoi(ulapi_optarg);
break;
case 'h':
strncpy(host, ulapi_optarg, sizeof(host));
host[sizeof(host) - 1] = 0;
break;
case 'd':
debug = 1;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", ulapi_optopt);
return 1;
break;
default: /* '?' */
fprintf(stderr, "unrecognized option -%c\n", ulapi_optopt);
return 1;
break;
}
}
if (ulapi_optind < argc) {
fprintf(stderr, "extra non-option characters: %s\n", argv[ulapi_optind]);
return 1;
}
if (ULAPI_OK != ulapi_init()) {
fprintf(stderr, "ulapi_init error\n");
return 1;
}
if (debug) ulapi_set_debug(ULAPI_DEBUG_ALL);
if (0 != sample_app_init()) {
fprintf(stderr, "can't init the sample app\n");
return 1;
}
client_id = ulapi_socket_get_client_id(port, host);
if (client_id < 0) {
fprintf(stderr, "can't connect to port %d\n", (int) port);
ulapi_exit();
return 1;
}
if (debug) {
printf("serving port %d\n", (int) port);
}
client_db.mutex = ulapi_mutex_new(0);
client_db.number = 0;
client_task = ulapi_task_new();
client_args_ptr = reinterpret_cast<client_args *>(malloc(sizeof(client_args)));
client_args_ptr->client_task = client_task;
client_args_ptr->client_id = client_id;
client_args_ptr->client_db_ptr = &client_db;
client_args_ptr->debug = debug;
ulapi_task_start(client_task, client_code, client_args_ptr, ulapi_prio_lowest(), 0);
/* enter application main loop */
while (!feof(stdin)) {
if (NULL == fgets(buffer, sizeof(buffer), stdin)) {
break;
}
ptr = buffer;
while (isspace(*ptr)) ptr++;
if ('q' == *ptr) break;
if (0 == *ptr) {
ulapi_mutex_take(client_db.mutex);
number = client_db.number;
ulapi_mutex_give(client_db.mutex);
printf("%d\n", number);
continue;
}
if (1 == sscanf(ptr, "%d", &number)) {
ulapi_mutex_take(client_db.mutex);
client_db.number = number;
ulapi_mutex_give(client_db.mutex);
continue;
}
}
ulapi_socket_close(client_id);
ulapi_exit();
sample_app_exit();
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;
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;
}
int main(int argc, char *argv[])
{
bool done;
int option;
int cmdConnection;
int script = 1;
char msgOut[buffSize];
RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE);
CRCLStatus status;
ulapi_task_struct statusTask;
RobotStatus robotStatus;
GripperStatus gripperStatus;
bool useFile = false;
char *fileName;
std::ifstream inFile;
std::istream *inputSource;
float counter = 0;
debug = 1;
while (true)
{
option = getopt(argc, argv, ":cdf:");
if (option == -1) break;
switch (option)
{
case 'd':
debug = 1;
break;
case 'c':
script = 0;
break;
case 'f':
script = 0;
useFile = true;
fileName = optarg;
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
// this code uses the ULAPI library to provide portability
// between different operating systems and architectures
if (ULAPI_OK != ulapi_init())
{
printf ("crclClient:: can't initialize ulapi");
return 1;
}
cmdConnection = ulapi_socket_get_client_id(CRCL_CMD_PORT_DEFAULT,
"localhost");
if(cmdConnection < 0)
return -1;
// start status thread
ulapi_task_init(&statusTask);
ulapi_task_start(&statusTask,
reinterpret_cast<ulapi_task_code>(statusThread),
reinterpret_cast<void*>(&status),
ulapi_prio_lowest(), 0);
if(!script)
{
if( useFile )
{
inFile.open(fileName);
inputSource = &inFile;
if( !inputSource->good() )
{
printf( "unable to open %s\n", fileName );
exit(1);
}
}
else
inputSource = &std::cin;
while(inputSource->good())
{
inputSource->getline(msgOut, buffSize);
if( msgOut[0] == ' ' || msgOut[0] == '/' )
continue;
ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut));
printf( "Sent: %s\n", msgOut);
done = false;
if(useFile)
{
for(int ii=0; ii<5; ii++)
cycleBlock->wait();
}
while( !done )
{
cycleBlock->wait();
gripperStatus = status.getGripperStatus();
robotStatus = status.getRobotStatus();
if( !useFile || status.getCurrentStatus()==CRCL_DONE )
done = true;
// printf( "Fmod: %f\n", fmod(counter, 100.));
if( fmod(counter++, 100.)==0 || done )
{
printf( "Status Cmd: %s Status: %s\n",
getCRCLCmdString((status.getCurrentCmd()).cmd).c_str(),
getCRCLStatusString(status.getCurrentStatus()).c_str());
printf( "\x1b[32mCart Status: <%3.4f, %3.4f, %3.4f> <%3.4f, %3.4f, %3.4f>\x1b[0m\n",
robotStatus.pose.x, robotStatus.pose.y,
robotStatus.pose.z,
robotStatus.pose.xrot, robotStatus.pose.yrot,
robotStatus.pose.zrot);
printf( "\x1b[32mJoint Status: <%4.2f %4.2f %4.2f %4.2f %4.2f %4.2f>\x1b[0m\n",
robotStatus.joint[0],
robotStatus.joint[1],
robotStatus.joint[2],
robotStatus.joint[3],
robotStatus.joint[4],
robotStatus.joint[5]);
}
}
}
}
for(int i=0; i<1; i++ )
{
ulapi_strncpy(msgOut, "InitCanon", strlen("InitCanon"));
ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut));
/*
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
msgOut = "MoveTo 1. 2. 3. 4. 5. 6.";
ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length());
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
msgOut = "Dwell 1";
ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length());
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
msgOut = "MoveTo 7. 8. 9. 10. 11. 12.";
ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length());
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
msgOut = "Dwell 4";
ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length());
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
msgOut = "EndCanon";
ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length());
printf( "crclClient writing: %s\n", msgOut.c_str());
sleep(2);
*/
cycleBlock->wait();
}
}
