//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <Action_Server::gazebo.action> customizes the simple action server to use our own "action" message 
// defined in our package, "Action_Server", in the subdirectory "action", called "Action.action"
// The name "Action" is prepended to other message types created automatically during compilation.
// e.g.,  "gazebo.action" is auto-generated from (our) base name "Action" and generic name "Action"
  void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<Action_Server::gazebo.action>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB");
    ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes
    ros::Rate timer(1.0); // 1Hz timer
    countdown_val_ = goal->input;
    //implement a simple timer, which counts down from provided countdown_val to 0, in seconds
    while (countdown_val_>0) {
     ROS_INFO("countdown = %d",countdown_val_);
     
       // each iteration, check if cancellation has been ordered
     if (as_.isPreemptRequested()){	
      ROS_WARN("goal cancelled!");
      result_.output = countdown_val_;
          as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
          return; // done with callback
        }
        
 	   //if here, then goal is still valid; provide some feedback
 	   feedback_.fdbk = countdown_val_; // populate feedback message with current countdown value
 	   as_.publishFeedback(feedback_); // send feedback to the action client that requested this goal
       countdown_val_--; //decrement the timer countdown
       timer.sleep(); //wait 1 sec between loop iterations of this timer
     }
    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.output = countdown_val_; //value should be zero, if completed countdown
    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
  }
	void executeCB(const tilting_servo::servoGoalConstPtr &goal)
	{
		if(start == true)
		{
			servo_move(Comport, servo_id, goal->angle, goal->speed);
			prev_goal = goal->angle;
			start = false;
		}
		if(as_.isNewGoalAvailable())
			as_.acceptNewGoal();
		if(goal->angle != prev_goal || goal->speed != prev_speed)
		{
		servo_move(Comport, servo_id, goal->angle, goal->speed);
		prev_goal = goal->angle;
		prev_speed = goal->speed;
		}				
		while((as_.isNewGoalAvailable() == false) && ros::ok())
		{	
			feedback_.angle = read_angle(Comport, servo_id);
			if(feedback_.angle >= min_angle - 10 && feedback_.angle <= max_angle + 10)			
			as_.publishFeedback(feedback_);
		}
		
		// check that preempt has not been requested by the client
		if (as_.isPreemptRequested() || !ros::ok())
		{
			// set the action state to preempted
			as_.setPreempted();
		}


	}
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");
    ROS_INFO("object code is: %d", goal->object_code);
    ROS_INFO("perception_source is: %d", goal->perception_source);
    g_status_code = 0; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    g_action_code = 1; //start with perceptual processing
    //do work here: this is where your interesting code goes
    while ((g_status_code != SUCCESS)&&(g_status_code != ABORTED)) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
        feedback_.feedback_status = g_status_code;
        as_.publishFeedback(feedback_);
        //ros::Duration(0.1).sleep();
        ROS_INFO("executeCB: g_status_code = %d",g_status_code);
        // each iteration, check if cancellation has been ordered

        if (as_.isPreemptRequested()) {
            ROS_WARN("goal cancelled!");
            result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
            g_action_code = 0;
            g_status_code = 0;
            as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
            return; // done with callback
        }
        //here is where we step through states:
        switch (g_action_code) {

            case 1:  
                ROS_INFO("starting new task; should call object finder");
                g_status_code = 1; //
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 2;                
                break;

            case 2: // also do nothing...but maybe comment on status? set a watchdog?
                g_status_code = 2;
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 3;
                break;

            case 3:
                g_status_code = SUCCESS; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                g_action_code = 0; // back to waiting state--regardless
                break;
            default:
                ROS_WARN("executeCB: error--case not recognized");
                break;
        }        
    ros::Duration(0.5).sleep();    
        
    }
    ROS_INFO("executeCB: manip success; returning success");
        //if we survive to here, then the goal was successfully accomplished; inform the client
        result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
        as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
            g_action_code = 0;
            g_status_code = 0;
    return;
}
      void setJointsCB( const staubliTX60::SetJointsGoalConstPtr &goal ) {
	 //staubli.ResetMotion();
	 ros::Rate rate(10);
	 bool success = true;
	 ROS_INFO("Set Joints Action Cmd received \n");
	 if( staubli.MoveJoints(goal->j,
				goal->params.movementType,
				goal->params.jointVelocity,
				goal->params.jointAcc,
				goal->params.jointDec,
				goal->params.endEffectorMaxTranslationVel, 
				goal->params.endEffectorMaxRotationalVel,
				goal->params.distBlendPrev,
				goal->params.distBlendNext
				) )
	   {
	    ROS_INFO("Cmd received, moving to desired joint angles.");
	    while(true){
	       if (as_.isPreemptRequested() || !ros::ok()) {
		  ROS_INFO("%s: Preempted", action_name_.c_str());
		  // set the action state to preempted
		  staubli.ResetMotion();
		  as_.setPreempted();
		  success = false;
		  break;
	       }
	       if( polling(goal->j) ) break;
	       rate.sleep();
	    }
	    if(success) as_.setSucceeded(result_);
	 }else {
	    as_.setAborted();
	    ROS_ERROR("Cannot move to specified joints' configuration.");
	 }
      }
    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
    {
        // publish info to the console for the user
        ROS_INFO("Starting Action");

        // start executing the action
        int i = 0;
        while (i < 5)
        {
            // check that preempt has not been requested by the client
            if (as_.isPreemptRequested())
            {
                ROS_INFO("Action Halted");

                // set the action state to preempted
                as_.setPreempted();
                break;
            }
            ROS_INFO("Executing Action");

            ros::Duration(0.5).sleep();  // waiting for 0.5 seconds
            i++;
        }

        if (i == 5)
        {
            set_status(SUCCESS);
        }
    }
  void executeCB(const speedydug_servers::SpinBucketGoalConstPtr &goal)
  {
	  // helper variables
	  bucket_detected_ = false;
      ir_detected_ = false;
      feedback_.success = false;
	  float z;
	  ros::Duration d(0.05);

	  if( goal->left ){
		  z = ANG_VEL;
	  }
	  else {
		  z = -1.0 * ANG_VEL;
      }
	  ROS_INFO(goal->left ? "true" : "false");
	  ROS_INFO("z = %f", z);

	  while(!bucket_detected_ && !ir_detected_ )
	  {
		  // check that preempt has not been requested by the client
		  if (as_.isPreemptRequested() || !ros::ok())
		  {
			  twist_.angular.z = 0;
			  twist_.linear.x = 0;
			  twist_pub_.publish(twist_);
			  ROS_INFO("%s: Preempted", action_name_.c_str());
			  // set the action state to preempted
			  as_.setPreempted();
			  break;
		  }

		  twist_.angular.z = z;
		  twist_.linear.x = 0;
		  twist_pub_.publish(twist_);
		  as_.publishFeedback(feedback_);
		  d.sleep();
	  }

      if(bucket_detected_ )
      {
          twist_.angular.z = 0;
          twist_.linear.x = 0;
          twist_pub_.publish(twist_);
          result_.success = true;
          ROS_INFO("%s: Succeeded", action_name_.c_str());
          // set the action state to succeeded
          as_.setSucceeded(result_);
      } else if(ir_detected_) {
          twist_.angular.z = 0;
          twist_.linear.x = 0;
          twist_pub_.publish(twist_);
          result_.success = false;
          ROS_INFO("%s: Succeeded", action_name_.c_str());
          // set the action state to succeeded
          as_.setSucceeded(result_);
      }
  }
Example #7
0
File: node.cpp Project: ErolB/Sub8
  void timer_callback(const ros::TimerEvent &) {
    if (!c3trajectory) return;

    ros::Time now = ros::Time::now();

    if (actionserver.isPreemptRequested()) {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
    }

    if (actionserver.isNewGoalAvailable()) {
      boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal();
      current_waypoint = subjugator::C3Trajectory::Waypoint(
          Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed,
          !goal->uncoordinated);
      current_waypoint_t = now;  // goal->header.stamp;
      this->linear_tolerance = goal->linear_tolerance;
      this->angular_tolerance = goal->angular_tolerance;
      c3trajectory_t = now;
    }

    while ((c3trajectory_t + traj_dt < now) and ros::ok()) {
      // ROS_INFO("Acting");

      c3trajectory->update(traj_dt.toSec(), current_waypoint,
                           (c3trajectory_t - current_waypoint_t).toSec());
      c3trajectory_t += traj_dt;
    }

    PoseTwistStamped msg;
    msg.header.stamp = c3trajectory_t;
    msg.header.frame_id = fixed_frame;
    msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
    trajectory_pub.publish(msg);

    PoseStamped posemsg;
    posemsg.header.stamp = c3trajectory_t;
    posemsg.header.frame_id = fixed_frame;
    posemsg.pose = Pose_from_Waypoint(current_waypoint);
    waypoint_pose_pub.publish(posemsg);

    if (actionserver.isActive() &&
        c3trajectory->getCurrentPoint().is_approximately(
            current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) &&
        current_waypoint.r.qdot == subjugator::Vector6d::Zero()) {
      actionserver.setSucceeded();
    }
  }
		/*!
		* \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_);
		}
Example #9
0
  //void executeCB()
  void executeCB(const amazon_challenge_bt_actions::BTGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // publish info to the console for the user
    ROS_INFO("Starting Action");

    // start executing the action
    for(int i=1; i<=20; i++)
    {

    ROS_INFO("Executing Action");

    //motion_proxy_ptr->post.move(0.1, 0.0, 0.0);

      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("Action Halted");


      // motion_proxy_ptr->stopMove();


        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.status = RUNNING;
      // publish the feedback
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes

      r.sleep();
   }

    if(success)
    {
      result_.status = feedback_.status;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }
	//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!");
		}
	}
Example #11
0
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message 
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g.,  "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
    ROS_INFO("callback activated");
    double yaw_desired, yaw_current, travel_distance, spin_angle;
    nav_msgs::Path paths;
    geometry_msgs::Pose pose_desired;
    paths = goal->input;

    int npts = paths.poses.size();
    ROS_INFO("received path request with %d poses",npts);    

	int move = 0;
	for (int i=0;i<npts;i++) { //visit each subgoal
		// odd notation: drill down, access vector element, drill some more to get pose
		pose_desired = paths.poses[i].pose; //get first pose from vector of poses        
        	get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
        	ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired, 		 	
				pose_desired.position.x,pose_desired.position.y); 
        	ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
        	ROS_INFO("travel distance = %f",travel_distance); 
		ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);        
		yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
		spin_angle = yaw_desired - yaw_current; // spin this much
		//spin_angle = min_spin(spin_angle);// but what if this angle is > pi?  then go the other way
		do_spin(spin_angle); // carry out this incremental action
		// we will just assume that this action was successful--really should have sensor feedback here
		g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
		move = pose_desired.position.x;
		
		ROS_INFO("Move:  %d", move);
		do_move(move);
		
		if(as_.isPreemptRequested()){
		   do_halt();
		   ROS_WARN("Srv: goal canceled.");
		   result_.output = -1;
		   as_.setAborted(result_);
		   return;
		}
        }
	result_.output = 0;
	as_.setSucceeded(result_);
	ROS_INFO("Move finished.");
}
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");
    ROS_INFO("object code is: %d", goal->object_code);
    ROS_INFO("perception_source is: %d", goal->perception_source);
    //do work here: this is where your interesting code goes
    feedback_.feedback_status = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing
    //get pickup pose:
    feedback_.feedback_status = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing    

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      


    // each iteration, check if cancellation has been ordered
    if (as_.isPreemptRequested()) {
        ROS_WARN("goal cancelled!");
        result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
        as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
        return; // done with callback
    }

    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
}
	// Go to this pose
	bool go_home(tf::Pose& pose_) {


		tf::Vector3 start_p(pose_.getOrigin());
		tf::Quaternion start_o(pose_.getRotation());

		msg_pose.pose.position.x = start_p.x();
		msg_pose.pose.position.y = start_p.y();
		msg_pose.pose.position.z = start_p.z();
		msg_pose.pose.orientation.w = start_o.w();
		msg_pose.pose.orientation.x = start_o.x();
		msg_pose.pose.orientation.y = start_o.y();
		msg_pose.pose.orientation.z = start_o.z();
		pub_.publish(msg_pose);
		sendNormalForce(0);

		ros::Rate thread_rate(2);
		ROS_INFO("Homing...");
		while(ros::ok()) {
			double oerr =(ee_pose.getRotation() - start_o).length() ;
			double perr = (ee_pose.getOrigin() - start_p).length();
			feedback_.progress = 0.5*(perr+oerr);
			as_.publishFeedback(feedback_);
			ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
			if(perr< 0.02 && oerr < 0.02) {
				break;
			}
			if (as_.isPreemptRequested() || !ros::ok() )
			{
				sendNormalForce(0);
				sendPose(ee_pose);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			thread_rate.sleep();
		}
		return ros::ok();

	}
    // move platform server receives a new goal
    void moveGoalCB()
    {
        set_terminal_state_ = true;

        move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
        ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);

        if (as_.isPreemptRequested() ||!ros::ok())
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
            if (planning_)
                ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            if (controlling_)
                ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            move_result_.result_state = 0;
            move_result_.error_string = "Preempt Requested!!!";
            as_.setPreempted(move_result_);
            return;
        }

        // Convert move goal to plan goal
        pose_goal_.pose_goal = move_goal_.nav_goal;

        if (planner_state_sub.getNumPublishers()==0)
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
            planning_ = false;
            move_result_.result_state = 0;
            move_result_.error_string = "Planner is down";
            as_.setAborted(move_result_);
        }
        else
        {
            ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
            planning_ = true;
            ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
        }
        return;
    }
    void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
    {
        // helper variables
        ros::Rate r(1);
        bool success = true;

        // push_back the seeds for the fibonacci sequence
        feedback_.sequence.clear();
        feedback_.sequence.push_back(0);
        feedback_.sequence.push_back(1);

        // publish info to the console for the user
        ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);

        // start executing the action
        for(int i=1; i<=goal->order; i++)
        {
            // check that preempt has not been requested by the client
            if (as_.isPreemptRequested() || !ros::ok())
            {
                ROS_INFO("%s: Preempted", action_name_.c_str());
                // set the action state to preempted
                as_.setPreempted();
                success = false;
                break;
            }
            feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
            // publish the feedback
            as_.publishFeedback(feedback_);
            // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
            r.sleep();
        }

        if(success)
        {
            result_.sequence = feedback_.sequence;
            ROS_INFO("%s: Succeeded", action_name_.c_str());
            // set the action state to succeeded
            as_.setSucceeded(result_);
        }
    }
Example #16
0
	void executeCB(const rm_multi_mapper_db::G2oWorkerGoalConstPtr & goal) {
		ros::Rate r(1);
		bool success = true;

		if (as_.isPreemptRequested() || !ros::ok()) {
			ROS_INFO("%s: Preempted", action_name_.c_str());
			as_.setPreempted();
			success = false;
		}

		for (size_t i = 0; i < goal->Overlap.size(); i++) {

			pcl::PointCloud<pcl::PointXYZ> keypoints3d1, keypoints3d2;
			cv::Mat descriptors1, descriptors2;

			//std::cerr << "Trying to match " << goal->Overlap[i].first << " "
			//		<< goal->Overlap[i].second << std::endl;

			U->get_keypoints(goal->Overlap[i].first, keypoints3d1, descriptors1);
			U->get_keypoints(goal->Overlap[i].second, keypoints3d2,
					descriptors2);

			Sophus::SE3f t;
			if (U->find_transform(keypoints3d1, keypoints3d2, descriptors1,
					descriptors2, t)) {
				U->add_measurement(goal->Overlap[i].first,
						goal->Overlap[i].second, t, "RANSAC");
			}

		}

		std::cout << "Done";
		if (success) {
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			result_.reply = true;
			as_.setSucceeded(result_);
		}
	}
Example #17
0
  void executeCB(const learning_actionlib::FibonacciGoalConstPtr& goal) {
    ros::Rate r(1);
    bool success = true;
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    ROS_INFO(
        "%s: Executing, creating fibonacci sequence of order %i with seeds %i, "
        "%i",
        action_name_.c_str(), goal->order, feedback_.sequence[0],
        feedback_.sequence[1]);

    for (int i = 1; i <= goal->order; i++) {
      if (as_.isPreemptRequested() || !ros::ok()) {
        ROS_INFO("%s: Preempted", action_name_.c_str());

        as_.setPreempted();
        success = false;
        break;
      }

      feedback_.sequence.push_back(feedback_.sequence[i] +
                                   feedback_.sequence[i - 1]);

      as_.publishFeedback(feedback_);

      r.sleep();
    }

    if (success) {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());

      as_.setSucceeded(result_);
    }
  }
  void executeCB(const lwr_peg_in_hole::RobotMoveGoalConstPtr& goal)
  {
    robot_moved_ = false;
    ros::Timer mover_thread = nh_.createTimer(ros::Duration(0.001), 
        boost::bind(&RobotMoveActionServer::moveBinCB, this, goal, _1), true);

    ros::Rate r(30);
    while (ros::ok()) {
      if(robot_moved_) {
        ROS_INFO("RobotMove action complete.");
        result_.success = true;
        act_srv_.setSucceeded(result_);
        return;
      }
      if(act_srv_.isPreemptRequested()) {
        robot_move_.stopJointTrajectory();
        ROS_INFO("Preempting robot move");
        mover_thread.stop();
        return;
      }
      ros::spinOnce();
      r.sleep();
    }
  }
  // add training images for a person
  void addTrainingImagesCb(const sensor_msgs::ImageConstPtr& msg) {
    // cout << "addTrainingImagesCb" << endl;

    if (_as.isPreemptRequested() || !ros::ok()) {
      // std::cout << "preempt req or not ok" << std::endl;
      ROS_INFO("%s: Preempted", _action_name.c_str());
      // set the action state to preempted
      _as.setPreempted();
      // success = false;
      // break;
      // cout << "shutting down _image_sub" << endl;
      _image_sub.shutdown();
      return;
    }

    if (!_as.isActive()) {
      // std::cout << "not active" << std::endl;
      // cout << "shutting down _image_sub" << endl;
      _image_sub.shutdown();
      return;
    }

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("%s: cv_bridge exception: %s", _action_name.c_str(), e.what());
      return;
    }

    if (_window_rows == 0) {
      _window_rows = cv_ptr->image.rows;
      _window_cols = cv_ptr->image.cols;
    }

    std::vector<cv::Rect> faces;
    // _fd.detectFaces(cv_ptr->image, faces, true);

    std::vector<cv::Mat> faceImgs = _fd.getFaceImgs(cv_ptr->image, faces, true);
    
    if (faceImgs.size() > 0)
      _fc->capture(faceImgs[0]);

    // call images capturing function
    _result.names.push_back(_goal_argument);
    // _result.confidence.push_back(2.0);
    
    int no_images_to_click;
    _ph.getParam("no_training_images", no_images_to_click);
    
    if (_fc->getNoImgsClicked() >= no_images_to_click) {
      // Mat inactive = cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_32F);
      Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
      appendStatusBar(inactive, "INACTIVE", "Images added. Please train.");
      cv::imshow(_windowName, inactive);  
      // cv::displayOverlay(_windowName, "Images added", 3000); 
      _as.setSucceeded(_result);
    } else {
      // update GUI window
      // check GUI parameter
      appendStatusBar(cv_ptr->image, "ADDING IMAGES.", "Images added");
      cv::imshow(_windowName, cv_ptr->image);  
    }

    cv::waitKey(3);
  }
Example #20
0
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");

    goal_action_code_ = goal->action_code; //verbatim from received goal
    action_code_ = goal->action_code; //init: this value changes as state machine advances through steps
    ROS_INFO("requested action code is: %d", goal_action_code_);
    //if action code is "MANIP_OBJECT", need to go through a sequence of action codes
    //otherwise, action code is a simple action code, and can use it as-is
    /*
    if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
        //if command is for manip, then we can expect an object code, perception source and dropoff pose
        object_code_ = goal->object_code; //what type of object is this?

        perception_source_ = goal->perception_source; //name sensor or provide coords
        dropoff_pose_ = goal->dropoff_frame;
        ROS_INFO("object code is: %d", object_code_);
        ROS_INFO("perception_source is: %d", goal->perception_source);
        //if (object_code_ == coordinator::ManipTaskGoal::TOY_BLOCK) {
        if (object_code_ == ObjectIdCodes::TOY_BLOCK_ID) {
            vision_object_code_ = object_code_; //same code;
            ROS_INFO("using object-finder object code %d", vision_object_code_);
            pickup_action_code_ = object_grabber::object_grabberGoal::GRAB_TOY_BLOCK;
            dropoff_action_code_ = object_grabber::object_grabberGoal::PLACE_TOY_BLOCK;
            //start the state machine with perceptual processing task:
            action_code_ = coordinator::ManipTaskGoal::GET_PICKUP_POSE;
        } else {
            ROS_WARN("unknown object type in manipulation action");
            as_.setAborted(result_);
        }
    } else if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) {
        object_code_ = goal->object_code; //what type of object is this?
 */
    if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) {
        object_code_ = goal->object_code; //what type of object is this?   

    
    } else if (goal_action_code_ == coordinator::ManipTaskGoal::GRAB_OBJECT) {
        object_code_ = goal->object_code; //what type of object is this?     
        ROS_INFO("object code is: %d", object_code_);
 
        if (goal->perception_source == coordinator::ManipTaskGoal::BLIND_MANIP) {
            ROS_INFO("blind manipulation; using provided pick-up pose");
            pickup_pose_ = goal->pickup_frame;
        }
    } else if (goal_action_code_ == coordinator::ManipTaskGoal::GET_PICKUP_POSE) {
        ROS_INFO("object code is: %d", object_code_);
        ROS_INFO("perception_source is: %d", goal->perception_source);
        object_code_ = goal->object_code; //what type of object is this?
        perception_source_ = goal->perception_source; //name sensor or provide coords
        vision_object_code_ = object_code_; //same code
    }

    status_code_ = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    working_on_task_ = true;
    //do work here
    while (working_on_task_) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
        feedback_.feedback_status = status_code_;
        as_.publishFeedback(feedback_);
        //ROS_INFO("executeCB: status_code = %d", status_code_);
        // each iteration, check if cancellation has been ordered

        if (as_.isPreemptRequested()) {
            ROS_WARN("goal cancelled!");
            result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
            action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
            status_code_ = coordinator::ManipTaskFeedback::ABORTED;
            working_on_task_ = false;
            as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
            return; // done with callback
        }
        //here is where we step through states:
        switch (action_code_) {
            case coordinator::ManipTaskGoal::FIND_TABLE_SURFACE:
                ROS_INFO("serving request to find table surface");
                found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY;
                object_finder_goal_.object_id = ObjectIdCodes::TABLE_SURFACE;
                //vision_object_code_;
                object_finder_goal_.known_surface_ht = false; //require find table height
                //object_finder_goal_.surface_ht = 0.05; //this is ignored for known_surface_ht=false
                object_finder_ac_.sendGoal(object_finder_goal_,
                        boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2));
                action_code_ = coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE;
                ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
                ROS_INFO("waiting on perception");
                break;
            case coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE:
                if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) {
                    ROS_INFO("surface-finder success");
                    surface_height_ = pickup_pose_.pose.position.z; // table-top height, as found by object_finder
                    found_surface_height_ = true;
                    ROS_INFO("found table ht = %f", surface_height_);
                    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
                    return; //done w/ callback
                } else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
                    //ROS_INFO("waiting on perception"); //do nothing
                } else {
                    ROS_WARN("object-finder failure; aborting");
                    action_code_ = coordinator::ManipTaskGoal::ABORT;
                    result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
                    found_surface_height_ = false;
                }

                break;

            case coordinator::ManipTaskGoal::GET_PICKUP_POSE:
                ROS_INFO("establishing pick-up pose");
                if (perception_source_ == coordinator::ManipTaskGoal::BLIND_MANIP) {
                    ROS_INFO("blind manipulation; using provided pick-up pose");
                    pickup_pose_ = goal->pickup_frame;
                    result_.object_pose = pickup_pose_;
                    //done with perception, but do fake waiting anyway
                    //declare victor on finding object
                    found_object_code_ = object_finder::objectFinderResult::OBJECT_FOUND;
                    action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER;
                    status_code_ = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
                } else if (perception_source_ == coordinator::ManipTaskGoal::PCL_VISION) {
                    ROS_INFO("invoking object finder");
                    found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY;
                    ROS_INFO("instructing finder to locate object %d", vision_object_code_);
                    object_finder_goal_.object_id = vision_object_code_;
                    if (found_surface_height_) {
                        object_finder_goal_.known_surface_ht = true;
                        object_finder_goal_.surface_ht = surface_height_;
                        ROS_INFO("using surface ht = %f", surface_height_);
                    } else {
                        object_finder_goal_.known_surface_ht = false; //require find table height
                        //object_finder_goal_.surface_ht = 0.05; //not needed
                    }

                    ROS_INFO("sending object-finder goal: ");

                    object_finder_ac_.sendGoal(object_finder_goal_,
                            boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2));

                    action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER;
                    ROS_INFO("waiting on perception");
                } else {
                    ROS_WARN("unrecognized perception mode; quitting");
                    action_code_ = coordinator::ManipTaskGoal::ABORT;
                    result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
                }

                ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
                break;

            case coordinator::ManipTaskGoal::WAIT_FOR_FINDER:
                if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) {
                    ROS_INFO("object-finder success");
                    //next step: use the pose to grab object:
                    /* if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
                        action_code_ = coordinator::ManipTaskGoal::GRAB_OBJECT;
                        status_code_ = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
                    } else*/ {
                        working_on_task_ = false; // test--set to goal achieved
                        action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
                        status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
                        result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
                        //object pose is in result message
                        as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
                        return; //done w/ callback
                    }
                    //will later test for result code of object grabber, so initialize it to PENDING
                    //(next step in state machine)
                    object_grabber_return_code_ = object_grabber::object_grabberResult::PENDING;
                } else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
                    //ROS_INFO("waiting on perception"); //continue waiting
                } else {
                    ROS_WARN("object-finder failure; aborting");
                    action_code_ = coordinator::ManipTaskGoal::ABORT;
                    result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
                }
                break;

            case coordinator::ManipTaskGoal::GRAB_OBJECT:
                status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
                ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
                //ros::Duration(2.0).sleep();
                //if here, then presumably have a valid pose for object of interest; grab it! 
                //send object-grabber action code to grab specified object
                object_grabber_goal_.action_code = object_grabber::object_grabberGoal::GRAB_OBJECT;//pickup_action_code_; //specify the object to be grabbed 
                object_grabber_goal_.object_frame = pickup_pose_; //and the object's current pose
                object_grabber_goal_.object_id = object_code_; // = goal->object_code;
                object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above
                
                ROS_INFO("sending goal to grab object: ");
                object_grabber_ac_.sendGoal(object_grabber_goal_,
                        boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));

                action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT;
                status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
                //will inspect this status to see if object grasp is eventually successful
                object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;

                break;

            case coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT:
                if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_ACQUIRED) { //success!
                    ROS_INFO("acquired object");


                    /*if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
                        //for manip command, have more steps to complete:
                        action_code_ = coordinator::ManipTaskGoal::DROPOFF_OBJECT;
                        status_code_ = coordinator::ManipTaskFeedback::PICKUP_SUCCESSFUL;
                    } else*/ { //if just a grab command, we are now done
                        working_on_task_ = false; // test--set to goal achieved
                        action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
                        status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
                        result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
                        //object pose is in result message
                        as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
                        return; //done w/ callback
                    }


                } else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
                    // do nothing--just wait patiently
                    //ROS_INFO("waiting for object grab");
                } else {
                    ROS_WARN("trouble with acquiring object");
                    action_code_ = coordinator::ManipTaskGoal::ABORT;
                    result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PICKUP;
                }
                break;


            case coordinator::ManipTaskGoal::DROPOFF_OBJECT:
                status_code_ = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
                ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
                object_grabber_goal_.action_code = object_grabber::object_grabberGoal::DROPOFF_OBJECT;//dropoff_action_code_; //specify the object to be grabbed 
                object_grabber_goal_.object_id = object_code_; // = goal->object_code;
                dropoff_pose_= goal->dropoff_frame;
                object_grabber_goal_.object_frame = dropoff_pose_; //and the object's current pose
                object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above

                        
                ROS_INFO("sending goal to drop off object: ");
                object_grabber_ac_.sendGoal(object_grabber_goal_,
                        boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));

                action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT;
                //will inspect this status to see if object grasp is eventually successful
                object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;

                break;


            case coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT:
                //ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_);
                if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success!
                    ROS_INFO("switch/case happiness!  dropped off object; manip complete");
                    working_on_task_ = false; // test--set to goal achieved
                    action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
                    status_code_ = coordinator::ManipTaskFeedback::COMPLETED_DROPOFF;

                    result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
                    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
                    return; //done w/ callback
                } else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
                    // do nothing--just wait patiently
                    //ROS_INFO("waiting for object dropoff");
                } else {
                    ROS_WARN("trouble with acquiring object");
                    action_code_ = coordinator::ManipTaskGoal::ABORT;
                    result_.manip_return_code = coordinator::ManipTaskResult::FAILED_DROPOFF;
                }
                break;

            case coordinator::ManipTaskGoal::MOVE_TO_PRE_POSE:
                status_code_ = coordinator::ManipTaskFeedback::PREPOSE_MOVE_BUSY;
                object_grabber_goal_.action_code = object_grabber::object_grabberGoal::MOVE_TO_WAITING_POSE; //specify the object to be grabbed 
                ROS_INFO("sending goal to move to pre-pose: ");
                object_grabber_ac_.sendGoal(object_grabber_goal_,
                        boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));

                action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE;
                //will inspect this status to see if object grasp is eventually successful
                object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;
                break;
            case coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE:
                if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success!
                    ROS_INFO("completed move to pre-pose");
                    working_on_task_ = false; // test--set to goal achieved
                    action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
                    status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
                    result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
                    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
                    return; //done w/ callback
                }
                else {
                    ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_);
                    ros::Duration(0.5).sleep();
                }

                break;

            case coordinator::ManipTaskGoal::ABORT:
                ROS_WARN("aborting goal...");
                //retain reason for failure to report back to client
                //result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
                action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
                status_code_ = coordinator::ManipTaskFeedback::ABORTED;
                working_on_task_ = false;
                as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
                return; // done with callback

            default:
                ROS_WARN("executeCB: error--case not recognized");
                working_on_task_ = false;
                break;
        }

    }
    ROS_INFO("executeCB: I should not be here...");
    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
    as_.setAborted(result_); // return the "result" message to client, along with "success" status
    return;
}
  int executeCB(const soft_move_base::SoftMoveBaseGoalConstPtr &goal)
  {
    ROS_INFO("Received new trajectory");
    SM_TRAJ tmpTraj;
    ros::Rate _loop_rate(1/_dt);
    _timeFromStart= 0;
    _tmpTimeFromStart= 0;
    _timer = 0;
    loadNewTraj(goal->traj); 
    double duration = 0.0;
    duration =  _newTraj.getDuration();


    limits.clear();
    for(int i=0; i<6; ++i)
    {
      limit.maxJerk = _newTraj.jmax[i] * 1.1;
      limit.maxAcc =  _newTraj.amax[i] * 1.1;
      limit.maxVel =  _newTraj.vmax[i] * 1.1;
      limits.push_back(limit);
    }

    //double t = 0;
    //FILE* file;
    //file  = fopen("baseExperiment.dat", "w");


   bool cont = true;
   do {
     // end if goal canceled
     if( as_.isPreemptRequested())
		break;

     getNextRobotCond(_timeFromStart + _dt);
     //printCond(_nextRobotCond_w);

     getPoseInRobotFrame(_nextRobotCond_w);
     //printCond(_nextRobotCond_r);

     _currRobotCond_r[0].x = 0;
     _currRobotCond_r[0].v = _lastVel[0];
     _currRobotCond_r[0].a = _lastAcc[0];
     _currRobotCond_r[1].x = 0;
     _currRobotCond_r[1].v = _lastVel[1];
     _currRobotCond_r[1].a = _lastAcc[1];
     _currRobotCond_r[5].x = 0;
     _currRobotCond_r[5].v = _lastVel[5];
     _currRobotCond_r[5].a = _lastAcc[5];
     //printCond(_currRobotCond_r);

     ROS_INFO("TMP TIME:%f", _tmpTimeFromStart);

     if( _timeFromStart == 0.0) 
     {
       //correction
       tmpTraj.clear(); 

       boundsCond(_currRobotCond_r);
       boundsCond(_nextRobotCond_r);
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
       //printCond( _newRobotCond_r);
     }
     else if(_timer - 0.5 >= 0.0001 
            || _tmpTimeFromStart > tmpTraj.getDuration())
     {
       _timer = 0;

       //correction
       tmpTraj.clear(); 
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
     }

     tmpTraj.getMotionCond(_tmpTimeFromStart,_newRobotCond_r);
     _lastVel[0]= _newRobotCond_r[0].v;
     _lastVel[1]= _newRobotCond_r[1].v;
     _lastVel[5]= _newRobotCond_r[5].v;
     _lastAcc[0]= _newRobotCond_r[0].a;
     _lastAcc[1]= _newRobotCond_r[1].a;
     _lastAcc[5]= _newRobotCond_r[5].a;

     //printCond(_newRobotCond_r);
    
     publishNewCond(_newRobotCond_r);

     _timeFromStart += _dt;
     _tmpTimeFromStart += _dt;
     _timer += _dt;
     if(_timeFromStart > _newTraj.getDuration())
       _timeFromStart = _newTraj.getDuration();

     _loop_rate.sleep();

     if(_timeFromStart >= _newTraj.getDuration())
     {
       cont = fabs(_newRobotCond_r[0].x) > _tolerance[0]
              || fabs(_newRobotCond_r[1].x) > _tolerance[1]
              || fabs(_newRobotCond_r[5].x) > _tolerance[2];
     }
     
     ros::spinOnce();
     
   } while(cont);

    soft_move_base::SoftMoveBaseResult result;
    result.result = true;
    as_.setSucceeded(result);

    return 0;
  } 
 void run()
 {
     if(executing_)
     {
         failure_ = false;
         watchdog_counter = 0;
         //if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
         if (!ros::ok() || current_operation_mode_ != "velocity")
         {
             // set the action state to preempted
             executing_ = false;
             traj_generator_->isMoving = false;
             //as_.setPreempted();
             failure_ = true;
             return;
         }
         if (as_follow_.isPreemptRequested())
         {
             //as_follow_.setAborted()
             failure_ = true;
             preemted_ = true;
             ROS_INFO("Preempted trajectory action");
             return;
         }
         std::vector<double> des_vel;
         if(traj_generator_->step(q_current, des_vel))
         {
             if(!traj_generator_->isMoving) //Finished trajectory
             {
                 executing_ = false;
                 preemted_ = false;
             }
             brics_actuator::JointVelocities target_joint_vel;
             target_joint_vel.velocities.resize(DOF);
             for(int i=0; i<DOF; i++)
             {
                 target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
                 target_joint_vel.velocities[i].unit = "rad";
                 target_joint_vel.velocities[i].value = des_vel.at(i);
             }
             
             //send everything
             joint_vel_pub_.publish(target_joint_vel);
         }
         else
         {
             ROS_INFO("An controller error occured!");
             failure_ = true;
             executing_ = false;
         }
     }
     else
     {  //WATCHDOG TODO: don't always send
         if(watchdog_counter < 10)
         {
             brics_actuator::JointVelocities target_joint_vel;
             target_joint_vel.velocities.resize(DOF);
             for (int i = 0; i < DOF; i += 1)
             {
                 target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
                 target_joint_vel.velocities[i].unit = "rad";
                 target_joint_vel.velocities[i].value = 0;
             }
             joint_vel_pub_.publish(target_joint_vel);
             ROS_INFO("Publishing 0-vel (%d)", DOF);
         }
         watchdog_counter++;
     }
 }
Example #23
0
  void timer_callback(const ros::TimerEvent &)
  {
    mil_msgs::MoveToResult actionresult;

    // Handle disabled, killed, or no odom before attempting to produce trajectory
    std::string err = "";
    if (disabled)
      err = "c3 disabled";
    else if (kill_listener.isRaised())
      err = "killed";
    else if (!c3trajectory)
      err = "no odom";

    if (!err.empty())
    {
      if (c3trajectory)
        c3trajectory.reset();  // On revive/enable, wait for odom before station keeping

      // Cancel all goals while killed/disabled/no odom
      if (actionserver.isNewGoalAvailable())
        actionserver.acceptNewGoal();
      if (actionserver.isActive())
      {
        actionresult.error = err;
        actionresult.success = false;
        actionserver.setAborted(actionresult);
      }
      return;
    }

    ros::Time now = ros::Time::now();

    auto old_waypoint = current_waypoint;

    if (actionserver.isNewGoalAvailable())
    {
      boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
      current_waypoint =
          subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
                                             goal->speed, !goal->uncoordinated, !goal->blind);
      current_waypoint_t = now;
      this->linear_tolerance = goal->linear_tolerance;
      this->angular_tolerance = goal->angular_tolerance;

      waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);

      // Check if waypoint is valid
      std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
          Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
      actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
      actionresult.success = checkWPResult.first;
      if (checkWPResult.first == false && waypoint_check_)  // got a point that we should not move to
      {
        waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
        if (checkWPResult.second ==
            WAYPOINT_ERROR_TYPE::UNKNOWN)  // if unknown, check if there's a huge displacement with the new waypoint
        {
          auto a_point = Pose_from_Waypoint(current_waypoint);
          auto b_point = Pose_from_Waypoint(old_waypoint);
          // If moved more than .5m, then don't allow
          if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
          {
            ROS_ERROR("can't move there! - need to rotate");
            current_waypoint = old_waypoint;
          }
        }
        // if point is occupied, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
        {
          ROS_ERROR("can't move there! - waypoint is occupied");
          current_waypoint = old_waypoint;
        }
        // if point is above water, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
        {
          ROS_ERROR("can't move there! - waypoint is above water");
          current_waypoint = old_waypoint;
        }
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
        {
          ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
        }
      }
    }
    if (actionserver.isPreemptRequested())
    {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.do_waypoint_validation = false;
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
    }

    // Remember the previous trajectory
    auto old_trajectory = c3trajectory->getCurrentPoint();

    while (c3trajectory_t + traj_dt < now)
    {
      c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
      c3trajectory_t += traj_dt;
    }

    // Check if we will hit something while in trajectory the new trajectory
    geometry_msgs::Pose traj_point;  // Convert messages to correct type
    auto p = c3trajectory->getCurrentPoint();
    traj_point.position = vec2xyz<Point>(p.q.head(3));
    quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);

    std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
        waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);

    if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
    {  // New trajectory will hit an occupied goal, so reject
      ROS_ERROR("can't move there! - bad trajectory");
      current_waypoint = old_trajectory;
      current_waypoint.do_waypoint_validation = false;
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
      actionresult.success = false;
      actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
    }

    PoseTwistStamped msg;
    msg.header.stamp = c3trajectory_t;
    msg.header.frame_id = fixed_frame;
    msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
    trajectory_pub.publish(msg);

    waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);

    PoseStamped msgVis;
    msgVis.header = msg.header;
    msgVis.pose = msg.posetwist.pose;
    trajectory_vis_pub.publish(msgVis);

    PoseStamped posemsg;
    posemsg.header.stamp = c3trajectory_t;
    posemsg.header.frame_id = fixed_frame;
    posemsg.pose = Pose_from_Waypoint(current_waypoint);
    waypoint_pose_pub.publish(posemsg);

    if (actionserver.isActive() &&
        c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance),
                                                         max(1e-3, angular_tolerance)) &&
        current_waypoint.r.qdot == subjugator::Vector6d::Zero())
    {
      actionresult.error = "";
      actionresult.success = true;
      actionserver.setSucceeded(actionresult);
    }
  }
	// Do stuff with learned models
	// Phase - Reach, Roll or Back?
	// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
	// reachingThreshold - you know
	// model_dt - you know
	bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {

		ROS_INFO_STREAM(" Model Path "<<model_base_path);	
		ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
		ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
		ROS_INFO_STREAM(" Model DT "<<model_dt);
		if (force_gmm_id!="")
                 ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);


		ros::Rate wait(1);
		tf::Transform  trans_final_target, ee_pos_att;

		// To Visualize EE Frames		    
		static tf::TransformBroadcaster br;
		br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
		br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));


		// convert attractor information to world frame
		trans_final_target.mult(trans_obj, trans_att);

		ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
		ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());

		// Publish attractors if running in simulation or with fixed values
		if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
		    br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
		}

		// Initialize CDS
		CDSExecution *cdsRun = new CDSExecution;
		cdsRun->initSimple(model_base_path, phase, force_gmm_id);
		cdsRun->setObjectFrame(toMatrix4(trans_obj));
		cdsRun->setAttractorFrame(toMatrix4(trans_att));
		cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
		cdsRun->setDT(model_dt);


		if (phase==PHASEBACK || phase==PHASEROLL)
			 masterType = CDSController::LINEAR_DYNAMICS;

		// Roll slow but move fast for reaching and back phases.
		// If models have proper speed, this whole block can go!
		if(phase == PHASEROLL) {
			//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
			cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
			// large threshold to avoid blocking forever
			// TODO: should rely on preempt in action client.
//			reachingThreshold = 0.02;
			reachingThreshold = 0.025;
		} else {
			cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
		}


		cdsRun->postInit();

		// If phase is rolling, need force model as well
		GMR* gmr_perr_force = NULL;
		if(phase == PHASEROLL) {
			gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
			if(!gmr_perr_force) {
				ROS_ERROR("Cannot initialize GMR model");
				return false;
			}
		}

		ros::Duration loop_rate(model_dt);
		tf::Pose mNextRobotEEPose = ee_pose;
		std::vector<double> gmr_in, gmr_out;
		gmr_in.resize(1);gmr_out.resize(1);
		double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
		tf::Vector3 att_t, curr_t;

		ROS_INFO("Execution started");
		tf::Pose p;
		bool bfirst = true;

		std_msgs::String string_msg, action_name_msg, model_fname_msg;
		std::stringstream ss, ss_model;

		if(phase ==  PHASEREACH) {
			ss << "reach ";
		}
		else if (phase == PHASEROLL){
			ss << "roll";
		}	
		else if (phase == PHASEBACK){
			ss << "back";
		}		

		if (!homing){
			string_msg.data = ss.str();
			pub_action_state_.publish(string_msg);


//			ss_model << path_matlab_plot << "/Phase" <<  phase << "/masterGMM.fig";

			if (force_gmm_id=="")
				ss_model << path_matlab_plot << "/Phase" <<  phase << "/masterGMM.fig";
			else
				ss_model << path_matlab_plot << "/Phase" <<  phase << "/ForceProfile_" << force_gmm_id << ".fig";
			//ss_model << "/Phase" <<  phase << "/masterGMM.fig";
			
			model_fname_msg.data = ss_model.str();		
			pub_model_fname_.publish(model_fname_msg);

			action_name_msg.data = ss.str();
			pub_action_name_.publish(action_name_msg);

			plot_dyn = 1;			
			plot_dyn_msg.data = plot_dyn;
			pub_plot_dyn_.publish(plot_dyn_msg);
			
		}
		
		while(ros::ok()) {
			
			if (!homing)
				plot_dyn = 1;
			else
				plot_dyn = 0;

			plot_dyn_msg.data = plot_dyn;
			pub_plot_dyn_.publish(plot_dyn_msg);

			// Nadia's stuff
			// Current progress variable (position/orientation error).
			// TODO: send this back to action client as current progress
			pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
			//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
			ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));


			ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
			ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);

			/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
			double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));

			ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
			ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/

			switch (phase) {
			// Home, reach and back are the same control-wise
			case PHASEREACH:

			case PHASEBACK:
				// Current progress variable (position/orientation error).
				// TODO: send this back to action client as current progress
				prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());

				// Compute Next Desired EE Pose
				cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
				toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
				p = mNextRobotEEPose;

				// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
				// going the wrong way around
				p.setRotation(trans_final_target.getRotation());

				//Publish desired force	
				gmr_msg.data = 0.0;
				pub_gmr_out_.publish(gmr_msg);

				break;
			case PHASEROLL:

				// Current progress in rolling phase is simply the position error	
				prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();

				// New position error being fed to GMR Force Model	
				att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
				curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
				new_err = (att_t - curr_t).length();

 				gmr_err = new_err;
				//Hack! Truncate errors to corresponding models				
				if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
					gmr_err = 0.03;
 				}
				
				if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
					gmr_err = 0.07;
 				}
				if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
					gmr_err = 0.13;
				}
				//pos_err = prog_curr;
				ori_err = 0;
				gmr_err = gmr_err;

				gmr_in[0] = gmr_err; // distance between EE and attractor

				// Query the model for desired force
				getGMRResult(gmr_perr_force, gmr_in, gmr_out);
	
/*				double fz_plot;
				getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
				//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
				// Send fz and distance to attractor for plotting		
				msg_ft.wrench.force.x = gmr_err;
				msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
				msg_ft.wrench.force.z = 0;
				msg_ft.wrench.torque.x = 0;
				msg_ft.wrench.torque.y = 0;
				msg_ft.wrench.torque.z = 0;
				pub_ee_ft_att_.publish(msg_ft);

				// Hack! Scale the force to be in reasonable values
				gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);

				ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);

				gmr_msg.data = gmr_out[0];
				pub_gmr_out_.publish(gmr_msg);

				// Hack! Safety first!
				if(gmr_out[0] > MAX_ROLLING_FORCE) {
					gmr_out[0] = MAX_ROLLING_FORCE;
				}

				// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
				if(bfirst) {
					sendAndWaitForNormalForce(-gmr_out[0]);
					bfirst = false;
				} else {
					sendNormalForce(-gmr_out[0]);
				}
				ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);



				cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
				toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);

				p = mNextRobotEEPose;

				// Hack! Dont rely on model orientation. Use target orientation instead.
				p.setRotation(trans_final_target.getRotation());

				// Hack! Dont rely on the Z component of the model. It might go below the table!
				p.getOrigin().setZ(trans_final_target.getOrigin().getZ());

				homing=false;
				break;
			default:
				ROS_ERROR_STREAM("No such phase defined "<<phase);
				return false;
			}


			// Add rotation of Tool wrt. flange_link for BOXY
			/*if (use_boxy_tool){
				tf::Matrix3x3 R = p.getBasis();
	      			Eigen::Matrix3d R_ee;
				tf::matrixTFToEigen(R,R_ee);

				Eigen::Matrix3d R_tool;
				R_tool << -0.7071, -0.7071, 0.0, 
                           		  0.7071,-0.7071, 0.0,
                            		      0.0,  0.0,  1.0;

	      			//multiply tool rot
				R_ee = R_ee*R_tool;
                                tf::matrixEigenToTF(R_ee,R);				
				p.setBasis(R);
			}*/			


			// Send the computed pose from one of the above phases
			sendPose(p);

			// convert and send ee pose to attractor frame to plots
			ee_pos_att.mult(trans_final_target.inverse(), p);
			geometry_msgs::PoseStamped msg;
			msg.pose.position.x = ee_pos_att.getOrigin().x();
			msg.pose.position.y = ee_pos_att.getOrigin().y();
			msg.pose.position.z = ee_pos_att.getOrigin().z();
			msg.pose.orientation.x = ee_pos_att.getRotation().x();
			msg.pose.orientation.y = ee_pos_att.getRotation().y();
			msg.pose.orientation.z = ee_pos_att.getRotation().z();
			msg.pose.orientation.w = ee_pos_att.getRotation().w();
			pub_ee_pos_att_.publish(msg);
			

			//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested() || !ros::ok())
			{
				sendPose(ee_pose);
				sendNormalForce(0);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			feedback_.progress = prog_curr;
			as_.publishFeedback(feedback_);

/*			if(prog_curr < reachingThreshold) {
				break;
			}*/

			//Orientation error	0.05
			if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {

				break;
			}

			loop_rate.sleep();
		}
		delete cdsRun;

		if(phase ==  PHASEREACH) {
			// Hack! If phase is "reach", find the table right after reaching
			if (bWaitForForces && !homing)	{		
				bool x = find_table_for_rolling(0.35, 0.05, 5);
//				bool x = find_table_for_rolling(0.35, 0.1, 5);
				//-> Send command to dynamics plotter to stop logging				 
				return x;
			}
			if (!homing){
				//-> Send command to dynamics plotter to stop logging				 
			}
		} else if (phase == PHASEROLL){
			// Hack! wait for zero force before getting ready to recieve further commands.
			// This is to avoid dragging the dough.
			sendAndWaitForNormalForce(0);
			//-> Send command to dynamics plotter to start plotting
			return ros::ok();
		} else {
			//->Send command to dynamics plotter to start plotting

			return ros::ok();
		}
	}
	/*!
	 * \brief Run the controller.
	 *
	 * The Controller generates desired velocities for every joints from the current positions.
	 *
	 */
	void run()
	{
		if(executing_)
		{
			watchdog_counter = 0;
			if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
			{
				/// set the action state to preempted
				executing_ = false;
				traj_generator_->isMoving = false;
				ROS_INFO("Preempted trajectory action");

				return;
			}

			std::vector<double> des_vel;
			if(traj_generator_->step(q_current, des_vel))
			{
				if(!traj_generator_->isMoving) //Trajectory is finished
				{
					executing_ = false;
				}

				brics_actuator::JointVelocities target_joint_vel;
				target_joint_vel.velocities.resize(7);
				for(unsigned int i=0; i<7; i++)
				{
					std::stringstream joint_name;
					joint_name << "arm_" << (i+1) << "_joint";
					target_joint_vel.velocities[i].joint_uri = joint_name.str();
					target_joint_vel.velocities[i].unit = "rad";
					target_joint_vel.velocities[i].value = des_vel.at(i);
				}

				/// send everything
				joint_vel_pub_.publish(target_joint_vel);
			}

			else
			{
				ROS_INFO("An controller error occured!");
				executing_ = false;
			}

		}

		else
		{
			/// WATCHDOG TODO: don't always send
			if(watchdog_counter < 10)
			{
				sensor_msgs::JointState target_joint_position;
				target_joint_position.position.resize(7);
				brics_actuator::JointVelocities target_joint_vel;
				target_joint_vel.velocities.resize(7);
				for (unsigned int i = 0; i < 7; i += 1)
				{
					std::stringstream joint_name;
					joint_name << "arm_" << (i+1) << "_joint";
					target_joint_vel.velocities[i].joint_uri = joint_name.str();
					target_joint_position.position[i] = 0;
					target_joint_vel.velocities[i].unit = "rad";
					target_joint_vel.velocities[i].value = 0;
				}

				joint_vel_pub_.publish(target_joint_vel);
				joint_pos_pub_.publish(target_joint_position);
			}

			watchdog_counter++;
		}
	}
  // run face recognition on the recieved image 
  void recognizeCb(const sensor_msgs::ImageConstPtr& msg) {
    // cout << "entering.. recognizeCb" << endl;

    _ph.getParam("algorithm", _recognitionAlgo);

    if (_as.isPreemptRequested() || !ros::ok()) {
      // std::cout << "preempt req or not ok" << std::endl;
      ROS_INFO("%s: Preempted", _action_name.c_str());
      // set the action state to preempted
      _as.setPreempted();
      // success = false;
      // break;
      // cout << "shutting down _image_sub" << endl;
      _image_sub.shutdown();
      Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
      appendStatusBar(inactive, "INACTIVE", "");
      cv::imshow(_windowName, inactive);
      cv::waitKey(3);
      return;
    }

    if (!_as.isActive()) {
      // std::cout << "not active" << std::endl;
      // cout << "shutting down _image_sub" << endl;
      _image_sub.shutdown();
      Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
      appendStatusBar(inactive, "INACTIVE", "");
      cv::imshow(_windowName, inactive);
      cv::waitKey(3);
      return;
    }

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("%s: cv_bridge exception: %s", _action_name.c_str(), e.what());
      return;
    }

    if (_window_rows == 0) {
      _window_rows = cv_ptr->image.rows;
      _window_cols = cv_ptr->image.cols;
    }

    // clear previous feedback
    _feedback.names.clear();
    _feedback.confidence.clear();
    // _result.names.clear();
    // _result.confidence.clear();

    std::vector<cv::Rect> faces;
    std::vector<cv::Mat> faceImgs = _fd.getFaceImgs(cv_ptr->image, faces, true);
    std::map<string, std::pair<string, double> > results;
    
    for( size_t i = 0; i < faceImgs.size(); i++ ) {
      cv::resize(faceImgs[i], faceImgs[i], cv::Size(100.0, 100.0));
      cv::cvtColor( faceImgs[i], faceImgs[i], CV_BGR2GRAY );
      cv::equalizeHist( faceImgs[i], faceImgs[i] );

      // perform recognition
      results = _fr->recognize(faceImgs[i],
                                ("eigenfaces" == _recognitionAlgo),
                                ("fisherfaces" == _recognitionAlgo),
                                ("lbph" == _recognitionAlgo)
                              );

      ROS_INFO("Face %lu:", i);
      if ("eigenfaces" == _recognitionAlgo)
        ROS_INFO("\tEigenfaces: %s %f", results["eigenfaces"].first.c_str(), results["eigenfaces"].second);
      if ("fisherfaces" == _recognitionAlgo)
        ROS_INFO("\tFisherfaces: %s %f", results["fisherfaces"].first.c_str(), results["fisherfaces"].second);
      if ("lbph" == _recognitionAlgo)
        ROS_INFO("\tLBPH: %s %f", results["lbph"].first.c_str(), results["lbph"].second);
    }


    // update GUI window
    // TODO check gui parameter
    appendStatusBar(cv_ptr->image, "RECOGNITION", "");
    cv::imshow(_windowName, cv_ptr->image);
    cv::waitKey(3);

    // if faces were detected
    if (faceImgs.size() > 0) {
      // recognize only once
      if (_goal_id == 0) {
        // std::cout << "goal_id 0. setting succeeded." << std::endl;
        // cout << _recognitionAlgo << endl;
        _result.names.push_back(results[_recognitionAlgo].first);
        _result.confidence.push_back(results[_recognitionAlgo].second);
        _as.setSucceeded(_result);
      }
      // recognize continuously
      else {
        _feedback.names.push_back(results[_recognitionAlgo].first);
        _feedback.confidence.push_back(results[_recognitionAlgo].second);
        _as.publishFeedback(_feedback);
      }
    }
  } 
void Trajectory_Tracker::track_trajectory(const planner::gen_trajGoalConstPtr &goal)
{
  ROS_INFO("TRACKING TRAJECTORY");
  float x = goal->x;
  float y = goal->y;
  x = x;
  y = y;
  if(goal->frame != "robot"){
    geometry_msgs::Point goal_point = transform_goal_to_robot(x,y,goal->frame);
    x = goal_point.x;
    y = goal_point.y;  
  }

  bool following_trajectory = true;
  tf::StampedTransform frame_transform = broadcast_new_traj_frame();
  current_trajectory.generate_trajectory(x,y);
  cout << "generated trajectory" << endl;
  ros::Rate loop_rate(10);
  tf::StampedTransform transform;
  tf::Quaternion q;
  ros::Time start_time = ros::Time::now();
  float t = 0;
  while(t<=(current_trajectory.t_end)+1)
  {
    if (action_server.isPreemptRequested() || !ros::ok())
    {
      float u_v = 0;
      float u_w = 0;
      geometry_msgs::Pose2D cmd_vector;
      cmd_vector.x = u_v;
      cmd_vector.y = 0;
      cmd_vector.theta = u_w;
      cmd_pub.publish(cmd_vector);
      result.success = 0;
      action_server.setSucceeded(result);
      cout << "CANCELING GOAL" << endl;
      return;
    }
    

    t = (ros::Time::now()-start_time).toSec();
    feedback.time = t;
    action_server.publishFeedback(feedback);

    Eigen::ArrayXf traj_at_t = current_trajectory.traj_time_lookup(t);
    
    marker_pub.publish(current_trajectory.points);
    marker_pub.publish(current_trajectory.line_strip);
    // marker_pub.publish(current_trajectory.line_list);
    transform.setOrigin( tf::Vector3(traj_at_t(1), traj_at_t(2), 0));

    q.setRPY(0, 0, traj_at_t(3));
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/traj_frame", "/target"));

    Eigen::ArrayXf error = get_error();
    cout << error << endl;
    // TODO: FEEDFORWARD
    float u_v = traj_at_t(4);
    float u_w = traj_at_t(5);
    u_v += Kp_x*error(0);
    u_w += Kp_y*error(1) + Kp_w*error(2);
    
    geometry_msgs::Pose2D cmd_vector;
    cmd_vector.x = u_v;
    cmd_vector.y = 0;
    cmd_vector.theta = u_w;

    cmd_pub.publish(cmd_vector);

    loop_rate.sleep();
    if(!ros::ok())
    {

      result.success = 0;
      action_server.setSucceeded(result);
      return;
    }
    br.sendTransform(tf::StampedTransform(frame_transform, ros::Time::now(), "/world", "/traj_frame"));
  }
  geometry_msgs::Pose2D cmd_vector;
  cmd_vector.x = 0;
  cmd_vector.y = 0;
  cmd_vector.theta = 0;
  cmd_pub.publish(cmd_vector);

  result.success = 1;
  action_server.setSucceeded(result);
}
  void executeCB(const corobot_face_recognition::FaceRecognitionGoalConstPtr &goal)
  {
    if (_as.isPreemptRequested() || !ros::ok()) {
      ROS_INFO("%s: Preempted", _action_name.c_str());
      // set the action state to preempted
      _as.setPreempted();
      // success = false;
      // break;
    }
    
    // helper variables
    ros::Rate r(60);
    bool success = true;

    _goal_id = goal->order_id;
    _goal_argument = goal->order_argument;

    _feedback.order_id = _goal_id;
    _feedback.names.clear();
    _feedback.confidence.clear();
    
    _result.order_id = _goal_id;
    _result.names.clear();
    _result.confidence.clear();

    // publish info to the console for the user
    ROS_INFO("%s: Executing. order_id: %i, order_argument: %s ", _action_name.c_str(), _goal_id, _goal_argument.c_str());

    switch (_goal_id) {
      // RECOGNIZE ONCE
      case 0:
        // cout << "case 0" << endl;
        // _fr = new FaceRec(true);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();

        break;
      
      // RECOGNIZE CONTINUOUSLY
      case 1:
        //cout << "case 1" << endl;
        // _fr = new FaceRec(true);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();
        
        break;
      
      // ADD TRAINING IMAGES
      case 2:
        // cout << "case 2" << endl;
        _fc = new FaceImgCapturer(_goal_argument);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::addTrainingImagesCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();

        break;
      
      // TRAIN DATABASE
      case 3:
        // call training function and check if successful
        // bool trainingSuccessful = true;
        // cout << "case 3" << endl;
        _fr->train(_preprocessing);
        if (true) 
          _as.setSucceeded(_result);
        else 
          _as.setAborted(_result);
        break;
      
      // STOP
      case 4:
        if (_as.isActive()) {
          _as.setPreempted();
          // _image_sub.shutdown();
        }
        break;

      // EXIT
      case 5:
        // cout << "case 4" << endl;
        ROS_INFO("%s: Exit request.", _action_name.c_str());
        _as.setSucceeded(_result);
        r.sleep();
        ros::shutdown();
        break;
    }

  }
Example #29
0
  void executeCB(const hector_quadrotor_msgs::followerGoalConstPtr &goal)
  {
    ROS_INFO("Client is registered, lets start the executing");
    publisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    gms_c = nh_.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    // smsl = m.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
    getmodelstate.request.model_name="quadrotor";


    geometry_msgs::Twist tw;
    publisher.publish(tw);
    ros::Duration(5.0).sleep();
    
    gms_c.call(getmodelstate);
    double now_x =  getmodelstate.response.pose.position.x;
    double now_y =  getmodelstate.response.pose.position.y;
    double now_z =  getmodelstate.response.pose.position.z;
    
    double new_x = goal->goal.position.x;
    double new_y = goal->goal.position.y;
    double new_z = goal->goal.position.z;

    double x_diff, y_diff, z_diff;
    double tmp_x, tmp_y, tmp_z;
    double var_x, var_y, var_z;

    ros::Rate r(1);
    bool success = true;
    
    // publish info to the console for the user
    
    ROS_INFO("%s: Executing!", action_name_.c_str());
    
    
    if (action_.isPreemptRequested() || !ros::ok())
      {
	ROS_INFO("%s: Preempted", action_name_.c_str());
	// set the action state to preempted
	action_.setPreempted();
	success = false;
	//break;
      }
    
    publisher.publish(tw);
    ros::Duration(2.0).sleep();
       
    
    x_diff=new_x - now_x;
    y_diff=new_y - now_y; 
    z_diff=new_z - now_z;

    // if(now_z < 0)
    //   {
    // 	now_z=now_z*(-1);
    //   z_diff=new_z - now_z;
    //   z_diff=z_diff*(-1);
    //   }else
    //   z_diff = new_z - now_z;
    
    
    // tmp_x= modf(new_x, &var_x);
    // tmp_y= modf(new_y, &var_y);
    // tmp_z= modf(new_z, &var_z);
    
    ROS_INFO(" Come Up Hector! ");
   
    if(now_z < 0.5)
      {
	tw.linear.z = 0.6;
      }
    else
      {
	tw.linear.z = 0.0;
      }
    
    publisher.publish(tw);
    ros::Duration(2.0).sleep();
    // tw.angular.z = -0.5;
    tw.linear.z = 0;
    tw.linear.x = 0;
    tw.linear.y = 0;

    // publisher.publish(tw);
    // ros::Duration(2.0).sleep();
    // tw.angular.z = 0;
    publisher.publish(tw);
    ros::Duration(2.0).sleep();	
    ROS_INFO(" Good Boy, let's go further! ");
    
    while( x_diff < -0.3 || x_diff > 0.3 || y_diff < -0.3 || y_diff > 0.3)
      {
	if(now_x < var_x || now_y < var_y)
	  {
	    
	    tw.linear.x = x_diff * 0.1;
	    tw.linear.y = y_diff * 0.1;
	  }else 
	  if(now_x > var_x || now_y > var_y)
	    {	
	      tw.linear.x =x_diff * 0.1;
	      tw.linear.y =y_diff * 0.1;
	    }
	
	publisher.publish(tw);
	ros::Duration(2.0).sleep();
	gms_c.call(getmodelstate);
	now_y =  getmodelstate.response.pose.position.y;
	y_diff=new_y - now_y;
	now_x =  getmodelstate.response.pose.position.x;
	x_diff=new_x - now_x;
      }
    

	ros::Duration(2.0).sleep();
	tw.linear.z = 0;
	tw.linear.x = 0;
	tw.linear.y = 0;
       	// tw.angular.z = -0.5;	
	publisher.publish(tw);
	
	ros::Duration(5.0).sleep();
	// tw.angular.z = 0;
	// publisher.publish(tw);
	
	// ros::Duration(5.0).sleep();
	
	action_.publishFeedback(feedback_);
	// this sleep is not necessary, the sequence is computed at 1    }
	
	if(success)
	  {
	    result_.result = feedback_.feedback;
	    ROS_INFO("%s: Succeeded", action_name_.c_str());
	    // set the action state to succeeded
	    action_.setSucceeded(result_);
	  }}
  void executeCB(const robot_actionlib::TestGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate loop_rate(25);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    // publish info to the console for the user
    ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);


    bool front = 0;
    bool right = 0;
    bool left = 0;
    double right_average = 0;
    double left_average = 0;
    
    
    while(ros::ok())
    {
      ROS_INFO("Preempted ?: %d ", as_.isPreemptRequested());
      ROS_INFO("Active ?: %d ", as_.isActive());
      ROS_INFO("GOOALLLLLL: %d ", goal->order);
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }

      
	front = (distance_front_left < front_limit) || (distance_front_right < front_limit);
        right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit);
        left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit);
        right_average = (distance_rights_front + distance_rights_back)/2;
        left_average = (distance_lefts_front + distance_lefts_back)/2;

	flagReady = true;
	if (!TestAction::flagReady) {
	  ros::spinOnce();
	  loop_rate.sleep();
	  continue;
        }

	// Update of the state
        if(front){ // The front is the most prior
	  if(right_average > left_average){
	    side = 0;
	  }else{
	    side = 1;
	  }
	  state = FRONT;
	  std::cerr << "FRONT" << std::endl;
        } else if (right) { // The right is prefered
	  /*if(state != RIGHT){
	    direction = rand()%2;
	    std::cerr << direction << std::endl;
            }*/
	  state = RIGHT;
	  std::cerr << "RIGHT" << std::endl;
        } else if (left) {
	  /*if(state != LEFT){
	    direction = rand()%2;
	    std::cerr << direction << std::endl;
            }*/
	  //direction = rand()%1;

	  state = LEFT;
	  std::cerr << "LEFT" << std::endl;
        } else {
	  state = EMPTY;
	  std::cerr << "EMPTY" << std::endl;
        }

        // Action depending on the state
        switch(state) {
        case(EMPTY): // Just go straight
	  msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
	  msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1);
	  break;
        case(FRONT):
	  msg.linear.x = 0;
	  if(side == 1){
	    msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1);
	  }else{
	    msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1);
	  }

	  break;
        case(RIGHT):
	  msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
	  msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back);
	  break;
        case(LEFT):
	  msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
	  msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back);
	  break;
        default:
	  break;
        }

        // P-controller to generate the angular velocity of the robot, to follow the wall
        //msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back);
        //ROS_INFO("Linear velocity :%f", msg.linear.x);
        //ROS_INFO("Angular velocity :%f", msg.angular.z);
        if(counter > 10){
	  twist_pub.publish(msg);
        }else {
	  counter += 1;
        }


	feedback_.sequence.push_back(feedback_.sequence[1] + feedback_.sequence[0]);
	// publish the feedback
	as_.publishFeedback(feedback_);
	
	
	

        ros::spinOnce();
        loop_rate.sleep();
	
    }
    
    if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }