当前位置: 首页>>代码示例>>C++>>正文


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

本文整理汇总了C++中UndercarriageCtrlGeom::SetActualWheelValues方法的典型用法代码示例。如果您正苦于以下问题:C++ UndercarriageCtrlGeom::SetActualWheelValues方法的具体用法?C++ UndercarriageCtrlGeom::SetActualWheelValues怎么用?C++ UndercarriageCtrlGeom::SetActualWheelValues使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在UndercarriageCtrlGeom的用法示例。


在下文中一共展示了UndercarriageCtrlGeom::SetActualWheelValues方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: GetJointState

// 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);
}
开发者ID:goa-uq,项目名称:care-o-bot,代码行数:52,代码来源:cob_undercarriage_ctrl.cpp

示例2: topicCallbackJointControllerStates

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

    }
开发者ID:RomanRobotnik,项目名称:cob_driver,代码行数:87,代码来源:cob_undercarriage_ctrl.cpp


注:本文中的UndercarriageCtrlGeom::SetActualWheelValues方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。