C++ GoalHandle::setSucceeded方法代码示例

void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh)
{
  ROS_ERROR_NAMED("actionlib", "In callback");
  //assign to our stored goal handle
  *gh_ = gh;

  TestGoal goal = *gh.getGoal();

  switch (goal.goal)
  {
    case 1:
      gh.setAccepted();
      gh.setSucceeded(TestResult(), "The ref server has succeeded");
      break;
    case 2:
      gh.setAccepted();
      gh.setAborted(TestResult(), "The ref server has aborted");
      break;
    case 3:
      gh.setRejected(TestResult(), "The ref server has rejected");
      break;
    default:
      break;
  }

  ros::shutdown();
}

  void goalCB(GoalHandle gh)
  {
    std::string nodeName = ros::this_node::getName();

    ROS_INFO("%s",(nodeName + ": Received grasping goal").c_str());

	switch(gh.getGoal()->goal)
	{
		case GraspHandPostureExecutionGoal::PRE_GRASP:
			gh.setAccepted();
			//gh.getGoal()->grasp
			ROS_INFO("%s",(nodeName + ": Pre-grasp command accepted").c_str());
			gh.setSucceeded();
			break;

		case GraspHandPostureExecutionGoal::GRASP:
			gh.setAccepted();
			ROS_INFO("%s",(nodeName + ": Executing a gripper grasp").c_str());

			// wait
			ros::Duration(0.25f).sleep();
			gh.setSucceeded();
			break;

		case GraspHandPostureExecutionGoal::RELEASE:
			gh.setAccepted();
			ROS_INFO("%s",(nodeName + ": Executing a gripper release").c_str());

			// wait
			ros::Duration(0.25f).sleep();
			gh.setSucceeded();
			break;

		default:

			ROS_INFO("%s",(nodeName + ": Unidentified grasp request, rejecting goal").c_str());
			gh.setSucceeded();
			//gh.setRejected();
			break;
	}

  }

  void goalCB(GoalHandle gh)
  {
    // Ensures that the joints in the goal match the joints we are commanding.
    ROS_DEBUG("Received goal: goalCB");
    if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names))
    {
      ROS_ERROR("Joints on incoming goal don't match our joints");
      gh.setRejected();
      return;
    }

    // Cancels the currently active goal.
    if (has_active_goal_)
    {
      ROS_DEBUG("Received new goal, canceling current goal");
      // Stops the controller.
      trajectory_msgs::JointTrajectory empty;
      empty.joint_names = joint_names_;
      pub_controller_command_.publish(empty);

      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    // Sends the trajectory along to the controller
    if (withinGoalConstraints(last_controller_state_, goal_constraints_, gh.getGoal()->trajectory))
    {
      ROS_INFO_STREAM("Already within goal constraints");
      gh.setAccepted();
      gh.setSucceeded();
    }
    else
    {
      gh.setAccepted();
      active_goal_ = gh;
      has_active_goal_ = true;

      ROS_INFO("Publishing trajectory");

      current_traj_ = active_goal_.getGoal()->trajectory;
      pub_controller_command_.publish(current_traj_);
    }
  }

  void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr &msg)
  {
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
      return;

    pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal;
    goal.command = active_goal_.getGoal()->command;

    pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback;
    feedback.data = *msg;

    pr2_gripper_sensor_msgs::PR2GripperFindContactResult result;
    result.data = *msg;

    // do not check until some dT has expired from message start
    double dT = 0.2;
    if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){}


    // if we are actually in a find_contact control state or positoin control state
    else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3)
    {
      if(feedback.data.contact_conditions_met)
      {
        active_goal_.setSucceeded(result);
        has_active_goal_ = false;
      }
    }

    else
      has_active_goal_ = false;

    active_goal_.publishFeedback(feedback);
  }

  void goalCB(GoalHandle gh)
  {
    GripperMessage gMsg;
    SimpleMessage request;
    SimpleMessage reply;

    ROS_DEBUG("Received grasping goal");

    while (!(robot_->isConnected()))
    {
      ROS_DEBUG("Reconnecting");
      robot_->makeConnect();
    }
      

    switch(gh.getGoal()->goal)
    {
      case GraspHandPostureExecutionGoal::PRE_GRASP:

        gh.setAccepted();
        ROS_WARN("Pre-grasp is not supported by this gripper");
        gh.setSucceeded();
        break;

      case GraspHandPostureExecutionGoal::GRASP:
      case GraspHandPostureExecutionGoal::RELEASE:

        gh.setAccepted();
        switch(gh.getGoal()->goal)
        {
          case GraspHandPostureExecutionGoal::GRASP:
            ROS_INFO("Executing a gripper grasp");
            gMsg.init(GripperOperationTypes::CLOSE);
            break;
          case GraspHandPostureExecutionGoal::RELEASE:
            ROS_INFO("Executing a gripper release");
            gMsg.init(GripperOperationTypes::OPEN);
            break;
        }
        gMsg.toRequest(request);
        this->robot_->sendAndReceiveMsg(request, reply);

        switch(reply.getReplyCode())
        {
          case ReplyTypes::SUCCESS:
            ROS_INFO("Robot gripper returned success");
            gh.setSucceeded();
            break;
          case ReplyTypes::FAILURE:
            ROS_ERROR("Robot gripper returned failure");
            gh.setCanceled();
            break;
        }
        break;

          default:
            gh.setRejected();
            break;

    }
  }

  void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
  {
    //ROS_DEBUG("Checking controller state feedback");
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
    {
      //ROS_DEBUG("No active goal, ignoring feedback");
      return;
    }
    if (current_traj_.points.empty())
    {
      ROS_DEBUG("Current trajectory is empty, ignoring feedback");
      return;
    }
    /* NOT CONCERNED ABOUT TRAJECTORY TIMING AT THIS POINT
     if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start)
     return;
     */

    if (!setsEqual(joint_names_, msg->joint_names))
    {
      ROS_ERROR("Joint names from the controller don't match our joint names.");
      return;
    }

    // Checking for goal constraints
    // Checks that we have ended inside the goal constraints and has motion stopped

    ROS_DEBUG("Checking goal constraints");
    if (withinGoalConstraints(msg, goal_constraints_, current_traj_))
    {
      // Additional check for motion stoppage since the controller goal may still
      // be moving.  The current robot driver calls a motion stop if it receives
      // a new trajectory while it is still moving.  If the driver is not publishing
      // the motion state (i.e. old driver), this will still work, but it warns you.
      if (last_robot_status_.in_motion.val == industrial_msgs::TriState::FALSE)
      {
        ROS_INFO("Inside goal constraints, stopped moving, return success for action");
        active_goal_.setSucceeded();
        has_active_goal_ = false;
      }
      else if (last_robot_status_.in_motion.val == industrial_msgs::TriState::UNKNOWN)
      {
        ROS_INFO("Inside goal constraints, return success for action");
        ROS_WARN("Robot status: in motion unknown, the robot driver node and controller code should be updated");
        active_goal_.setSucceeded();
        has_active_goal_ = false;
      }
      else
      {
        ROS_DEBUG("Within goal constraints but robot is still moving");
      }
    }
    // Verifies that the controller has stayed within the trajectory constraints.
    /*  DISABLING THIS MORE COMPLICATED GOAL CHECKING AND ERROR DETECTION


     int last = current_traj_.points.size() - 1;
     ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start;
     if (now < end_time)
     {
     // Checks that the controller is inside the trajectory constraints.
     for (size_t i = 0; i < msg->joint_names.size(); ++i)
     {
     double abs_error = fabs(msg->error.positions[i]);
     double constraint = trajectory_constraints_[msg->joint_names[i]];
     if (constraint >= 0 && abs_error > constraint)
     {
     // Stops the controller.
     trajectory_msgs::JointTrajectory empty;
     empty.joint_names = joint_names_;
     pub_controller_command_.publish(empty);

     active_goal_.setAborted();
     has_active_goal_ = false;
     ROS_WARN("Aborting because we would up outside the trajectory constraints");
     return;
     }
     }
     }
     else
     {
     // Checks that we have ended inside the goal constraints
     bool inside_goal_constraints = true;
     for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
     {
     double abs_error = fabs(msg->error.positions[i]);
     double goal_constraint = goal_constraints_[msg->joint_names[i]];
     if (goal_constraint >= 0 && abs_error > goal_constraint)
     inside_goal_constraints = false;

     // It's important to be stopped if that's desired.
     if (fabs(msg->desired.velocities[i]) < 1e-6)
     {
     if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
     inside_goal_constraints = false;
     }
     }
//.........这里部分代码省略.........

  void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
  {
    //ROS_DEBUG("Checking controller state feedback");
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
    {
      //ROS_DEBUG("No active goal, ignoring feedback");
      return;
    }
    if (current_traj_.points.empty())
    {
      ROS_DEBUG("Current trajecotry is empty, ignoring feedback");
      return;
    }
    /* NOT CONCERNED ABOUT TRAJECTORY TIMING AT THIS POINT
    if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start)
      return;
    */

    if (!setsEqual(joint_names_, msg->joint_names))
    {
      ROS_ERROR("Joint names from the controller don't match our joint names.");
      return;
    }

    // Checking for goal constraints
    // Checks that we have ended inside the goal constraints

    ROS_DEBUG("Checking goal contraints");
    if(withinGoalConstraints(msg, 
                             goal_constraints_,
                             current_traj_)) {
      ROS_INFO("Inside goal contraints, return success for action");
      active_goal_.setSucceeded();
      has_active_goal_ = false;
    }
    // Verifies that the controller has stayed within the trajectory constraints.
    /*  DISABLING THIS MORE COMPLICATED GOAL CHECKING AND ERROR DETECTION


    int last = current_traj_.points.size() - 1;
    ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start;
    if (now < end_time)
    {
      // Checks that the controller is inside the trajectory constraints.
      for (size_t i = 0; i < msg->joint_names.size(); ++i)
      {
        double abs_error = fabs(msg->error.positions[i]);
        double constraint = trajectory_constraints_[msg->joint_names[i]];
        if (constraint >= 0 && abs_error > constraint)
        {
          // Stops the controller.
          trajectory_msgs::JointTrajectory empty;
          empty.joint_names = joint_names_;
          pub_controller_command_.publish(empty);

          active_goal_.setAborted();
          has_active_goal_ = false;
          ROS_WARN("Aborting because we would up outside the trajectory constraints");
          return;
        }
      }
    }
    else
    {
      // Checks that we have ended inside the goal constraints
      bool inside_goal_constraints = true;
      for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i)
      {
        double abs_error = fabs(msg->error.positions[i]);
        double goal_constraint = goal_constraints_[msg->joint_names[i]];
        if (goal_constraint >= 0 && abs_error > goal_constraint)
          inside_goal_constraints = false;

        // It's important to be stopped if that's desired.
        if (fabs(msg->desired.velocities[i]) < 1e-6)
        {
          if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_)
            inside_goal_constraints = false;
        }
      }

      if (inside_goal_constraints)
      {
        active_goal_.setSucceeded();
        has_active_goal_ = false;
      }
      else if (now < end_time + ros::Duration(goal_time_constraint_))
      {
        // Still have some time left to make it.
      }
      else
      {
        ROS_WARN("Aborting because we wound up outside the goal constraints");
        active_goal_.setAborted();
        has_active_goal_ = false;
      }

//.........这里部分代码省略.........

//.........这里部分代码省略.........
                    traj[k][4] = max_vel;
                  }
                  else if (traj[k][4]<min_vel)
                  {
                    traj[k][4] = min_vel;
                  }
		
		  //Z-Velocity
                  traj[k][5] /= dt;
                  if (traj[k][5]>max_vel)
                  {
                    traj[k][5] = max_vel;
                  }
                  else if (traj[k][5]<min_vel)
                  {
                    traj[k][5] = min_vel;
                  }

                }


		//Post process in narrow edges
		std::vector<double> edges;
		for(unsigned i=1; i<n_points-1;i++)
		{
		  //Get the angles between two consecutives velocity vectors
		  //Source:http://de.mathworks.com/matlabcentral/answers/16243-angle-between-two-vectors-in-3d
		  double cross_i = traj[i][4]*traj[i-1][5] - traj[i][5]*traj[i-1][4];
		  double cross_j = (-1)*(traj[i][3]*traj[i-1][5] - traj[i][5]*traj[i-1][3]);
		  double cross_k = traj[i][3]*traj[i-1][4] - traj[i][4]*traj[i-1][3];
		  double dot = traj[i][3]*traj[i-1][3] + traj[i][4]*traj[i-1][4] + traj[i][5]*traj[i-1][5];
		  double norm_cross = sqrt(cross_i*cross_i + cross_j*cross_j + cross_k*cross_k);

		  double angle = atan2(norm_cross,dot)*180/M_PI;		 
		  //ROS_INFO_STREAM(angle); 
		  if (angle >= 25)
		  {
		    edges.push_back(i);
		  }
		}		

		for(int i=0; i<edges.size(); i++)
		{
		  ROS_INFO_STREAM(edges[i]);
		}


		std::string dir = path + "/trajectory_ompl.csv";
                //Open file to write trajectory
                std::ofstream myFile;
                myFile.open(dir.c_str());
                //Save all the values separated by comas except the last
                for(unsigned i=0; i<n_points-2;i++)
                {
                  myFile << traj[i][0] <<","
                         << traj[i][1] <<","
                         << traj[i][2] <<","
			  /*
			 << 0.0 <<","
                         << 0.0 <<","
                         << 0.0 <<",\n";
			 */
                         << traj[i][3] <<","
                         << traj[i][4] <<","
                         << traj[i][5] <<",\n";
			 

                }
                //Which is saved here, but without a coma in the last element.
                int i = n_points-1;

                        myFile    << traj[i][0] <<","
                                  << traj[i][1] <<","
                                  << traj[i][2] <<","
                                  << 0.0 <<","
                                  << 0.0 <<","
                                  << 0.0 <<",\n";
                myFile.close();

 		/*
   		//Show trajectory
 		for (int k=0; k<toExecute.points.size(); k++)
                {
                  ROS_INFO_STREAM(traj[k][0]<<" "<<traj[k][1] <<" "<<traj[k][2] <<" "<<
		  		  traj[k][3]<<" "<<traj[k][4] <<" "<<traj[k][5]);
                }
		*/

		//std::string command = "rosrun quad_gazebo trajectory_client ompl";
	        //system(command.c_str());
                active_goal_.setSucceeded();
                has_active_goal_=false;
                creato=0;
  		ROS_INFO_STREAM("Calling the client to execute the trajectory, please wait...");
                std::string command = "rosrun quad_gazebo trajectory_client ompl";
                system(command.c_str());



	}

  void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
  {
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
      return;

    // grab the goal threshold off the param server
    if(!node_.getParam("position_servo_position_tolerance", goal_threshold_))
        ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str());


    // Ensures that the controller is tracking my setpoint.
    if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_)
    {
      if (now - goal_received_ < ros::Duration(1.0))
      {
        return;
      }
      else
      {
        ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint.");
        active_goal_.setCanceled();
        has_active_goal_ = false;
      }
    }


    pr2_controllers_msgs::Pr2GripperCommandFeedback feedback;
    feedback.position = msg->process_value;
    feedback.effort = msg->command;
    feedback.reached_goal = false;
    feedback.stalled = false;

    pr2_controllers_msgs::Pr2GripperCommandResult result;
    result.position = msg->process_value;
    result.effort = msg->command;
    result.reached_goal = false;
    result.stalled = false;

    // check if we've reached the goal
    if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_)
    {
      feedback.reached_goal = true;

      result.reached_goal = true;
      active_goal_.setSucceeded(result);
      has_active_goal_ = false;
    }
    else
    {
      // Determines if the gripper has stalled.
      if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
      {
        last_movement_time_ = ros::Time::now();
      }
      else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ &&
               active_goal_.getGoal()->command.max_effort != 0.0)
      {
        feedback.stalled = true;

        result.stalled = true;
        active_goal_.setAborted(result);
        has_active_goal_ = false;
      }
    }
    active_goal_.publishFeedback(feedback);
  }