void Rmp440LE::UpdateStatus()
{
UpdateImu();
UpdateOdometry();
UpdateBattery();
UpdateMotorStatus();
UpdateFaultStatus();
}
void topicCallbackJointControllerStates(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg) {
int num_joints;
int iter_k, iter_j;
std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM;
std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM;
cob_base_drive_chain::GetJointState srv_get_joint;
joint_state_odom_stamp_ = msg->header.stamp;
// copy configuration into vector classes
num_joints = msg->joint_names.size();
// drive joints
drive_joint_ang_rad.assign(m_iNumJoints, 0.0);
drive_joint_vel_rads.assign(m_iNumJoints, 0.0);
drive_joint_effort_NM.assign(m_iNumJoints, 0.0);
// steer joints
steer_joint_ang_rad.assign(m_iNumJoints, 0.0);
steer_joint_vel_rads.assign(m_iNumJoints, 0.0);
steer_joint_effort_NM.assign(m_iNumJoints, 0.0);
// init iterators
iter_k = 0;
iter_j = 0;
for(int i = 0; i < num_joints; i++)
{
// associate inputs to according steer and drive joints
// ToDo: specify this globally (Prms-File or config-File or via msg-def.)
if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
{
drive_joint_ang_rad[0] = msg->actual.positions[i];
drive_joint_vel_rads[0] = msg->actual.velocities[i];
//drive_joint_effort_NM[0] = msg->effort[i];
}
if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
{
drive_joint_ang_rad[1] = msg->actual.positions[i];
drive_joint_vel_rads[1] = msg->actual.velocities[i];
//drive_joint_effort_NM[1] = msg->effort[i];
}
if(msg->joint_names[i] == "br_caster_r_wheel_joint")
{
drive_joint_ang_rad[2] = msg->actual.positions[i];
drive_joint_vel_rads[2] = msg->actual.velocities[i];
//drive_joint_effort_NM[2] = msg->effort[i];
}
if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
{
drive_joint_ang_rad[3] = msg->actual.positions[i];
drive_joint_vel_rads[3] = msg->actual.velocities[i];
//drive_joint_effort_NM[3] = msg->effort[i];
}
if(msg->joint_names[i] == "fl_caster_rotation_joint")
{
steer_joint_ang_rad[0] = msg->actual.positions[i];
steer_joint_vel_rads[0] = msg->actual.velocities[i];
//steer_joint_effort_NM[0] = msg->effort[i];
}
if(msg->joint_names[i] == "bl_caster_rotation_joint")
{
steer_joint_ang_rad[1] = msg->actual.positions[i];
steer_joint_vel_rads[1] = msg->actual.velocities[i];
//steer_joint_effort_NM[1] = msg->effort[i];
}
if(msg->joint_names[i] == "br_caster_rotation_joint")
{
steer_joint_ang_rad[2] = msg->actual.positions[i];
steer_joint_vel_rads[2] = msg->actual.velocities[i];
//steer_joint_effort_NM[2] = msg->effort[i];
}
if(msg->joint_names[i] == "fr_caster_rotation_joint")
{
steer_joint_ang_rad[3] = msg->actual.positions[i];
steer_joint_vel_rads[3] = msg->actual.velocities[i];
//steer_joint_effort_NM[3] = msg->effort[i];
}
}
// Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic)
ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads,
drive_joint_ang_rad, steer_joint_ang_rad);
// calculate odometry every time
UpdateOdometry();
}