/*!
* \brief Publishes the state of the powercube_chain as ros messages.
*
* Published to "/joint_states" as "sensor_msgs/JointState"
* Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
*/
void publishState()
{
if (initialized_)
{
ROS_INFO("publish state");
pc_ctrl_->updateStates();
sensor_msgs::JointState joint_state_msg;
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name = pc_params_->GetJointNames();
joint_state_msg.position = pc_ctrl_->getPositions();
joint_state_msg.velocity = pc_ctrl_->getVelocities();
pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
controller_state_msg.header.stamp = joint_state_msg.header.stamp;
controller_state_msg.joint_names = pc_params_->GetJointNames();
controller_state_msg.actual.positions = pc_ctrl_->getPositions();
controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
topicPub_JointState_.publish(joint_state_msg);
topicPub_ControllerState_.publish(controller_state_msg);
last_publish_time_ = ros::Time::now();
}
}
/*!
* \brief Publishes the state of the powercube_chain as ros messages.
*
* Published to "/joint_states" as "sensor_msgs/JointState"
* Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
*/
void publishState(bool update=true)
{
if (initialized_)
{
ROS_DEBUG("publish state");
if(update)
{pc_ctrl_->updateStates();}
sensor_msgs::JointState joint_state_msg;
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name = pc_params_->GetJointNames();
joint_state_msg.position = pc_ctrl_->getPositions();
joint_state_msg.velocity = pc_ctrl_->getVelocities();
joint_state_msg.effort.resize(pc_params_->GetDOF());
pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
controller_state_msg.header.stamp = joint_state_msg.header.stamp;
controller_state_msg.joint_names = pc_params_->GetJointNames();
controller_state_msg.actual.positions = pc_ctrl_->getPositions();
controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
std_msgs::String opmode_msg;
opmode_msg.data = "velocity";
/// publishing joint and controller states on topic
topicPub_JointState_.publish(joint_state_msg);
topicPub_ControllerState_.publish(controller_state_msg);
topicPub_OperationMode_.publish(opmode_msg);
last_publish_time_ = joint_state_msg.header.stamp;
}
// check status of PowerCube chain
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
// check status of PowerCube chain
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
{
error_ = true;
}
else
{
error_ = false;
}
// publishing diagnotic messages
diagnostic_msgs::DiagnosticArray diagnostics;
diagnostics.status.resize(1);
// set data to diagnostics
if(error_)
{
diagnostics.status[0].level = 2;
diagnostics.status[0].name = n_.getNamespace();;
diagnostics.status[0].message = pc_ctrl_->getErrorMessage();
}
else
{
if (initialized_)
{
diagnostics.status[0].level = 0;
diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
diagnostics.status[0].message = "powercubechain initialized and running";
}
else
{
diagnostics.status[0].level = 1;
diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
diagnostics.status[0].message = "powercubechain not initialized";
}
}
// publish diagnostic message
topicPub_Diagnostic_.publish(diagnostics);
}