// service callback functions
// function will be called when a service is querried
bool srvCallback_Init(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == false) {
ROS_INFO("...initializing camera axis...");
// init powercubes
if (CamAxis_->Init(CamAxisParams_))
{
CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
ROS_INFO("Initializing of camera axis succesful");
isInitialized_ = true;
res.success.data = true;
res.error_message.data = "initializing camera axis successfull";
}
else
{
ROS_ERROR("Initializing camera axis not succesful \n");
res.success.data = false;
res.error_message.data = "initializing camera axis not successfull";
}
}
else
{
ROS_WARN("...camera axis already initialized...");
res.success.data = true;
res.error_message.data = "camera axis already initialized";
}
return true;
}
// other function declarations
void updateCommands()
{
if (isInitialized_ == true)
{
if (operationMode_ == "position")
{
ROS_DEBUG("moving head_axis in position mode");
if (fabs(ActualVel_) < 0.02)
{
//feedback_.isMoving = false;
ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
if (traj_point_nr_ < traj_.points.size())
{
// if axis is not moving and not reached last point of trajectory, then send new target point
ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
traj_point_ = traj_.points[traj_point_nr_];
CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
usleep(900000);
CamAxis_->m_Joint->requestPosVel();
traj_point_nr_++;
//feedback_.isMoving = true;
//feedback_.pointNr = traj_point_nr;
//as_.publishFeedback(feedback_);
}
else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
{
ROS_DEBUG("...reached end of trajectory");
finished_ = true;
}
else
{
//do nothing until GoalPos_ is reached
}
}
else
{
ROS_INFO("...axis still moving to point[%d]",traj_point_nr_);
}
}
else if (operationMode_ == "velocity")
{
ROS_WARN("Moving in velocity mode currently disabled");
}
else
{
ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
}
}
else
{
ROS_DEBUG("axis not initialized");
}
}