Beispiel #1
0
	// service callback functions
	// function will be called when a service is querried
	bool srvCallback_Init(cob_srvs::Trigger::Request &req,
				  cob_srvs::Trigger::Response &res )
	{
		if (isInitialized_ == false) {
			ROS_INFO("...initializing camera axis...");
			// init powercubes 
			if (CamAxis_->Init(CamAxisParams_))
			{
				CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
				ROS_INFO("Initializing of camera axis succesful");
				isInitialized_ = true;
				res.success.data = true;
				res.error_message.data = "initializing camera axis successfull";
			}
			else
			{
				ROS_ERROR("Initializing camera axis not succesful \n");
				res.success.data = false;
				res.error_message.data = "initializing camera axis not successfull";
			}
			}
			else
			{
				ROS_WARN("...camera axis already initialized...");			
				res.success.data = true;
				res.error_message.data = "camera axis already initialized";
		}
		
		return true;
	}
	// other function declarations
	void updateCommands()
		{
			if (isInitialized_ == true)
			{
				if (operationMode_ == "position")
				{
					ROS_DEBUG("moving head_axis in position mode");

					if (fabs(ActualVel_) < 0.02)
					{
						//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 axis is not moving and not reached last point of trajectory, then send new target point
							ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
							traj_point_ = traj_.points[traj_point_nr_];
							CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
							usleep(900000);
							CamAxis_->m_Joint->requestPosVel();
							traj_point_nr_++;
							//feedback_.isMoving = true;
							//feedback_.pointNr = traj_point_nr;
							//as_.publishFeedback(feedback_);
						}
						else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
						{
							ROS_DEBUG("...reached end of trajectory");
							finished_ = true;
						}
						else
						{
							//do nothing until GoalPos_ is reached
						}
					}
					else
					{
						ROS_INFO("...axis still moving to point[%d]",traj_point_nr_);
					}
				}
				else if (operationMode_ == "velocity")
				{
					ROS_WARN("Moving in velocity mode currently disabled");
				}
				else
				{
					ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
				}
			}
			else
			{
				ROS_DEBUG("axis not initialized");
			}
		}