EDELIB_NS_BEGIN
// Set 'pathname' of specified menuitem
// If finditem==NULL, mvalue() is used (the most recently picked menuitem)
// Returns:
// 0 : OK
// -1 : item not found (name="")
// -2 : 'name' not large enough (name="")
//
#define SAFE_STRCAT(s) \
{ len += strlen(s); if ( len >= namelen ) { *name='\0'; return(-2); } else strcat(name,(s)); }
int MenuBase::item_pathname(char *name, int namelen, const MenuItem *finditem) const {
int len = 0;
finditem = finditem ? finditem : mvalue();
name[0] = '\0';
for ( int t=0; t<size(); t++ ) {
const MenuItem *m = &(menu()[t]);
if ( m->submenu() ) { // submenu? descend
if (*name) SAFE_STRCAT("/");
if (m->label()) SAFE_STRCAT(m->label());
} else {
if (m->label()) { // menu item?
if ( m == finditem ) { // found? tack on itemname, done.
SAFE_STRCAT("/");
SAFE_STRCAT(m->label());
return(0);
}
} else { // end of submenu? pop
char *ss = strrchr(name, '/');
if ( ss ) { *ss = 0; len = strlen(name); } // "File/Edit" -> "File"
else { name[0] = '\0'; len = 0; } // "File" -> ""
continue;
}
}
}
*name = '\0';
return(-1); // item not found
}
bool TrajectoryJob::toJobString(JointTraj & trajectory, char* str_buffer, size_t buffer_size)
{
bool rtn = false;
if (0 < buffer_size)
{
// Header begin
// ----------------------------------------------------------------------------
strcpy(str_buffer, "");
APPEND_LINE(str_buffer, buffer_size, "/JOB");
sprintf(this->line_buffer_, "//NAME %s", this->name_);
APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
APPEND_LINE(str_buffer, buffer_size, "//POS");
sprintf(this->line_buffer_, "///NPOS %d,0,0,0,0,0", trajectory.size());
APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
APPEND_LINE(str_buffer, buffer_size, "///TOOL 0");
APPEND_LINE(str_buffer, buffer_size, "///POSTYPE PULSE");
APPEND_LINE(str_buffer, buffer_size, "///PULSE");
// Point declaration and initialization
// ----------------------------------------------------------------------------
JointTrajPt pt;
JointData rosData;
float mpRadians[MAX_PULSE_AXES];
long mpPulses[MAX_PULSE_AXES];
for(int i = 0; i < trajectory.size(); i++)
{
sprintf(this->line_buffer_, "C%05d=", i);
SAFE_STRCAT(str_buffer, buffer_size, this->line_buffer_);
trajectory.getPoint(i, pt);
pt.getJointPosition(rosData);
// Converting to a motoplus joint (i.e. the correct order and units
toMpJoint(rosData, mpRadians);
memset(mpPulses, 0, MAX_PULSE_AXES); // can't convert radian->pulse on PC-side
LOG_ERROR("Failed to create Job string: Radian->Pulse scaling is not available for PC-side code.");
return false;
for(int j = 0; j < MAX_PULSE_AXES; j++)
{
// Don't append comma to last position, instead line-feed.
if (j < (MAX_PULSE_AXES - 1))
{
sprintf(this->line_buffer_, "%d,", mpPulses[j]);
SAFE_STRCAT(str_buffer, buffer_size, this->line_buffer_);
}
else
{
sprintf(this->line_buffer_, "%d", mpPulses[j]);
APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
}
}
}
// Header end
// ----------------------------------------------------------------------------
APPEND_LINE(str_buffer, buffer_size, "//INST");
APPEND_LINE(str_buffer, buffer_size, "///DATE 1979/10/01 00:00");
APPEND_LINE(str_buffer, buffer_size, "///ATTR SC,RW");
APPEND_LINE(str_buffer, buffer_size, "///GROUP1 RB1");
// Program
APPEND_LINE(str_buffer, buffer_size, "NOP");
for(int i = 0; i < trajectory.size(); i++)
{
trajectory.getPoint(i, pt);
sprintf(this->line_buffer_, "MOVJ C%05d VJ=%.2f", i, pt.getVelocity());
APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
}
APPEND_LINE(str_buffer, buffer_size, "END");
rtn = true;
}
else
{
rtn = false;
LOG_ERROR("Failed to generate job string");
}
return rtn;
}
