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