예제 #1
0
	void publishJointState()
	{
		if (isInitialized_ == true) {			
			// create message
			int DOF = 1;

			CamAxis_->evalCanBuffer();
			CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
			CamAxis_->m_Joint->requestPosVel();

			sensor_msgs::JointState msg;
			msg.header.stamp = ros::Time::now();
			msg.name.resize(DOF);
			msg.position.resize(DOF);
			msg.velocity.resize(DOF);
			
			msg.name[0] = JointName_;
			msg.position[0] = ActualPos_;
			msg.velocity[0] = ActualVel_;


			std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
				
			// publish message
			topicPub_JointState_.publish(msg);
		}
	}
예제 #2
0
	void publishJointState()
	{
		if (isInitialized_ == true) {			
			// create message
			int DOF = 1;

			CamAxis_->evalCanBuffer();
			CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
			CamAxis_->m_Joint->requestPosVel();

			// really bad hack
			ActualPos_ = HomingDir_ * ActualPos_;
			ActualVel_ = HomingDir_ * ActualVel_;

			sensor_msgs::JointState msg;
			msg.header.stamp = ros::Time::now();
			msg.name.resize(DOF);
			msg.position.resize(DOF);
			msg.velocity.resize(DOF);
			msg.effort.resize(DOF);
			
			msg.name[0] = JointName_;
			msg.position[0] = ActualPos_;
			msg.velocity[0] = ActualVel_;
			msg.effort[0] = 0.0;


			//std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
				
			// publish message
			topicPub_JointState_.publish(msg);

			// publish controller state message
			pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
			controllermsg.header = msg.header;
			controllermsg.joint_names.resize(DOF);
			controllermsg.desired.positions.resize(DOF);
			controllermsg.desired.velocities.resize(DOF);
			controllermsg.actual.positions.resize(DOF);
			controllermsg.actual.velocities.resize(DOF);
			controllermsg.error.positions.resize(DOF);
			controllermsg.error.velocities.resize(DOF);
			
			controllermsg.joint_names = msg.name;
			controllermsg.desired.positions = msg.position;
			controllermsg.desired.velocities = msg.velocity;
			controllermsg.actual.positions = msg.position;
			controllermsg.actual.velocities = msg.velocity;
			// error, calculated out of desired and actual values
			for (int i = 0; i<DOF; i++ )
			{
				controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
				controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
			}
			topicPub_ControllerState_.publish(controllermsg);
		}
		// publishing diagnotic messages
	    diagnostic_msgs::DiagnosticArray diagnostics;
	    diagnostics.status.resize(1);
	    // set data to diagnostics
	    if(isError_)
	    {
	      diagnostics.status[0].level = 2;
	      diagnostics.status[0].name = "head_axis";
	      diagnostics.status[0].message = "one or more drives are in Error mode";
	    }
	    else
	    {
	      if (isInitialized_)
	      {
	        diagnostics.status[0].level = 0;
	        diagnostics.status[0].name = n_.getNamespace();
	        diagnostics.status[0].message = "head axis initialized and running";
	      }
	      else
	      {
	        diagnostics.status[0].level = 1;
	        diagnostics.status[0].name = n_.getNamespace();
	        diagnostics.status[0].message = "head axis not initialized";
	      }
	    }
	    // publish diagnostic message
	    topicPub_Diagnostic_.publish(diagnostics);
	}