00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #ifdef ROS
00032 #include "dx100/ros_conversion.h"
00033 #include "dx100/trajectory_job.h"
00034 #include "string.h"
00035 #include "simple_message/joint_traj_pt.h"
00036 #include "simple_message/shared_types.h"
00037 #include "simple_message/log_wrapper.h"
00038 #define MAX_PULSE_AXES (8)
00039 #endif
00040
00041 #ifdef MOTOPLUS
00042 #include "ros_conversion.h"
00043 #include "trajectory_job.h"
00044 #include "string.h"
00045 #include "joint_traj_pt.h"
00046 #include "shared_types.h"
00047 #include "log_wrapper.h"
00048 #include "motoPlus.h"
00049 #endif
00050
00051 using namespace industrial::shared_types;
00052 using namespace industrial::joint_traj;
00053 using namespace industrial::joint_traj_pt;
00054 using namespace industrial::joint_data;
00055 using namespace motoman::ros_conversion;
00056
00057 namespace motoman
00058 {
00059 namespace trajectory_job
00060 {
00061
00070
00071
00072 #define SAFE_STRCAT(dest, dest_max_size, src) \
00073 do \
00074 { \
00075 if (strlen(src) < dest_max_size - strlen(dest) - 1) \
00076 { \
00077 strcat(dest, src); \
00078 } \
00079 else \
00080 { \
00081 LOG_ERROR("STRCAT failed to append %s to %s", src, dest); \
00082 return false; \
00083 } \
00084 } while (0)
00085
00091 #define APPEND_LINEFEED(dest, dest_max_size) SAFE_STRCAT(dest, dest_max_size, "\r\n");
00092
00098 #define APPEND_LINE(dest, dest_max_size, src) \
00099 do \
00100 { \
00101 SAFE_STRCAT(dest, dest_max_size, src); \
00102 APPEND_LINEFEED(dest, dest_max_size); \
00103 } while (0)
00104
00105 TrajectoryJob::TrajectoryJob(void)
00106 {
00107 }
00108 TrajectoryJob::~TrajectoryJob(void)
00109 {
00110 }
00111
00112 bool TrajectoryJob::init(const char* name)
00113 {
00114
00115 bool rtn = false;
00116
00117 if( strlen(name) <= NAME_BUFFER_SIZE_ )
00118 {
00119 strcpy(this->name_, name);
00120 rtn = true;
00121 }
00122 else
00123 {
00124 LOG_ERROR("Failed to initialize trajectory job, name size %d too large",
00125 NAME_BUFFER_SIZE_);
00126 rtn = false;
00127 }
00128 return rtn;
00129
00130 }
00131
00132 bool TrajectoryJob::toJobString(JointTraj & trajectory, char* str_buffer, size_t buffer_size)
00133 {
00134 bool rtn = false;
00135 if (0 < buffer_size)
00136 {
00137
00138
00139
00140 strcpy(str_buffer, "");
00141 APPEND_LINE(str_buffer, buffer_size, "/JOB");
00142
00143 sprintf(this->line_buffer_, "//NAME %s", this->name_);
00144 APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
00145
00146 APPEND_LINE(str_buffer, buffer_size, "//POS");
00147
00148 sprintf(this->line_buffer_, "///NPOS %d,0,0,0,0,0", trajectory.size());
00149 APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
00150
00151 APPEND_LINE(str_buffer, buffer_size, "///TOOL 0");
00152 APPEND_LINE(str_buffer, buffer_size, "///POSTYPE PULSE");
00153 APPEND_LINE(str_buffer, buffer_size, "///PULSE");
00154
00155
00156
00157 JointTrajPt pt;
00158 JointData rosData;
00159 float mpRadians[MAX_PULSE_AXES];
00160 long mpPulses[MAX_PULSE_AXES];
00161
00162 for(int i = 0; i < trajectory.size(); i++)
00163 {
00164 sprintf(this->line_buffer_, "C%05d=", i);
00165 SAFE_STRCAT(str_buffer, buffer_size, this->line_buffer_);
00166 trajectory.getPoint(i, pt);
00167 pt.getJointPosition(rosData);
00168
00169
00170 toMpJoint(rosData, mpRadians);
00171 memset(mpPulses, 0, MAX_PULSE_AXES);
00172 LOG_ERROR("Failed to create Job string: Radian->Pulse scaling is not available for PC-side code.");
00173 return false;
00174
00175 for(int j = 0; j < MAX_PULSE_AXES; j++)
00176 {
00177
00178 if (j < (MAX_PULSE_AXES - 1))
00179 {
00180 sprintf(this->line_buffer_, "%d,", mpPulses[j]);
00181 SAFE_STRCAT(str_buffer, buffer_size, this->line_buffer_);
00182 }
00183 else
00184 {
00185 sprintf(this->line_buffer_, "%d", mpPulses[j]);
00186 APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
00187 }
00188 }
00189 }
00190
00191
00192
00193 APPEND_LINE(str_buffer, buffer_size, "//INST");
00194 APPEND_LINE(str_buffer, buffer_size, "///DATE 1979/10/01 00:00");
00195 APPEND_LINE(str_buffer, buffer_size, "///ATTR SC,RW");
00196 APPEND_LINE(str_buffer, buffer_size, "///GROUP1 RB1");
00197
00198
00199 APPEND_LINE(str_buffer, buffer_size, "NOP");
00200 for(int i = 0; i < trajectory.size(); i++)
00201 {
00202 trajectory.getPoint(i, pt);
00203 sprintf(this->line_buffer_, "MOVJ C%05d VJ=%.2f", i, pt.getVelocity());
00204 APPEND_LINE(str_buffer, buffer_size, this->line_buffer_);
00205 }
00206 APPEND_LINE(str_buffer, buffer_size, "END");
00207
00208 rtn = true;
00209 }
00210 else
00211 {
00212 rtn = false;
00213 LOG_ERROR("Failed to generate job string");
00214 }
00215
00216 return rtn;
00217
00218 }
00219
00220
00221 }
00222 }
00223