static void emove_client_write_task_code(emove_client_write_task_args *args)
{
ulapi_task_struct *task;
int id;
int period_nsecs;
enum {OUTBUF_SIZE = 1024};
char outbuf[OUTBUF_SIZE];
int nchars;
nist_kitting::emove_stat emove_stat;
task = args->task;
id = args->id;
period_nsecs = args->period_nsecs;
free(args);
while (true) {
ros::spinOnce();
ulapi_mutex_take(&emove_stat_mutex);
emove_stat = emove_stat_buf;
ulapi_mutex_give(&emove_stat_mutex);
/*
uint8 type
uint8 serial_number
uint8 state
uint8 status
uint8 heartbeat
float32 period
float32 cycle
float32 duration
extern char *kitting_cmd_to_string(int s);
extern char *rcs_state_to_string(int s);
extern char *rcs_status_to_string(int s);
*/
ulapi_snprintf(outbuf, sizeof(outbuf), "%s %d %s %s %d %s %s %s\n",
kitting_cmd_to_string(emove_stat.stat.type),
emove_stat.stat.serial_number,
rcs_state_to_string(emove_stat.stat.state),
rcs_status_to_string(emove_stat.stat.status),
emove_stat.stat.heartbeat,
emove_stat.name.c_str(), emove_stat.line.c_str(), emove_stat.crcl.c_str());
outbuf[sizeof(outbuf)-1] = 0;
nchars = ulapi_socket_write(id, outbuf, strlen(outbuf));
if (nchars <= 0) break;
ulapi_wait(period_nsecs);
}
ulapi_socket_close(id);
if (debug) printf("emove client write 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_write(rtapi_integer id,
const char *buf,
rtapi_integer len)
{
return ulapi_socket_write(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[])
{
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();
}
}
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;
}
static void state_connection_thread_code(state_connection_thread_args *args)
{
void *thread;
int id;
double period;
joint_traj_pt_state_message jsmsg;
robot_status_message rsmsg;
object_state_message obj_state(3);
enum {OUTBUF_LEN = 1024};
char outbuf[OUTBUF_LEN];
int nchars, len;
int which = 0;
thread = args->thread;
id = args->id;
period = args->period;
free(args);
for (int t = 0; t < obj_state.object_number(); t++) {
obj_state.objects[t].id = t + 1;
obj_state.objects[t].x = t + 1.2;
obj_state.objects[t].y = t - 3.4;
obj_state.objects[t].z = t + 5.6;
obj_state.objects[t].qx = 0;
obj_state.objects[t].qy = 0;
obj_state.objects[t].qz = sin(0.1*t);
obj_state.objects[t].qw = cos(0.1*t);
}
while (true) {
float p;
int i;
switch (which) {
case 0:
ulapi_mutex_take(&robot_mutex);
for (int t = 0; t < JOINT_MAX; t++) {
if (the_robot.get_robot_joint_pos(&p, t)) jsmsg.set_pos(p, t);
}
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jsmsg), sizeof(jsmsg));
if (nchars < 0) break;
break;
case 1:
ulapi_mutex_take(&robot_mutex);
the_robot.get_drives_powered(&i);
rsmsg.set_drives_powered(i);
the_robot.get_e_stopped(&i);
rsmsg.set_e_stopped(i);
the_robot.get_in_error(&i);
rsmsg.set_in_error(i);
the_robot.get_in_motion(&i);
rsmsg.set_in_motion(i);
the_robot.get_mode(&i);
rsmsg.set_mode(i);
the_robot.get_motion_possible(&i);
rsmsg.set_motion_possible(i);
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&rsmsg), sizeof(rsmsg));
if (nchars < 0) break;
break;
case 2:
#undef DO_OBJECTS
#ifdef DO_OBJECTS
len = obj_state.size();
if (len <= sizeof(outbuf)) {
obj_state.write_object_state(outbuf, sizeof(outbuf));
nchars = ulapi_socket_write(id, outbuf, len);
}
#endif // DO_OBJECTS
break;
} // switch (which)
if (++which > 2) which = 0;
ulapi_wait(period * 1e9);
} // while (true)
ulapi_socket_close(id);
if (debug) printf("joint state connection handler %d done\n", id);
free(thread);
return;
}
