C++ PowerCubeCtrl::MoveVel方法代码示例

		/*!
		* \brief Routine for updating command to the powercubes.
		*
		* Depending on the operation mode (position/velocity) either position or velocity goals are sent to the hardware.
		*/
		void updatePCubeCommands()
		{
			if (isInitialized_ == true)
			{
				std::string operationMode;
				n_.getParam("OperationMode", operationMode);
				if (operationMode == "position")
				{
					ROS_DEBUG("moving powercubes in position mode");
					if (PCube_->statusMoving() == false)
					{
						//feedback_.isMoving = false;
			
						ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
			
						if (traj_point_nr_ < traj_.points.size())
						{
							// if powercubes are not moving and not reached last point of trajectory, then send new target point
							ROS_DEBUG("...moving to trajectory point[%d]",traj_point_nr_);
							traj_point_ = traj_.points[traj_point_nr_];
							PCube_->MoveJointSpaceSync(traj_point_.positions);
							traj_point_nr_++;
							//feedback_.isMoving = true;
							//feedback_.pointNr = traj_point_nr;
							//as_.publishFeedback(feedback_);
						}
						else
						{
							ROS_DEBUG("...reached end of trajectory");
							finished_ = true;
						}
					}
					else
					{
						ROS_DEBUG("...powercubes still moving to point[%d]",traj_point_nr_);
					}
				}
				else if (operationMode == "velocity")
				{
					ROS_DEBUG("moving powercubes in velocity mode");
					if(newvel_)
					{	
						ROS_INFO("MoveVel Call");
						PCube_->MoveVel(cmd_vel_);
						newvel_ = false;
					}
				}
				else
				{
					ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
				}
			}
			else
			{
				ROS_DEBUG("powercubes not initialized");
			}
		}

		/*!
		* \brief Executes the callback from the actionlib.
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			finished_ = false;
			
			// stoping arm to prepare for new trajectory
			std::vector<double> VelZero;
			VelZero.resize(ModIds_param_.size());
			PCube_->MoveVel(VelZero);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
			
			while(finished_ == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		}

 // topic callback functions 
 // function will be called when a new message arrives on a topic
 void topicCallback_JointCommand(const cob3_msgs::JointCommand::ConstPtr& msg)
 {
     std::string operationMode;
     n.getParam("OperationMode", operationMode);
     if (operationMode == "position")
     {
         ROS_INFO("moving powercubes in position mode");
         //TODO PowerCubeCtrl
         PCube->MoveJointSpaceSync(msg->positions);
     }
     else if (operationMode == "velocity")
     {
     	ROS_INFO("moving powercubes in velocity mode");
         //TODO PowerCubeCtrl
         PCube->MoveVel(msg->velocities);  
     }
     else
     {
         ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
     }
 }

  /*!
   * \brief Executes the callback from the command_vel topic.
   *
   * Set the current velocity target.
   * \param msg JointVelocities
   */
  void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
  {
	  ROS_DEBUG("Received new velocity command");
	  if (!initialized_)
	  {	
			ROS_WARN("Skipping command: powercubes not initialized");
			publishState(false);
			return; 
		}
		
		if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
		{
			publishState(false);
			return; 
		}

 
	  PowerCubeCtrl::PC_CTRL_STATUS status;
	  std::vector<std::string> errorMessages;
	  pc_ctrl_->getStatus(status, errorMessages);

	  /// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates)

	  unsigned int DOF = pc_params_->GetDOF();
	  std::vector<std::string> jointNames = pc_params_->GetJointNames();
	  std::vector<double> cmd_vel(DOF);
	  std::string unit = "rad";

	  /// check dimensions
	  if (msg->velocities.size() != DOF)
	  {
		  ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
		  return;
	  }

	  /// parse velocities
	  for (unsigned int i = 0; i < DOF; i++)
	  {
		  /// check joint name
		  if (msg->velocities[i].joint_uri != jointNames[i])
		  {
			  ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
			  return;
		  }

		  /// check unit
		  if (msg->velocities[i].unit != unit)
		  {
			  ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
			  return;
		  }

		  /// if all checks are successful, parse the velocity value for this joint
		  ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
		  cmd_vel[i] = msg->velocities[i].value;
	  }
		
	  /// command velocities to powercubes
	  if (!pc_ctrl_->MoveVel(cmd_vel))
	  {
     	error_ = true;
			error_msg_ = pc_ctrl_->getErrorMessage();
		  ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
		  return;
	  }

	  ROS_DEBUG("Executed velocity command");
	 
	  publishState(false);
  }