Ejemplo n.º 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;
	}
Ejemplo n.º 2
0
	void publishJointState()
	{
		if (isInitialized_ == true) {			
			// create message
			int DOF = 1;

			CamAxis_->evalCanBuffer();
			CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
			CamAxis_->m_Joint->requestPosVel();

			sensor_msgs::JointState msg;
			msg.header.stamp = ros::Time::now();
			msg.name.resize(DOF);
			msg.position.resize(DOF);
			msg.velocity.resize(DOF);
			
			msg.name[0] = JointName_;
			msg.position[0] = ActualPos_;
			msg.velocity[0] = ActualVel_;


			std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
				
			// publish message
			topicPub_JointState_.publish(msg);
		}
	}
Ejemplo n.º 3
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_))
			{
				ROS_INFO("Initializing of camera axis succesful");
					isInitialized_ = true;
				res.success = 0; // 0 = true, else = false
			}
			else
			{
				ROS_ERROR("Initializing camera axis not succesful \n");
				//res.success = 1; // 0 = true, else = false
				//res.errorMessage.data = PCube->getErrorMessage();
			}
			}
			else
			{
				ROS_ERROR("...camera axis already initialized...");			
				res.success = 1;
				res.errorMessage.data = "camera axis already initialized";
		}
		
		return true;
	}
Ejemplo n.º 4
0
	// 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");
			}
		}
Ejemplo n.º 5
0
	/*!
	* \brief Executes the service callback for set_default_vel.
	*
	* Sets the default velocity.
	* \param req Service request
	* \param res Service response
	*/
	bool srvCallback_SetDefaultVel(	cob_srvs::SetDefaultVel::Request &req,
									cob_srvs::SetDefaultVel::Response &res )
	{
		ROS_INFO("Set default velocity to [%f]", req.default_vel);
		MaxVel_ = req.default_vel;
		CamAxisParams_->SetMaxVel(MaxVel_);
		CamAxis_->setMaxVelocity(MaxVel_);
		res.success.data = true; // 0 = true, else = false
		return true;
	}
Ejemplo n.º 6
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;
	}
Ejemplo n.º 7
0
	//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!");
		}
	}
Ejemplo n.º 8
0
	bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
				  	 cob_srvs::Trigger::Response &res )
	{
		if (isInitialized_ == true) {
			ROS_INFO("Recovering camera axis");
			
			// stopping all arm movements
			if (CamAxis_->RecoverAfterEmergencyStop()) {
				ROS_INFO("Recovering camera axis succesful");
				res.success = 0; // 0 = true, else = false
			} else {
				ROS_ERROR("Recovering camera axis not succesful. error");
			}
		} else {
			ROS_ERROR("...camera axis already recovered...");			
		}

		return true;
	}
Ejemplo n.º 9
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;
	}
Ejemplo n.º 10
0
	bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
				  	 cob_srvs::Trigger::Response &res )
	{
		if (isInitialized_) {
			ROS_INFO("Recovering camera axis");
			
			// stopping all arm movements
			if (CamAxis_->RecoverAfterEmergencyStop()) {
				ROS_INFO("Recovering camera axis successfully");
				res.success.data = true;
				res.error_message.data = "camera axis successfully recovered";
			} else {
				ROS_ERROR("Recovering camera axis not successfully. error");
				res.success.data = false;
				res.error_message.data = "recovering camera axis failed";
			}
		} else {
			ROS_WARN("...camera axis already recovered...");			
			res.success.data = true;
			res.error_message.data = "camera axis already recovered";
		}

		return true;
	}
Ejemplo n.º 11
0
	void publishJointState()
	{
		if (isInitialized_ == true) {			
			// create message
			int DOF = 1;

			CamAxis_->evalCanBuffer();
			CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
			CamAxis_->m_Joint->requestPosVel();

			// really bad hack
			ActualPos_ = HomingDir_ * ActualPos_;
			ActualVel_ = HomingDir_ * ActualVel_;

			sensor_msgs::JointState msg;
			msg.header.stamp = ros::Time::now();
			msg.name.resize(DOF);
			msg.position.resize(DOF);
			msg.velocity.resize(DOF);
			msg.effort.resize(DOF);
			
			msg.name[0] = JointName_;
			msg.position[0] = ActualPos_;
			msg.velocity[0] = ActualVel_;
			msg.effort[0] = 0.0;


			//std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
				
			// publish message
			topicPub_JointState_.publish(msg);

			// publish controller state message
			pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
			controllermsg.header = msg.header;
			controllermsg.joint_names.resize(DOF);
			controllermsg.desired.positions.resize(DOF);
			controllermsg.desired.velocities.resize(DOF);
			controllermsg.actual.positions.resize(DOF);
			controllermsg.actual.velocities.resize(DOF);
			controllermsg.error.positions.resize(DOF);
			controllermsg.error.velocities.resize(DOF);
			
			controllermsg.joint_names = msg.name;
			controllermsg.desired.positions = msg.position;
			controllermsg.desired.velocities = msg.velocity;
			controllermsg.actual.positions = msg.position;
			controllermsg.actual.velocities = msg.velocity;
			// error, calculated out of desired and actual values
			for (int i = 0; i<DOF; i++ )
			{
				controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
				controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
			}
			topicPub_ControllerState_.publish(controllermsg);
		}
		// publishing diagnotic messages
	    diagnostic_msgs::DiagnosticArray diagnostics;
	    diagnostics.status.resize(1);
	    // set data to diagnostics
	    if(isError_)
	    {
	      diagnostics.status[0].level = 2;
	      diagnostics.status[0].name = "head_axis";
	      diagnostics.status[0].message = "one or more drives are in Error mode";
	    }
	    else
	    {
	      if (isInitialized_)
	      {
	        diagnostics.status[0].level = 0;
	        diagnostics.status[0].name = n_.getNamespace();
	        diagnostics.status[0].message = "head axis initialized and running";
	      }
	      else
	      {
	        diagnostics.status[0].level = 1;
	        diagnostics.status[0].name = n_.getNamespace();
	        diagnostics.status[0].message = "head axis not initialized";
	      }
	    }
	    // publish diagnostic message
	    topicPub_Diagnostic_.publish(diagnostics);
	}