bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector<double> &vec,
JointData &joints)
{
if (vec.size() > joints.getMaxNumJoints())
ROS_ERROR_RETURN(false, "Failed to copy to JointData. Len (%d) out of range (0 to %d)",
vec.size(), joints.getMaxNumJoints());
joints.init();
for (int i = 0; i < vec.size(); ++i)
{
joints.setJoint(i, vec[i]);
}
return true;
}
TEST(DISABLED_TrajectoryJob, init)
{
using namespace std;
TrajectoryJob job;
JointTraj traj;
JointTrajPt pt;
JointData init;
string job_name = "JOB_NAME";
string job_ext = ".JBI";
const int TRAJ_SIZE = 50;
const int BIG_JOB_BUFFER_SIZE = 500000;
char bigJobBuffer[BIG_JOB_BUFFER_SIZE];
const int SMALL_JOB_BUFFER_SIZE = 10;
char smallJobBuffer[SMALL_JOB_BUFFER_SIZE];
for (int i = 1; i <= TRAJ_SIZE; i++)
{
//ROS_DEBUG("Initializing joint: %d", i);
for (int j = 0; j < init.getMaxNumJoints(); j++)
{
//ROS_DEBUG("Initializing point: %d.%d", i, j);
ASSERT_TRUE(init.setJoint(j, (i*(j+1))));
}
pt.init(i, init, 11.1111, 0.01);
ASSERT_TRUE(traj.addPoint(pt));
}
EXPECT_FALSE(job.init("this name is way too long to be the name of a file and this should fail"));
ASSERT_TRUE(job.init((char*)job_name.c_str()));
EXPECT_FALSE(job.toJobString(traj, &smallJobBuffer[0], SMALL_JOB_BUFFER_SIZE));
// Test is disabled because the line below fails due to missing conversions functions. This
// ability has been moved to the controller and now is dynamically updated at runtime. There
// may not be a solution to this issue.
EXPECT_TRUE(job.toJobString(traj, &bigJobBuffer[0], BIG_JOB_BUFFER_SIZE));
ofstream file;
job_name.append(job_ext);
file.open(job_name.c_str());
ASSERT_TRUE(file.is_open());
file << bigJobBuffer;
file.close();
}