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

  void goalCB(GoalHandle gh)
  {
    // Cancels the currently active goal.
    if (has_active_goal_)
    {
      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    gh.setAccepted();
    active_goal_ = gh;
    has_active_goal_ = true;
    goal_received_ = ros::Time::now();

    // update our zero values for 0.25 seconds
    if(active_goal_.getGoal()->command.zero_fingertip_sensors)
    {
      std_srvs::Empty::Request req;
      std_srvs::Empty::Response resp;
      if(ros::service::exists("zero_fingertip_sensors",true))
      {
        ROS_INFO("updating zeros!");
        ros::service::call("zero_fingertip_sensors",req,resp);
      }
    }
    
    // Sends the command along to the controller.
    pub_controller_command_.publish(active_goal_.getGoal()->command);
    
    last_movement_time_ = ros::Time::now();
    action_start_time = ros::Time::now();
  }

  void cancelCB(GoalHandle gh)
  {
    if (active_goal_ == gh)
    {
      /*
      // Stops the controller.
      if (last_controller_state_)
      {
        pr2_controllers_msgs::Pr2GripperCommand stop;
        stop.position = last_controller_state_->process_value;
        stop.max_effort = 0.0;
        pub_controller_command_.publish(stop);
      }
      */
      // stop the real-time motor control
      std_srvs::Empty::Request req;
      std_srvs::Empty::Response resp;
      if(ros::service::exists("stop_motor_output",true))
      {
        ROS_INFO("Stopping Motor Output");
        ros::service::call("stop_motor_output",req,resp);
      }

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

  void cancelCB(GoalHandle gh)
  {
    std::string nodeName = ros::this_node::getName();
	ROS_INFO("%s",(nodeName + ": Canceling current grasp action").c_str());
    //gh.setAccepted();
    gh.setCanceled();
    ROS_INFO("%s",(nodeName + ": Current grasp action has been canceled").c_str());
  }

	void cancelCB(GoalHandle gh){
		if (active_goal_ == gh)
		{
			// Stops the controller.
			if(creato){
				ROS_INFO_STREAM("Stop thread");
				pthread_cancel(trajectoryExecutor);
				creato=0;
			}
			pub_topic.publish(empty);

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

  void cancelCB(GoalHandle gh)
  {
    
    if (active_goal_ == gh)
    {
      // stop the real-time motor control
      std_srvs::Empty::Request req;
      std_srvs::Empty::Response resp;
      if(ros::service::exists("stop_motor_output",true))
      {
        ROS_INFO("Stopping Motor Output");
        ros::service::call("stop_motor_output",req,resp);
      }

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

  void goalCB(GoalHandle gh)
  {
    // Cancels the currently active goal.
    if (has_active_goal_)
    {
      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    gh.setAccepted();
    active_goal_ = gh;
    has_active_goal_ = true;
    goal_received_ = ros::Time::now();
    min_error_seen_ = 1e10;

    // Sends the command along to the controller.
    pub_controller_command_.publish(active_goal_.getGoal()->command);
    last_movement_time_ = ros::Time::now();
  }

  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 goalCB(GoalHandle gh){
		if (has_active_goal_)
		{
			// Stops the controller.
			if(creato){
				pthread_cancel(trajectoryExecutor);
				creato=0;
			}
			pub_topic.publish(empty);

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

		gh.setAccepted();
		active_goal_ = gh;
		has_active_goal_ = true;
		toExecute = gh.getGoal()->trajectory;
		//moveit::planning_interface::MoveGroup group_quad("quad_group");

		ROS_INFO_STREAM("Trajectory received, processing"); 
		int n_points = toExecute.points.size();
		double meters = 0.0;
		double vel_cte = 0.5;
		double t = 0.0;
		double dt = 0.0;
		double traj [n_points][6];
		
		//Get Position of Waypoints
		for (int k=0; k<toExecute.points.size(); k++)
		{
                  geometry_msgs::Transform_<std::allocator<void> > punto=toExecute.points[k].transforms[0];
		  traj[k][0] = punto.translation.x;
		  traj[k][1] = punto.translation.y;
		  traj[k][2] = punto.translation.z;
		}

		//Determine distance between consecutive Waypoints
		//to get aproximate path lenght
                for (int k=0; k<toExecute.points.size()-1; k++)
                {
                  traj[k][3] = traj[k+1][0] - traj[k][0];
                  traj[k][4] = traj[k+1][1] - traj[k][1];
                  traj[k][5] = traj[k+1][2] - traj[k][2];
		  
		  meters += sqrt(traj[k][3]*traj[k][3]+traj[k][4]*traj[k][4]+traj[k][5]*traj[k][5]);
          	}
		ROS_INFO_STREAM("Aprox distance: "<< meters << " meters");

		//Determine time in cte velocity profile
		t = meters/vel_cte;

		//We suppose equal separation between waypoint, so we
		//can get the time we have between each waypoint
		dt = t/(toExecute.points.size()-1);

		//Determine the velocity between waypoints
		double max_vel = 0.3;
		double min_vel = -0.3;
		for (int k=0; k<toExecute.points.size()-1; k++)
                {
		  //X-Velocity 
                  traj[k][3] /= dt;
		  if (traj[k][3]>max_vel)
		  {
		    traj[k][3] = max_vel;
		  }
		  else if (traj[k][3]<min_vel)
		  {
		    traj[k][3] = min_vel;
		  }

		  //Y-Velocity
                  traj[k][4] /= dt;
		  if (traj[k][4]>max_vel)
                  {
                    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;
//.........这里部分代码省略.........

  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);
  }