static void ws_client_read_task_code(ws_client_read_task_args *args) { ulapi_task_struct *task; int id; enum {INBUF_SIZE = 1024}; char inbuf[INBUF_SIZE]; char *ptr; char *endptr; int nchars; ros::NodeHandle nh; ros::Publisher pub; nist_kitting::ws_cmd ws_cmd; int serial_number = 1; task = args->task; id = args->id; free(args); pub = nh.advertise<nist_kitting::ws_cmd>(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN); while (true) { nchars = ulapi_socket_read(id, inbuf, sizeof(inbuf)); if (nchars <= 0) break; if (nchars < sizeof(inbuf)) inbuf[nchars] = 0; else inbuf[sizeof(inbuf)-1] = 0; ptr = inbuf; // strip off leading and trailing whitespace while (isspace(*ptr)) ptr++; endptr = ptr + strlen(ptr); while (isspace(*endptr)) *endptr-- = 0; if (0 == *ptr) continue; ws_cmd.cmd.type = KITTING_WS_ASSEMBLE_KIT; ws_cmd.assemble_kit.name = std::string(ptr); ws_cmd.assemble_kit.quantity = 1; ws_cmd.cmd.serial_number = serial_number++; pub.publish(ws_cmd); } ulapi_socket_close(id); if (debug) printf("ws client read handler %d done\n", id); ulapi_task_delete(task); return; }
int main(int argc, char *argv[]) { double myJoints[ROBOT_DOF] = {1,2,3,4,5,6}; double jointsIn[ROBOT_DOF]; robotPose myPose, poseIn; int kukaConnection; RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE); TiXmlDocument kukaStatus; TiXmlHandle toSendHandle(&kukaStatus); TiXmlElement *cartesianStatus; TiXmlElement *cartesianUpdate; TiXmlElement *jointStatus; TiXmlElement *jointUpdate; TiXmlElement *IPOCUpdate; int nchars; TiXmlElement *cartesian; int debug = 0; int option; int counter = 0; double jointMotorScale[ROBOT_DOF], cmdMotorScale[ROBOT_DOF]; jointMotorScale[0] = 80.; jointMotorScale[1] = 100.; jointMotorScale[2] = 80.; jointMotorScale[3] = 80.; jointMotorScale[4] = 80.; jointMotorScale[5] = 40.5; cmdMotorScale[0] = 1.4; cmdMotorScale[1] = 1.74; cmdMotorScale[2] = 1.4; cmdMotorScale[3] = 1.4; cmdMotorScale[4] = 1.4; cmdMotorScale[5] = 0.87; while (true) { option = getopt(argc, argv, ":d"); if (option == -1) break; switch (option) { 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; } // switch (option) } // while (true) for getopt if( !kukaStatus.LoadFile(DEFAULT_FROM_KUKA)) { printf( "kukaRobot:: fatal error on load of %s\n", DEFAULT_FROM_KUKA); exit(1); } kukaConnection = ulapi_socket_get_client_id(KUKA_PORT, "localhost"); if( kukaConnection < 0 ) return -1; myPose.x = 0.1; myPose.y = 0.2; myPose.z = 0.3; myPose.xrot = 0.4; myPose.yrot = 0.5; myPose.zrot = 0.6; while(true) { TiXmlDocument kukaCorrections; TiXmlHandle correctionsHandle(&kukaCorrections); std::string str; enum {INBUF_LEN = 2048}; char inbuf[INBUF_LEN]; std::ostringstream s; cartesianStatus = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(1).ToElement(); IPOCUpdate = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(9). ToElement(); jointStatus = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(3).ToElement(); cartesianStatus->SetDoubleAttribute("X", myPose.x); cartesianStatus->SetDoubleAttribute ("Y", myPose.y); cartesianStatus->SetDoubleAttribute ("Z", myPose.z); cartesianStatus->SetDoubleAttribute ("A", myPose.xrot); cartesianStatus->SetDoubleAttribute ("B", myPose.yrot); cartesianStatus->SetDoubleAttribute ("C", myPose.zrot); jointStatus->SetDoubleAttribute("A1", myJoints[0]); jointStatus->SetDoubleAttribute("A2", myJoints[1]); jointStatus->SetDoubleAttribute("A3", myJoints[2]); jointStatus->SetDoubleAttribute("A4", myJoints[3]); jointStatus->SetDoubleAttribute("A5", myJoints[4]); jointStatus->SetDoubleAttribute("A6", myJoints[5]); s << counter++; s << '\0'; TiXmlText *text = new TiXmlText((s.str()).c_str()); IPOCUpdate->Clear(); IPOCUpdate->LinkEndChild(text); // kukaStatus.Print(); str << kukaStatus; ulapi_socket_write(kukaConnection, str.c_str(), str.length()); inbuf[0] = '\0'; nchars = ulapi_socket_read(kukaConnection, inbuf, sizeof(inbuf)-1); if (nchars <= 0) { printf("kukaRobot::status client disconnected\n"); break; } else { inbuf[nchars] = '\0'; } kukaCorrections.Parse(inbuf); if(debug) kukaCorrections.Print(); cartesianUpdate = correctionsHandle.FirstChild("Sen").FirstChild("Dat"). Child(1).ToElement(); jointUpdate = correctionsHandle.FirstChild("Sen").FirstChild("Dat"). Child(2).ToElement(); cartesianUpdate->QueryDoubleAttribute("X", &(poseIn.x)); cartesianUpdate->QueryDoubleAttribute("Y", &(poseIn.y)); cartesianUpdate->QueryDoubleAttribute("Z", &(poseIn.z)); cartesianUpdate->QueryDoubleAttribute("A", &(poseIn.xrot)); cartesianUpdate->QueryDoubleAttribute("B", &(poseIn.yrot)); cartesianUpdate->QueryDoubleAttribute("C", &(poseIn.zrot)); jointUpdate->QueryDoubleAttribute("A1", &(jointsIn[0])); jointUpdate->QueryDoubleAttribute("A2", &(jointsIn[1])); jointUpdate->QueryDoubleAttribute("A3", &(jointsIn[2])); jointUpdate->QueryDoubleAttribute("A4", &(jointsIn[3])); jointUpdate->QueryDoubleAttribute("A5", &(jointsIn[4])); jointUpdate->QueryDoubleAttribute("A6", &(jointsIn[5])); myPose.x += poseIn.x; myPose.y += poseIn.y; myPose.z += poseIn.z; myPose.xrot += poseIn.xrot; myPose.yrot += poseIn.yrot; myPose.zrot += poseIn.zrot; for( int i=0; i<ROBOT_DOF; i++ ) { myJoints[i] += jointsIn[i] * cmdMotorScale[i] / jointMotorScale[i]; if( debug ) printf( "J%d <%lf %lf> ", i+1, myJoints[i], jointsIn[i] ); } if(debug) printf( "\nkukaRobot Status: <%4.2f, %4.2f, %4.2f> <%4.2f, %4.2f, %4.2f>\n\n", myPose.x, myPose.y, myPose.z, myPose.xrot, myPose.yrot, myPose.zrot); cycleBlock->wait(); } }
rtapi_integer rtapi_socket_read(rtapi_integer id, char *buf, rtapi_integer len) { return ulapi_socket_read(id, buf, len); }
void client_code(void *args) { void *client_task; ulapi_integer client_id; client_db_struct *client_db_ptr; ulapi_integer debug; enum {BUFFERLEN = 256}; char inbuf[BUFFERLEN]; char outbuf[BUFFERLEN]; ulapi_integer nchars; int number; int lastnumber = 0; client_task = ((client_args *) args)->client_task; client_id = ((client_args *) args)->client_id; client_db_ptr = ((client_args *) args)->client_db_ptr; debug = ((client_args *) args)->debug; free(args); /* The client thread asks for an update every second, blocks until it gets a response, prints it, and loops again. */ for (;;) { ulapi_mutex_take(client_db_ptr->mutex); number = client_db_ptr->number; ulapi_mutex_give(client_db_ptr->mutex); if (number != lastnumber) { ulapi_snprintf(outbuf, sizeof(outbuf), "write %d", number); lastnumber = number; } else { ulapi_snprintf(outbuf, sizeof(outbuf), "read"); } ulapi_socket_write(client_id, outbuf, strlen(outbuf)+1); nchars = ulapi_socket_read(client_id, inbuf, sizeof(inbuf)-1); if (-1 == nchars) { if (debug) printf("connection closed\n"); break; } if (0 == nchars) { if (debug) printf("end of file\n"); break; } inbuf[nchars] = 0; /* Parse and handle the message here as your application requires. */ printf("%s\n", inbuf); ulapi_wait(1000000000); } ulapi_socket_close(client_id); }
int main(int argc, char *argv[]) { enum {HOST_LEN = 256}; char host[HOST_LEN] = "localhost"; int state_port = STATE_PORT_DEFAULT; int cognex_port = COGNEX_PORT_DEFAULT; const double confidence = 0.95; double period = 1; int option; int ival; double dval; int state_client_id; ulapi_task_struct cognex_client_thread; cognex_object_info_db db; ulapi_task_struct *server_task; ulapi_integer retval; opterr = 0; while (true) { option = getopt(argc, argv, ":h:s:p:t:d"); if (option == -1) break; switch (option) { case 'h': strncpy(host, optarg, sizeof(host)); host[sizeof(host)-1] = 0; break; case 's': ival = atoi(optarg); state_port = ival; break; case 'p': ival = atoi(optarg); cognex_port = ival; break; case 't': dval = atof(optarg); period = dval; 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 // set up mapping of integer ids with object names object_names[1] = std::string("small_gear"); object_names[2] = std::string("medium_gear"); object_names[3] = std::string("large_gear"); object_names[4] = std::string("bottom_cover"); object_names[5] = std::string("top_cover"); // connect to state server state_client_id = ulapi_socket_get_client_id(state_port, host); if (state_client_id < 0) { fprintf(stderr, "can't connect to %s on port %d\n", host, state_port); return 1; } server_task = db.serve(cognex_port, period * 1e9); if (server_task == NULL) { fprintf(stderr, "can't serve port %d\n", cognex_port); return 1; } while (true) { enum {INBUF_LEN = 1024}; char inbuf[INBUF_LEN]; char *ptr; int nchars; int nleft; int length; int message_type; object_state_message obj_state; nchars = ulapi_socket_read(state_client_id, inbuf, sizeof(inbuf)); if (nchars <= 0) break; inbuf[sizeof(inbuf)-1] = 0; ptr = inbuf; nleft = nchars; while (nleft > 0) { // get the length and type of the message memcpy(&length, ptr, sizeof(length)); memcpy(&message_type, ptr + sizeof(length), sizeof(message_type)); // switch on the message type and handle it switch (message_type) { case MESSAGE_ROBOT_STATUS: case MESSAGE_JOINT_POSITION: break; case MESSAGE_OBJECT_STATE: if (0 == obj_state.read_object_state(ptr)) { if (debug) obj_state.print_object_state(); for (int t = 0; t < obj_state.number; t++) { db.add(obj_state.objects[t].id, cognex_object_info(object_names[obj_state.objects[t].id].c_str(), 2*atan2(obj_state.objects[t].qz, obj_state.objects[t].qw), obj_state.objects[t].x, obj_state.objects[t].y, confidence)); } } break; default: // unknown message break; } // switch (message type) nleft -= (sizeof(length) + length); ptr += (sizeof(length) + length); } // while (nleft) } // while (true) ulapi_task_join(server_task, &retval); return 0; }
static void request_connection_thread_code(request_connection_thread_args *args) { void *thread; int id; enum {INBUF_LEN = 1024}; char inbuf[INBUF_LEN]; enum {OUTBUF_LEN = 1024}; char outbuf[OUTBUF_LEN]; char *ptr; int nchars; int nleft; int length; int message_type; ping_reply_message pingrep; joint_traj_pt_request_message jtreq; joint_traj_pt_reply_message jtrep; joint_info jinfo; cart_traj_pt_request_message ctreq; cart_traj_pt_reply_message ctrep; float f1, f2, f3, f4, f5, f6, f7; thread = args->thread; id = args->id; free(args); bool done = false; while (! done) { nchars = ulapi_socket_read(id, inbuf, sizeof(inbuf)); if (nchars <= 0) break; inbuf[sizeof(inbuf)-1] = 0; ptr = inbuf; nleft = nchars; while (nleft > 0) { // get the length and type of the message memcpy(&length, ptr, sizeof(length)); memcpy(&message_type, ptr + sizeof(length), sizeof(message_type)); // switch on the message type and handle it switch (message_type) { case MESSAGE_PING: nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&pingrep), sizeof(pingrep)); if (nchars < 0) done = true; break; case MESSAGE_JOINT_TRAJ_PT: jtreq.read_joint_traj_pt_request(ptr); if (debug) { printf("connection %d requested:\n", id); jtreq.print_joint_traj_pt_request(); } ulapi_mutex_take(&robot_mutex); for (int t = 0; t < JOINT_MAX; t++) { if (! jtreq.get_pos(&f1, t)) break; the_robot.set_robot_joint_pos(f1, t); } // FIXME -- testing with a delay, should thread this off ulapi_sleep(3); ulapi_mutex_give(&robot_mutex); jtrep.set_joint_traj_pt_reply(REPLY_SUCCESS); nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jtrep), sizeof(jtrep)); if (debug) { printf("replied to connection %d:\n", id); jtrep.print_joint_traj_pt_reply(); } if (nchars < 0) done = true; break; case MESSAGE_CART_TRAJ_PT: ctreq.read_cart_traj_pt_request(ptr); if (debug) { printf("connection %d requested:\n", id); ctreq.print_cart_traj_pt_request(); } // FIXME -- ditto ulapi_sleep(3); ulapi_mutex_take(&robot_mutex); if (! ctreq.get_pos(&f1, &f2, &f3, &f4, &f5, &f6, &f7)) break; the_robot.set_robot_cart_pos(f1, f2, f3, f4, f5, f6, f7); ulapi_mutex_give(&robot_mutex); ctrep.set_seq_number(ctreq.get_seq_number()); ctrep.set_cart_traj_pt_reply(REPLY_SUCCESS); nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&ctrep), sizeof(ctrep)); if (debug) { printf("replied to connection %d:\n", id); ctrep.print_cart_traj_pt_reply(); } if (nchars < 0) done = true; break; default: // unknown message if (debug) printf("unknown message type: %d\n", message_type); break; } // switch (message type) nleft -= (sizeof(length) + length); ptr += (sizeof(length) + length); } // while (nleft) } // while (true) ulapi_socket_close(id); if (debug) printf("simple message connection handler %d done\n", id); free(thread); return; }