Esempio n. 1
0
	bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
				  cob_srvs::Trigger::Response &res )
	{
	ROS_INFO("Stopping camera axis");
	
	// stopping all arm movements
	if (CamAxis_->Stop()) {
		ROS_INFO("Stopping camera axis succesful");
		res.success = 0; // 0 = true, else = false
	}
	else {
		ROS_ERROR("Stopping camera axis not succesful. error");
	}

	return true;
	}
	//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
	void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {	
		if(isInitialized_) {	
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			GoalPos_ = traj_point_.positions[0];
			finished_ = false;
			
			// stoping axis to prepare for new trajectory
			CamAxis_->Stop();

			// 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(2000000); // needed sleep until drive starts to change status from idle to moving
			
			while (not finished_)
			{
				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_);
		
		} else {
			as_.setAborted();
			ROS_WARN("Camera_axis not initialized yet!");
		}
	}
Esempio n. 3
0
	bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
				  cob_srvs::Trigger::Response &res )
	{
	ROS_INFO("Stopping camera axis");
	
	// stopping all arm movements
	if (CamAxis_->Stop()) {
		ROS_INFO("Stopping camera axis successful");
		res.success.data = true;
		res.error_message.data = "camera axis stopped successfully";
	}
	else {
		ROS_ERROR("Stopping camera axis not succesful. error");
		res.success.data = false;
		res.error_message.data = "stopping camera axis not successful";
	}

	return true;
	}