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; }