コード例 #1
0
		/*!
		* \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");
			}
		}
コード例 #2
0
 // 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());
     }
 }