示例#1
0
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();

    }