void callback(const std_msgs::Float32MultiArray& msg)
{
/*std::vector<float> test_vector;
for (size_t i = 0; i < 8; ++i)
{
test_vector.push_back(msg.data[i]);
ROS_INFO("%f", msg.data[i]);
}
if(test_vector != goal_points_)
{
ROS_INFO("It was not the same!");
for (size_t i = 0; i < 8; ++i)
{
goal_points_[i] = msg.data[i];
ROS_INFO("%f", msg.data[i]);
}
startTrajectory(armExtensionTrajectory(), torsoExtensionTrajectory());
}
*/
for (size_t i = 0; i < 8; ++i)
{
goal_points_[i] = msg.data[i];
ROS_INFO("%f", msg.data[i]);
}
startTrajectory(armExtensionTrajectory(), torsoExtensionTrajectory());
}
bool TrajectoryDownloadHandler::internalCB(industrial::simple_message::SimpleMessage & in)
{
bool rtn = false;
SimpleMessage reply;
JointTrajPtMessage jMsg;
jMsg.init(in);
switch( jMsg.point_.getSequence() )
{
case SpecialSeqValues::START_TRAJECTORY_DOWNLOAD:
startTrajectory(jMsg);
rtn = true;
break;
case SpecialSeqValues::END_TRAJECTORY:
endTrajectory(jMsg);
rtn = true;
break;
case SpecialSeqValues::STOP_TRAJECTORY:
LOG_DEBUG("Stoping trajectory");
this->ctrl_->stopMotionJob(JOB_NAME);
rtn = true;
break;
default:
LOG_ERROR("Adding point: %d to trajectory", jMsg.point_.getSequence());
this->traj_.addPoint(jMsg.point_);
break;
}
// Send ack
if (CommTypes::SERVICE_REQUEST == in.getCommType())
{
if (rtn)
{
jMsg.toReply(reply, ReplyTypes::SUCCESS);
}
else
{
jMsg.toReply(reply, ReplyTypes::FAILURE);
}
if(this->getConnection()->sendMsg(reply))
{
LOG_INFO("Ack sent");
}
else
{
LOG_ERROR("Failed to send joint ack");
}
}
return rtn;
}
