C++ UndercarriageCtrlGeom::GetNewCtrlStateSteerDriveSetValues方法代码示例

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

}