// 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;
}
void publishJointState()
{
if (isInitialized_ == true) {
// create message
int DOF = 1;
CamAxis_->evalCanBuffer();
CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
CamAxis_->m_Joint->requestPosVel();
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name[0] = JointName_;
msg.position[0] = ActualPos_;
msg.velocity[0] = ActualVel_;
std::cout << "Joint " << msg.name[0] <<": pos="<< msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
// publish message
topicPub_JointState_.publish(msg);
}
}
// 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_))
{
ROS_INFO("Initializing of camera axis succesful");
isInitialized_ = true;
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Initializing camera axis not succesful \n");
//res.success = 1; // 0 = true, else = false
//res.errorMessage.data = PCube->getErrorMessage();
}
}
else
{
ROS_ERROR("...camera axis already initialized...");
res.success = 1;
res.errorMessage.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");
}
}
/*!
* \brief Executes the service callback for set_default_vel.
*
* Sets the default velocity.
* \param req Service request
* \param res Service response
*/
bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
cob_srvs::SetDefaultVel::Response &res )
{
ROS_INFO("Set default velocity to [%f]", req.default_vel);
MaxVel_ = req.default_vel;
CamAxisParams_->SetMaxVel(MaxVel_);
CamAxis_->setMaxVelocity(MaxVel_);
res.success.data = true; // 0 = true, else = false
return true;
}
bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping camera axis");
// stopping all arm movements
if (CamAxis_->Stop()) {
ROS_INFO("Stopping camera axis succesful");
res.success = 0; // 0 = true, else = false
}
else {
ROS_ERROR("Stopping camera axis not succesful. error");
}
return true;
}
//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
if(isInitialized_) {
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
GoalPos_ = traj_point_.positions[0];
finished_ = false;
// stoping axis to prepare for new trajectory
CamAxis_->Stop();
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(2000000); // needed sleep until drive starts to change status from idle to moving
while (not finished_)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
} else {
as_.setAborted();
ROS_WARN("Camera_axis not initialized yet!");
}
}
bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == true) {
ROS_INFO("Recovering camera axis");
// stopping all arm movements
if (CamAxis_->RecoverAfterEmergencyStop()) {
ROS_INFO("Recovering camera axis succesful");
res.success = 0; // 0 = true, else = false
} else {
ROS_ERROR("Recovering camera axis not succesful. error");
}
} else {
ROS_ERROR("...camera axis already recovered...");
}
return true;
}
bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping camera axis");
// stopping all arm movements
if (CamAxis_->Stop()) {
ROS_INFO("Stopping camera axis successful");
res.success.data = true;
res.error_message.data = "camera axis stopped successfully";
}
else {
ROS_ERROR("Stopping camera axis not succesful. error");
res.success.data = false;
res.error_message.data = "stopping camera axis not successful";
}
return true;
}
bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_) {
ROS_INFO("Recovering camera axis");
// stopping all arm movements
if (CamAxis_->RecoverAfterEmergencyStop()) {
ROS_INFO("Recovering camera axis successfully");
res.success.data = true;
res.error_message.data = "camera axis successfully recovered";
} else {
ROS_ERROR("Recovering camera axis not successfully. error");
res.success.data = false;
res.error_message.data = "recovering camera axis failed";
}
} else {
ROS_WARN("...camera axis already recovered...");
res.success.data = true;
res.error_message.data = "camera axis already recovered";
}
return true;
}
void publishJointState()
{
if (isInitialized_ == true) {
// create message
int DOF = 1;
CamAxis_->evalCanBuffer();
CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
CamAxis_->m_Joint->requestPosVel();
// really bad hack
ActualPos_ = HomingDir_ * ActualPos_;
ActualVel_ = HomingDir_ * ActualVel_;
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.effort.resize(DOF);
msg.name[0] = JointName_;
msg.position[0] = ActualPos_;
msg.velocity[0] = ActualVel_;
msg.effort[0] = 0.0;
//std::cout << "Joint " << msg.name[0] <<": pos="<< msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
// publish message
topicPub_JointState_.publish(msg);
// publish controller state message
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header = msg.header;
controllermsg.joint_names.resize(DOF);
controllermsg.desired.positions.resize(DOF);
controllermsg.desired.velocities.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.error.positions.resize(DOF);
controllermsg.error.velocities.resize(DOF);
controllermsg.joint_names = msg.name;
controllermsg.desired.positions = msg.position;
controllermsg.desired.velocities = msg.velocity;
controllermsg.actual.positions = msg.position;
controllermsg.actual.velocities = msg.velocity;
// error, calculated out of desired and actual values
for (int i = 0; i<DOF; i++ )
{
controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
}
topicPub_ControllerState_.publish(controllermsg);
}
// publishing diagnotic messages
diagnostic_msgs::DiagnosticArray diagnostics;
diagnostics.status.resize(1);
// set data to diagnostics
if(isError_)
{
diagnostics.status[0].level = 2;
diagnostics.status[0].name = "head_axis";
diagnostics.status[0].message = "one or more drives are in Error mode";
}
else
{
if (isInitialized_)
{
diagnostics.status[0].level = 0;
diagnostics.status[0].name = n_.getNamespace();
diagnostics.status[0].message = "head axis initialized and running";
}
else
{
diagnostics.status[0].level = 1;
diagnostics.status[0].name = n_.getNamespace();
diagnostics.status[0].message = "head axis not initialized";
}
}
// publish diagnostic message
topicPub_Diagnostic_.publish(diagnostics);
}