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