Ejemplo n.º 1
0
  /*!
   * \brief Publishes the state of the powercube_chain as ros messages.
   *
   * Published to "/joint_states" as "sensor_msgs/JointState"
   * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
   */
  void publishState()
  {
    if (initialized_)
    {
      ROS_INFO("publish state");

      pc_ctrl_->updateStates();

      sensor_msgs::JointState joint_state_msg;
      joint_state_msg.header.stamp = ros::Time::now();
      joint_state_msg.name = pc_params_->GetJointNames();
      joint_state_msg.position = pc_ctrl_->getPositions();
      joint_state_msg.velocity = pc_ctrl_->getVelocities();

      pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
      controller_state_msg.header.stamp = joint_state_msg.header.stamp;
      controller_state_msg.joint_names = pc_params_->GetJointNames();
      controller_state_msg.actual.positions = pc_ctrl_->getPositions();
      controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
      controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();

      topicPub_JointState_.publish(joint_state_msg);
      topicPub_ControllerState_.publish(controller_state_msg);

      last_publish_time_ = ros::Time::now();
    }
  }
  /*!
   * \brief Publishes the state of the powercube_chain as ros messages.
   *
   * Published to "/joint_states" as "sensor_msgs/JointState"
   * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
   */
  void publishState(bool update=true)
  {
	  if (initialized_)
	  {
		  ROS_DEBUG("publish state");

		  if(update)
			{pc_ctrl_->updateStates();}

		  sensor_msgs::JointState joint_state_msg;
		  joint_state_msg.header.stamp = ros::Time::now();
		  joint_state_msg.name = pc_params_->GetJointNames();
		  joint_state_msg.position = pc_ctrl_->getPositions();
		  joint_state_msg.velocity = pc_ctrl_->getVelocities();
		  joint_state_msg.effort.resize(pc_params_->GetDOF());

		  pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
		  controller_state_msg.header.stamp = joint_state_msg.header.stamp;
		  controller_state_msg.joint_names = pc_params_->GetJointNames();
		  controller_state_msg.actual.positions = pc_ctrl_->getPositions();
		  controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
		  controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();

		  std_msgs::String opmode_msg;
		  opmode_msg.data = "velocity";

		  /// publishing joint and controller states on topic
		  topicPub_JointState_.publish(joint_state_msg);
		  topicPub_ControllerState_.publish(controller_state_msg);
		  topicPub_OperationMode_.publish(opmode_msg);

		  last_publish_time_ = joint_state_msg.header.stamp;

	  }

	// check status of PowerCube chain
	if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
	
		// check status of PowerCube chain 
		if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) 
		{ 	
			error_ = true;
		} 
		else 
		{
			error_ = false;
		} 

    // publishing diagnotic messages
    diagnostic_msgs::DiagnosticArray diagnostics;
    diagnostics.status.resize(1);

    // set data to diagnostics
    if(error_)
    {
      diagnostics.status[0].level = 2;
      diagnostics.status[0].name = n_.getNamespace();;
      diagnostics.status[0].message =  pc_ctrl_->getErrorMessage();
    }
    else
    {
      if (initialized_)
      {
        diagnostics.status[0].level = 0;
        diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
        diagnostics.status[0].message = "powercubechain initialized and running";
      }
      else
      {
        diagnostics.status[0].level = 1;
        diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
        diagnostics.status[0].message = "powercubechain not initialized";
      }
    }
    // publish diagnostic message
    topicPub_Diagnostic_.publish(diagnostics);
  }