// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) void NodeClass::CalcCtrlStep() { double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy; std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad; sensor_msgs::JointState joint_state_cmd; int num_joints = 8; int j, k; // if controller is initialized and underlying hardware is operating normal if (is_initialized_bool_ && (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)) { // perform one control step, // get the resulting cmd's for the wheel velocities and -angles from the controller class // and output the achievable pltf velocity-cmds (if velocity limits where exceeded) ucar_ctrl_.GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy); // ToDo: adapt interface of controller class --> remove last values (not used anymore) // convert variables to SI-Units vx_cmd_ms = vx_cmd_ms/1000.0; vy_cmd_ms = vy_cmd_ms/1000.0; // compose jointcmds // compose header joint_state_cmd.header.stamp = ros::Time::now(); //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? // ToDo: configure over Config-File (number of motors) and Msg // assign right size to JointState data containers //joint_state_cmd.set_name_size(m_iNumMotors); joint_state_cmd.set_position_size(num_joints); joint_state_cmd.set_velocity_size(num_joints); joint_state_cmd.set_effort_size(num_joints); // compose data body j = 0; k = 0; for(int i = 0; i<num_joints; i++) { // for steering motors if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg { joint_state_cmd.position[i] = steer_jointang_cmds_rad[j]; joint_state_cmd.velocity[i] = steer_jointvel_cmds_rads[j]; joint_state_cmd.effort[i] = 0.0; j = j + 1; } else { joint_state_cmd.position[i] = 0.0; joint_state_cmd.velocity[i] = drive_jointvel_cmds_rads[k]; joint_state_cmd.effort[i] = 0.0; k = k + 1; } } // publish jointcmds topic_pub_joint_state_cmd_.publish(joint_state_cmd); } }
// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) void NodeClass::CalcCtrlStep() { double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy; std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad; pr2_controllers_msgs::JointTrajectoryControllerState joint_state_cmd; int j, k; iwatchdog_ += 1; // if controller is initialized and underlying hardware is operating normal if (is_initialized_bool_) //&& (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)) { // as soon as (but only as soon as) platform drive chain is initialized start to send velocity commands // Note: topicCallbackDiagnostic checks whether drives are operating nominal. // -> if warning or errors are issued target velocity is set to zero // perform one control step, // get the resulting cmd's for the wheel velocities and -angles from the controller class // and output the achievable pltf velocity-cmds (if velocity limits where exceeded) ucar_ctrl_->GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy); // ToDo: adapt interface of controller class --> remove last values (not used anymore) // if drives not operating nominal -> force commands to zero if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) { steer_jointang_cmds_rad.assign(m_iNumJoints, 0.0); steer_jointvel_cmds_rads.assign(m_iNumJoints, 0.0); } // convert variables to SI-Units vx_cmd_ms = vx_cmd_ms/1000.0; vy_cmd_ms = vy_cmd_ms/1000.0; // compose jointcmds // compose header joint_state_cmd.header.stamp = ros::Time::now(); //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? // ToDo: configure over Config-File (number of motors) and Msg // assign right size to JointState data containers //joint_state_cmd.set_name_size(m_iNumMotors); joint_state_cmd.desired.positions.resize(m_iNumJoints); joint_state_cmd.desired.velocities.resize(m_iNumJoints); //joint_state_cmd.effort.resize(m_iNumJoints); joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("br_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint"); // compose data body j = 0; k = 0; for(int i = 0; i<m_iNumJoints; i++) { if(iwatchdog_ < 50) { // for steering motors if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg { joint_state_cmd.desired.positions[i] = steer_jointang_cmds_rad[j]; joint_state_cmd.desired.velocities[i] = steer_jointvel_cmds_rads[j]; //joint_state_cmd.effort[i] = 0.0; j = j + 1; } else { joint_state_cmd.desired.positions[i] = 0.0; joint_state_cmd.desired.velocities[i] = drive_jointvel_cmds_rads[k]; //joint_state_cmd.effort[i] = 0.0; k = k + 1; } } else { joint_state_cmd.desired.positions[i] = 0.0; joint_state_cmd.desired.velocities[i] = 0.0; //joint_state_cmd.effort[i] = 0.0; } } // publish jointcmds topic_pub_controller_joint_command_.publish(joint_state_cmd); } }