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

		// Listen for Pltf Cmds
		void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg)
		{
			double vx_cmd_mms, vy_cmd_mms, w_cmd_rads;
			
			// controller expects velocities in mm/s, ROS works with SI-Units -> convert
			// ToDo: rework Controller Class to work with SI-Units
			vx_cmd_mms = msg->linear.x*1000.0;
			vy_cmd_mms = msg->linear.y*1000.0;
			w_cmd_rads = msg->angular.z;

			// only process if controller is already initialized
			if (is_initialized_bool_)
            {
				ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]", 
                     msg->linear.x, msg->linear.y, msg->angular.z);

				// Set desired value for Plattform Velocity to UndercarriageCtrl (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
			}
			else
			{	
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
			}
		}

		// Listen for Emergency Stop
		void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg)
		{
			int EM_state;
			EM_state = msg->emergency_state;

			if (EM_state == msg->EMFREE)
			{
				// Reset EM flag in Ctrlr
				if (is_initialized_bool_) 
				{
					ucar_ctrl_.setEMStopActive(false);
					// reset only done, when system initialized
					// -> allows to stop ctrlr during init, reset and shutdown
				}
			}
			else
			{
            	ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop");

				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
		
				// Set EM flag and stop Ctrlr
				ucar_ctrl_.setEMStopActive(true);
			}
		}

		// Listens for status of underlying hardware (base drive chain)
		void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
		{
			sensor_msgs::JointState joint_state_cmd;

			// prepare joint_cmds for heartbeat (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.position.resize(m_iNumJoints);
			joint_state_cmd.velocity.resize(m_iNumJoints);            
			joint_state_cmd.effort.resize(m_iNumJoints);
			// compose jointcmds
			for(int i=0; i<m_iNumJoints; i++)
			{					
				joint_state_cmd.position[i] = 0.0;
				joint_state_cmd.velocity[i] = 0.0;
				joint_state_cmd.effort[i] = 0.0;
			}
			
			// set status of underlying drive chain to member variable 
			drive_chain_diagnostic_ = msg->level;

			// if controller is already started up ...
			if (is_initialized_bool_)
			{
				// ... but underlying drive chain is not yet operating normal
				if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
				{
					// halt controller
            		ROS_DEBUG("drive chain not availlable: halt Controller");

					// Set EM flag to Ctrlr (resets internal states)
					ucar_ctrl_->setEMStopActive(true);

					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
					ROS_DEBUG("Forced platform-velocity cmds to zero");
					
					// if is not Initializing
					if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
					{
						// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
						topic_pub_joint_state_cmd_.publish(joint_state_cmd);
					}
				}
			}
			// ... while controller is not initialized send heartbeats to keep motors alive
			else
			{
				// ... as soon as base drive chain is initialized
				if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
				{
					// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
					topic_pub_joint_state_cmd_.publish(joint_state_cmd);
				}
			}
		}

		// shutdown Controller
        bool srvCallbackShutdown(cob_srvs::Switch::Request &req,
                                     cob_srvs::Switch::Response &res )
        {
			if (is_initialized_bool_)
			{
				// stop controller
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);

				// flag that controller is not running anymore
				is_initialized_bool_ = false;
	
				res.success = true;
			}
			else
			{
				// shutdown not possible, because pltf not running
                ROS_ERROR("...platform not initialized...");
                res.success = false;
                res.errorMessage.data = "platform already or still down";
			}
	    	return true;
        }

		// Listens for status of underlying hardware (base drive chain)
		void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
		{
			// set status of underlying drive chain to member variable 
			drive_chain_diagnostic_ = msg->level;

			// if controller is already started up ...
			if (is_initialized_bool_)
			{
				// ... but underlying drive chain is not yet operating normal
				if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
				{
					// halt controller
            		ROS_DEBUG("drive chain not availlable: halt Controller");

					// Set EM flag to Ctrlr (resets internal states)
					ucar_ctrl_.setEMStopActive(true);

					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
				}
			}
		}

		// reset Controller Configuration
        bool srvCallbackReset(cob_srvs::Switch::Request &req, cob_srvs::Switch::Response &res )
        {
			bool ctrlr_reset;

			if (is_initialized_bool_)
			{

				// flag that Controller is not initialized
				is_initialized_bool_ = false;

				// first of all stop controller (similar to EMStop)
				// Set desired value for Plattform Velocity to zero (setpoint setting)
				ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
				// ToDo: last value (0.0) is not used anymore --> remove from interface
				// Set EM flag and stop Ctrlr
				ucar_ctrl_.setEMStopActive(true);

				// now re-init controller configuration
				ctrlr_reset = InitCtrl();

				if (ctrlr_reset)
				{
					// restart controller
					// Set desired value for Plattform Velocity to zero (setpoint setting)
					ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
					// ToDo: last value (0.0) is not used anymore --> remove from interface
					// Set EM flag and stop Ctrlr
					ucar_ctrl_.setEMStopActive(false);

					// reset Time
					last_time_ = ros::Time::now();
					// and reset odometry
					x_rob_m_ = 0.0;
					y_rob_m_ = 0.0;
					theta_rob_rad_ = 0.0;

					// flag that controller is initialized
					is_initialized_bool_ = true;

					// set answer for service request
	       	    	ROS_INFO("Undercarriage Controller resetted");
					res.success = true;
				}
				else
				{
					// set answer for service request
	       	    	ROS_INFO("Re-Init after Reset of Undercarriage Controller failed");
					res.success = false;
                	res.errorMessage.data = "reinit after reset of undercarriage controller failed";
				}
			}
			else
			{
				// Reset not possible, because Controller not yet set up
                ROS_ERROR("... undercarriage controller not yet initialized, reset not possible ...");
				// set answer for service request
                res.success = false;
                res.errorMessage.data = "undercarriage controller not yet initialized";
			}

		    return true;
        }