Exemplo n.º 1
0
		//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
					{
						// correct for initial offset of steering angle (arbitrary homing position)
						vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
						MathSup::normalizePi(vdAngGearRad[i]);
						j = j+1;
						// create name for identification in JointState msg
/*
						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;

				}

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

			// set answer to srv request
			// res.jointstate = jointstate;

			// publish jointstate message
			topicPub_JointState.publish(jointstate);
			ROS_DEBUG("published new drive-chain configuration (JointState message)");
			

			if(m_bisInitialized)
			{
				// read Can only after initialization
				bIsError = m_CanCtrlPltf->isPltfError();
			}

			// set data to diagnostics
			if(bIsError)
			{
				diagnostics.level = 2;
				diagnostics.name = "drive-chain can node";
				diagnostics.message = "one or more drives are in Error mode";
			}
			else
			{
				if (m_bisInitialized)
				{
					diagnostics.level = 0;
					diagnostics.name = "drive-chain can node";
					diagnostics.message = "drives operating normal";
				}
				else
				{
					diagnostics.level = 1;
					diagnostics.name = "drive-chain can node";
					diagnostics.message = "drives are initializing";
				}
			}

			// publish diagnostic message
			topicPub_Diagnostic.publish(diagnostics);
			ROS_DEBUG("published new drive-chain configuration (JointState message)");

			return true;
		}
Exemplo n.º 2
0
		//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;
					}
				}

				// set data to jointstate
				for(int i = 0; i<m_iNumMotors; i++)
				{
					jointstate.position[i] = vdAngGearRad[i];
					jointstate.velocity[i] = vdVelGearRad[i];
					jointstate.effort[i] = vdEffortGearNM[i];
				}
				
				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");
				
			}

			controller_state.joint_names = jointstate.name;
			controller_state.actual.positions = jointstate.position;
			controller_state.actual.velocities = jointstate.velocity;

			// publish jointstate message
			topicPub_JointState.publish(jointstate);
			topicPub_ControllerState.publish(controller_state);
			
			ROS_DEBUG("published new drive-chain configuration (JointState message)");
			

			if(m_bisInitialized)
			{
				// read Can only after initialization
#ifdef __SIM__
				bIsError = false;
#else
				bIsError = m_CanCtrlPltf->isPltfError();
#endif
			}

			// set data to diagnostics
			if(bIsError)
			{
				diagnostics.level = 2;
				diagnostics.name = "drive-chain can node";
				diagnostics.message = "one or more drives are in Error mode";
			}
			else
			{
				if (m_bisInitialized)
				{
					diagnostics.level = 0;
					diagnostics.name = "drive-chain can node";
					diagnostics.message = "drives operating normal";
				}
				else
				{
					diagnostics.level = 1;
					diagnostics.name = "drive-chain can node";
					diagnostics.message = "drives are initializing";
				}
			}

			// publish diagnostic message
			topicPub_Diagnostic.publish(diagnostics);
			ROS_DEBUG("published new drive-chain configuration (JointState message)");

			return true;
		}