C++ CanCtrlPltfCOb3类代码示例

		bool srvCallback_ElmoRecorderConfig(cob_srvs::ElmoRecorderConfig::Request &req,
							  cob_srvs::ElmoRecorderConfig::Response &res ){
			if(m_bisInitialized) {			
				m_CanCtrlPltf->evalCanBuffer();
				res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
				res.message = "Successfully configured all motors for instant record";
			}

			return true;
		}

		bool srvCallback_ElmoRecorderReadout(cob_srvs::ElmoRecorderReadout::Request &req,
							  cob_srvs::ElmoRecorderReadout::Response &res ){
			if(m_bisInitialized) {
				m_CanCtrlPltf->evalCanBuffer();
				res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
				if(res.success == 0) res.message = "Successfully requested reading out of Recorded data";
				else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
				else if(res.success == 2) res.message = "A previous transmission is still in progress";
			}

			return true;
		}

		bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req,
							  cob_base_drive_chain::ElmoRecorderConfig::Response &res ){
			if(m_bisInitialized) 
			{
#ifdef __SIM__
				res.success = true;
#else
				m_CanCtrlPltf->evalCanBuffer();
				res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
#endif
				res.message = "Successfully configured all motors for instant record";
			}

			return true;
		}

		// topic callback functions 
		// function will be called when a new message arrives on a topic
		void topicCallback_JointStateCmd(const sensor_msgs::JointState::ConstPtr& msg)
		{
			ROS_DEBUG("Topic Callback joint_command");
			// only process cmds when system is initialized
			if(m_bisInitialized == true)
			{
				ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
		   		int iRet;
				sensor_msgs::JointState JointStateCmd = *msg;
				// check if velocities lie inside allowed boundaries
				for(int i = 0; i < m_iNumMotors; i++)
				{
					// for steering motors
					if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
					{
						if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
						{
							JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
						}
						if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
						{
							JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
						}
					}
					// for driving motors
					else
					{
						if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
						{
							JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
						}
						if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
						{
							JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS;
						}
					}

					// and cmd velocities to Can-Nodes
					//m_CanCtrlPltf->setVelGearRadS(iCanIdent, dVelEncRadS);
					ROS_DEBUG("Send data to drives");
					iRet = m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]);
					ROS_DEBUG("Successfully sent data to drives");
					
					if(m_bPubEffort) 
						m_CanCtrlPltf->requestMotorTorque();
	  			}
			}
		}

//##################################
//#### function implementations ####
bool NodeClass::initDrives()
{
	ROS_INFO("Initializing Base Drive Chain");

	// init member vectors
	m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0);
//	m_Param.vdWheelNtrlPosRad.assign(4,0);
	// ToDo: replace the following steps by ROS configuration files
	// create Inifile class and set target inifile (from which data shall be read)
	IniFile iniFile;

	//n.param<std::string>("PltfIniLoc", sIniFileName, "Platform/IniFiles/Platform.ini");
	iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");

	// get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
	iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true);
	iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true);

#ifdef __SIM__
	// get Offset from Zero-Position of Steering
	if(m_iNumDrives >=1)
		m_Param.vdWheelNtrlPosRad[0] = 0.0f;
	if(m_iNumDrives >=2)
		m_Param.vdWheelNtrlPosRad[1] = 0.0f;
	if(m_iNumDrives >=3)
		m_Param.vdWheelNtrlPosRad[2] = 0.0f;
	if(m_iNumDrives >=4)
		m_Param.vdWheelNtrlPosRad[3] = 0.0f;
#else
	// get Offset from Zero-Position of Steering
	if(m_iNumDrives >=1)
		iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_Param.vdWheelNtrlPosRad[0], true);
	if(m_iNumDrives >=2)
		iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_Param.vdWheelNtrlPosRad[1], true);
	if(m_iNumDrives >=3)
		iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_Param.vdWheelNtrlPosRad[2], true);
	if(m_iNumDrives >=4)
		iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_Param.vdWheelNtrlPosRad[3], true);

	//Convert Degree-Value from ini-File into Radian:
	for(int i=0; i<m_iNumDrives; i++)
	{
		m_Param.vdWheelNtrlPosRad[i] = MathSup::convDegToRad(m_Param.vdWheelNtrlPosRad[i]);
	}
#endif

	// debug log
	ROS_INFO("Initializing CanCtrlItf");
	bool bTemp1;
#ifdef __SIM__
	bTemp1 = true;
#else
	bTemp1 =  m_CanCtrlPltf->initPltf();
#endif
	// debug log
	ROS_INFO("Initializing done");


	return bTemp1;
}

		// reset Can-Configuration
		bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
									 cob_srvs::Trigger::Response &res )
		{
			if(m_bisInitialized)
			{
				ROS_DEBUG("Service callback reset");
#ifdef __SIM__
				res.success.data = true;
#else
				res.success.data = m_CanCtrlPltf->resetPltf();
#endif
				if (res.success.data) {
		   			ROS_INFO("base resetted");
				} else {
					res.error_message.data = "reset of base failed";
					ROS_WARN("Resetting base failed");
				}
			}
			else
			{
				ROS_WARN("...base already recovered...");
				res.success.data = true;
				res.error_message.data = "base already recovered";
			}

			return true;
		}

		// Destructor
		~NodeClass() 
		{
#ifdef __SIM__

#else
			m_CanCtrlPltf->shutdownPltf();
#endif
		}

		bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req,
							  cob_base_drive_chain::ElmoRecorderReadout::Response &res ){
			if(m_bisInitialized) {
#ifdef __SIM__
				res.success = true;
#else
				m_CanCtrlPltf->evalCanBuffer();
				res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
#endif
				if(res.success == 0) {
					res.message = "Successfully requested reading out of Recorded data";
					m_bReadoutElmo = true;
					ROS_WARN("CPU consuming evalCanBuffer used for ElmoReadout activated");
				} else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
				else if(res.success == 2) res.message = "A previous transmission is still in progress";
			}

			return true;
		}

		// shutdown Drivers and Can-Node
		bool srvCallback_Shutdown(cob_srvs::Trigger::Request &req,
									 cob_srvs::Trigger::Response &res )
		{
			ROS_DEBUG("Service callback shutdown");
			res.success.data = m_CanCtrlPltf->shutdownPltf();
			if (res.success.data)
	   			ROS_INFO("Drives shut down");
			else
	   			ROS_INFO("Shutdown of Drives FAILED");

			return true;
		}

		// reset Can-Configuration
        bool srvCallback_Reset(cob_srvs::Trigger::Request &req,
                                     cob_srvs::Trigger::Response &res )
        {
			ROS_DEBUG("Service Callback Reset");
			res.success = m_CanCtrlPltf->resetPltf();
			if (res.success)
	   			ROS_INFO("Can-Node resetted");
			else
				res.errorMessage.data = "reset of can-nodes failed";
				ROS_INFO("Reset of Can-Node FAILED");

			return true;
		}

		// reset Can-Configuration
		bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
									 cob_srvs::Trigger::Response &res )
		{
			ROS_DEBUG("Service callback reset");
			res.success.data = m_CanCtrlPltf->resetPltf();
			if (res.success.data) {
	   			ROS_INFO("Can-Node resetted");
			} else {
				res.error_message.data = "reset of can-nodes failed";
				ROS_WARN("Reset of Can-Node FAILED");
			}

			return true;
		}

		//EXPERIMENTAL: publish JointStates cyclical instead of service callback
		bool publish_JointStates()
		{
			ROS_DEBUG("Service Callback GetJointState");
			// init local variables
			int j, k, ret_sprintf;
			bool bIsError;
			std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
			std::string str_steer, str_drive, str_num, str_cat;
			// ToDo: search for a more elegant way to compose JointNames
			char c_num [1];

			// init strings
			str_steer = "Steer";
			str_drive = "Drive";

			// set default values
			vdAngGearRad.resize(m_iNumMotors, 0);
			vdVelGearRad.resize(m_iNumMotors, 0);
			vdEffortGearNM.resize(m_iNumMotors, 0);

			// create temporary (local) JointState/Diagnostics Data-Container
			sensor_msgs::JointState jointstate;
			diagnostic_msgs::DiagnosticStatus diagnostics;
			

			//Do you have to set frame_id manually??

			// get time stamp for header
			jointstate.header.stamp = ros::Time::now();
			// set frame_id for header
			// jointstate.header.frame_id = frame_id; //Where to get this id from?

			// assign right size to JointState
			jointstate.name.resize(m_iNumMotors);
			jointstate.position.resize(m_iNumMotors);
			jointstate.velocity.resize(m_iNumMotors);
			jointstate.effort.resize(m_iNumMotors);

			if(m_bisInitialized == false)
			{
				// as long as system is not initialized
				bIsError = false;

				j = 0;
				k = 0;

				// set data to jointstate			
				for(int i = 0; i<m_iNumMotors; i++)
				{
					jointstate.position[i] = 0.0;
					jointstate.velocity[i] = 0.0;
					jointstate.effort[i] = 0.0;

/*
					// set joint names
   					if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
					{
						// create name for identification in JointState msg
						j = j+1;
						ret_sprintf = sprintf(c_num, "%i", j);
						str_num.assign(1, c_num[0]);
						str_cat = str_steer + str_num;
					}
					else
					{
						// create name for identification in JointState msg
						k = k+1;
						ret_sprintf = sprintf(c_num, "%i", k);
						str_num.assign(1, c_num[0]);
						str_cat = str_drive + str_num;
					}
					// set joint names
					jointstate.name[i] = str_cat;
*/
				}
			}
			else
			{
				// as soon as drive chain is initialized
				// read Can-Buffer
				ROS_DEBUG("Read CAN-Buffer");
				m_CanCtrlPltf->evalCanBuffer();
				ROS_DEBUG("Successfully read CAN-Buffer");
				
				j = 0;
				k = 0;
				for(int i = 0; i<m_iNumMotors; i++)
				{
					m_CanCtrlPltf->getGearPosVelRadS(i,  &vdAngGearRad[i], &vdVelGearRad[i]);
					
					//Get motor torque
					if(m_bPubEffort) {
						for(int i=0; i<m_iNumMotors; i++) {
							m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
						}
					}
					
   					// if a steering motor was read -> correct for offset
   					if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
//.........这里部分代码省略.........

		// Destructor
		~NodeClass() 
		{
			m_CanCtrlPltf->shutdownPltf();
		}

		//publish JointStates cyclical instead of service callback
		bool publish_JointStates()
		{
			ROS_DEBUG("Service Callback GetJointState");
			// init local variables
			int j, k;
			bool bIsError;
			std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;

			// set default values
			vdAngGearRad.resize(m_iNumMotors, 0);
			vdVelGearRad.resize(m_iNumMotors, 0);
			vdEffortGearNM.resize(m_iNumMotors, 0);

			// create temporary (local) JointState/Diagnostics Data-Container
			sensor_msgs::JointState jointstate;
			diagnostic_msgs::DiagnosticStatus diagnostics;
			pr2_controllers_msgs::JointTrajectoryControllerState controller_state;
			
			// get time stamp for header
			jointstate.header.stamp = ros::Time::now();
			controller_state.header.stamp = jointstate.header.stamp;

			// assign right size to JointState
			//jointstate.name.resize(m_iNumMotors);
			jointstate.position.assign(m_iNumMotors, 0.0);
			jointstate.velocity.assign(m_iNumMotors, 0.0);
			jointstate.effort.assign(m_iNumMotors, 0.0);

			if(m_bisInitialized == false)
			{
				// as long as system is not initialized
				bIsError = false;

				j = 0;
				k = 0;

				// set data to jointstate			
				for(int i = 0; i<m_iNumMotors; i++)
				{
					jointstate.position[i] = 0.0;
					jointstate.velocity[i] = 0.0;
					jointstate.effort[i] = 0.0;
				}
				jointstate.name.push_back("fl_caster_r_wheel_joint");
				jointstate.name.push_back("fl_caster_rotation_joint");
				jointstate.name.push_back("bl_caster_r_wheel_joint");
				jointstate.name.push_back("bl_caster_rotation_joint");
				jointstate.name.push_back("br_caster_r_wheel_joint");
				jointstate.name.push_back("br_caster_rotation_joint");
				jointstate.name.push_back("fr_caster_r_wheel_joint");
				jointstate.name.push_back("fr_caster_rotation_joint");
			
			}
			else
			{
				// as soon as drive chain is initialized
				// read Can-Buffer
#ifdef __SIM__

#else
				ROS_DEBUG("Read CAN-Buffer");
				m_CanCtrlPltf->evalCanBuffer();
				ROS_DEBUG("Successfully read CAN-Buffer");
#endif
				j = 0;
				k = 0;
				for(int i = 0; i<m_iNumMotors; i++)
				{
#ifdef __SIM__
					vdAngGearRad[i] = m_gazeboPos[i];
					vdVelGearRad[i] = m_gazeboVel[i];
#else
					m_CanCtrlPltf->getGearPosVelRadS(i,  &vdAngGearRad[i], &vdVelGearRad[i]);
#endif
					
					//Get motor torque
					if(m_bPubEffort) {
						for(int i=0; i<m_iNumMotors; i++) 
						{
#ifdef __SIM__
							//vdEffortGearNM[i] = m_gazeboEff[i];
#else
							m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
#endif
						}
					}



					
   					// if a steering motor was read -> correct for offset
   					if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
					{
						// correct for initial offset of steering angle (arbitrary homing position)
						vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
						MathSup::normalizePi(vdAngGearRad[i]);
						j = j+1;
					}
				}
//.........这里部分代码省略.........

		// topic callback functions 
		// function will be called when a new message arrives on a topic
		void topicCallback_JointStateCmd(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
		{
			ROS_DEBUG("Topic Callback joint_command");
			// only process cmds when system is initialized
			if(m_bisInitialized == true)
			{
				ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
				sensor_msgs::JointState JointStateCmd;
				JointStateCmd.position.resize(m_iNumMotors);
				JointStateCmd.velocity.resize(m_iNumMotors);
				JointStateCmd.effort.resize(m_iNumMotors);
				
				for(unsigned int i = 0; i < msg->joint_names.size(); i++)
				{
					// associate inputs to according steer and drive joints
					// ToDo: specify this globally (Prms-File or config-File or via msg-def.)
					// check if velocities lie inside allowed boundaries
					
					//DRIVES
					if(msg->joint_names[i] ==  "fl_caster_r_wheel_joint")
					{
							JointStateCmd.position[0] = msg->desired.positions[i];
							JointStateCmd.velocity[0] = msg->desired.velocities[i];
							//JointStateCmd.effort[0] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "bl_caster_r_wheel_joint")
					{
							JointStateCmd.position[2] = msg->desired.positions[i];
							JointStateCmd.velocity[2] = msg->desired.velocities[i];
							//JointStateCmd.effort[2] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "br_caster_r_wheel_joint")
					{
							JointStateCmd.position[4] = msg->desired.positions[i];
							JointStateCmd.velocity[4] = msg->desired.velocities[i];
							//JointStateCmd.effort[4] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "fr_caster_r_wheel_joint")
					{
							JointStateCmd.position[6] = msg->desired.positions[i];
							JointStateCmd.velocity[6] = msg->desired.velocities[i];
							//JointStateCmd.effort[6] = msg->effort[i];
					}
					//STEERS
					if(msg->joint_names[i] ==  "fl_caster_rotation_joint")
					{
							JointStateCmd.position[1] = msg->desired.positions[i];
							JointStateCmd.velocity[1] = msg->desired.velocities[i];
							//JointStateCmd.effort[1] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "bl_caster_rotation_joint")
					{ 
							JointStateCmd.position[3] = msg->desired.positions[i];
							JointStateCmd.velocity[3] = msg->desired.velocities[i];
							//JointStateCmd.effort[3] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "br_caster_rotation_joint")
					{
							JointStateCmd.position[5] = msg->desired.positions[i];
							JointStateCmd.velocity[5] = msg->desired.velocities[i];
							//JointStateCmd.effort[5] = msg->effort[i];
					}
					if(msg->joint_names[i] ==  "fr_caster_rotation_joint")
					{
							JointStateCmd.position[7] = msg->desired.positions[i];
							JointStateCmd.velocity[7] = msg->desired.velocities[i];
							//JointStateCmd.effort[7] = msg->effort[i];
					}
			
				}
				
			
				// check if velocities lie inside allowed boundaries
				for(int i = 0; i < m_iNumMotors; i++)
				{
#ifdef __SIM__
#else	
					// for steering motors
					if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
					{
						if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
						{
							JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
						}
						if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
						{
							JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
						}
					}
					// for driving motors
					else
					{
						if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
						{
							JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
						}
						if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
						{
//.........这里部分代码省略.........