// acquires the current undercarriage configuration from base_drive_chain
void NodeClass::GetJointState()
{
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_srvs::GetJointState srv_get_joint;
// request update for pltf config --> call GetJointstate service
srv_client_get_joint_state_.call(srv_get_joint);
// copy configuration into vector classes
num_joints = srv_get_joint.response.jointstate.position.size();
// drive joints
drive_joint_ang_rad.assign(num_joints, 0.0);
drive_joint_vel_rads.assign(num_joints, 0.0);
drive_joint_effort_NM.assign(num_joints, 0.0);
// steer joints
steer_joint_ang_rad.assign(num_joints, 0.0);
steer_joint_vel_rads.assign(num_joints, 0.0);
steer_joint_effort_NM.assign(num_joints, 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.)
// ToDo: use joint names instead of magic integers
if( i == 1 || i == 3 || i == 5 || i == 7)
{
steer_joint_ang_rad[iter_k] = srv_get_joint.response.jointstate.position[i];
steer_joint_vel_rads[iter_k] = srv_get_joint.response.jointstate.velocity[i];
steer_joint_effort_NM[iter_k] = srv_get_joint.response.jointstate.effort[i];
iter_k = iter_k + 1;
}
else
{
drive_joint_ang_rad[iter_j] = srv_get_joint.response.jointstate.position[i];
drive_joint_vel_rads[iter_j] = srv_get_joint.response.jointstate.velocity[i];
drive_joint_effort_NM[iter_j] = srv_get_joint.response.jointstate.effort[i];
iter_j = iter_j + 1;
}
}
// 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);
}
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();
}
