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