/*!
* \brief Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
if (isInitialized_ == true)
{
// create joint_state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
PCube_->getConfig(ActualPos);
PCube_->getJointVelocities(ActualVel);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
// publish message
topicPub_JointState_.publish(msg);
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
controllermsg.joint_names.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.joint_names = JointNames_;
for (int i = 0; i<DOF; i++ )
{
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_ControllerState_.publish(controllermsg);
}
}
/*!
* \brief Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
updater_.update();
if (isInitialized_ == true)
{
// publish joint state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
lock_semaphore(can_sem);
int success = PCube_->getConfig(ActualPos);
if (!success) return;
PCube_->getJointVelocities(ActualVel);
unlock_semaphore(can_sem);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_JointState_.publish(msg);
// publish controller state message
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
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 = JointNames_;
for (int i = 0; i<DOF; i++ )
{
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
if (traj_point_.positions.size() != 0)
controllermsg.desired.positions[i] = traj_point_.positions[i];
else
controllermsg.desired.positions[i] = 0.0;
controllermsg.desired.velocities[i] = 0.0;
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[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);
}
}