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

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)
  {
    // Ensures that the joints in the goal match the joints we are commanding.
    ROS_DEBUG("Received goal: goalCB");
    ROS_INFO("Received goal: goalCB");
    if (!is_joint_set_ok(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;
    }

    gh.setAccepted();
    active_goal_ = gh;
    has_active_goal_ = true;

    // Sends the trajectory along to the controller
    ROS_INFO("Publishing trajectory; sending commands to the joint controller ");
    current_traj_ = active_goal_.getGoal()->trajectory;
    pub_controller_command_.publish(current_traj_);
  }

  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 BaseTrajectoryActionController::goalCB(GoalHandle gh)
{
  ROS_DEBUG("goalCB");

  std::vector<std::string> joint_names(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    joint_names[j] = joints_[j]->name;

  // Ensures that the joints in the goal match the joints we are commanding.
  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 (active_goal_)
  {
    // Marks the current goal as canceled.
    active_goal_->setCanceled();
  }

  starting(); // reset trajectory

  gh.setAccepted();

  // Sends the trajectory along to the controller
  active_goal_ = boost::shared_ptr<GoalHandle>(new GoalHandle(gh));
  commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_);
}

  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 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 CartesianTrajectoryAction::goalCB(GoalHandle gh) {
  // cancel active goal
  if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) {
    active_goal_.setCanceled();
  }

  Goal g = gh.getGoal();

  CartesianTrajectory* trj_ptr =  new CartesianTrajectory;
  *trj_ptr = g->trajectory;
  CartesianTrajectoryConstPtr trj_cptr = CartesianTrajectoryConstPtr(trj_ptr);
  port_cartesian_trajectory_command_.write(trj_cptr);

  gh.setAccepted();
  active_goal_ = gh;
}

  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)
  {
    ROS_DEBUG("GoalHandle request received");

    // accept new goals
    gh.setAccepted();

    // get goal from handle
    const tf2_web_republisher::TFSubscriptionGoal::ConstPtr& goal = gh.getGoal();

    // generate goal_info struct
    boost::shared_ptr<ClientGoalInfo> goal_info = boost::make_shared<ClientGoalInfo>();
    goal_info->handle = gh;
    goal_info->client_ID_ = client_ID_count_++;
    goal_info->timer_ = nh_.createTimer(ros::Duration(1.0 / goal->rate),
                                        boost::bind(&TFRepublisher::processGoal, this, goal_info, _1));

    std::size_t request_size_ = goal->source_frames.size();
    goal_info->tf_subscriptions_.resize(request_size_);

    for (std::size_t i=0; i<request_size_; ++i )
    {
      TFPair& tf_pair = goal_info->tf_subscriptions_[i];

      std::string source_frame = cleanTfFrame(goal->source_frames[i]);
      std::string target_frame = cleanTfFrame(goal->target_frame);

      tf_pair.setSourceFrame(source_frame);
      tf_pair.setTargetFrame(target_frame);
      tf_pair.setAngularThres(goal->angular_thres);
      tf_pair.setTransThres(goal->trans_thres);
    }

    {
      boost::mutex::scoped_lock l(goals_mutex_);
      // add new goal to list of active goals/clients
      active_goals_.push_back(goal_info);
    }

  }

void CartesianImpedanceAction::goalCB(GoalHandle gh) {
  // cancel active goal
  if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) {
    active_goal_.setCanceled();
  }

  Goal g = gh.getGoal();

  bool valid = true;

  for (size_t i = 0; i < g->trajectory.points.size(); i++) {
    valid = valid && checkImpedance(g->trajectory.points[i].impedance);
  }

  if (!valid) {
    cartesian_trajectory_msgs::CartesianImpedanceResult res;
    res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::INVALID_GOAL;
    gh.setRejected(res);
  }

  CartesianImpedanceTrajectory* trj_ptr =  new CartesianImpedanceTrajectory;
  *trj_ptr = g->trajectory;
  CartesianImpedanceTrajectoryConstPtr trj_cptr = CartesianImpedanceTrajectoryConstPtr(trj_ptr);

  if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
    valid = false;
    cartesian_trajectory_msgs::CartesianImpedanceResult res;
    res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::OLD_HEADER_TIMESTAMP;
    gh.setRejected(res);
  }

  if (valid) {
    port_cartesian_trajectory_command_.write(trj_cptr);

    gh.setAccepted();
    active_goal_ = gh;
  }
}

  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;

    }
  }

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

    // Sprawdzenie ograniczeń w jointach INVALID_GOAL
    bool invalid_goal = false;
    for (unsigned int i = 0; i < numberOfJoints_; i++) {
      for (int j = 0; j < g->trajectory.points.size(); j++) {
        if (g->trajectory.points[j].positions[i] > upperLimits_[remapTable_[i]]
            || g->trajectory.points[j].positions[i]
                < lowerLimits_[remapTable_[i]]) {
          RTT::Logger::log(RTT::Logger::Debug)
              << "Invalid goal [" << i << "]: " << upperLimits_[remapTable_[i]]
              << ">" << g->trajectory.points[j].positions[i] << ">"
              << lowerLimits_[remapTable_[i]] << RTT::endlog();
          invalid_goal = true;
        }
      }
    }

    if (invalid_goal) {
      RTT::Logger::log(RTT::Logger::Debug)
          << "Trajectory contains invalid goal!" << RTT::endlog();
      res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
      gh.setRejected(res, "");
      goal_active_ = false;
      return;
    }

    // Remap joints
    trj_ptr->header = g->trajectory.header;
    trj_ptr->points.resize(g->trajectory.points.size());

    for (unsigned int i = 0; i < g->trajectory.points.size(); i++) {
      trj_ptr->points[i].positions.resize(
          g->trajectory.points[i].positions.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].positions.size();
          j++) {
        trj_ptr->points[i].positions[j] =
            g->trajectory.points[i].positions[remapTable_[j]];
      }

      trj_ptr->points[i].velocities.resize(
          g->trajectory.points[i].velocities.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].velocities.size();
          j++) {
        trj_ptr->points[i].velocities[j] =
            g->trajectory.points[i].velocities[remapTable_[j]];
      }

      trj_ptr->points[i].accelerations.resize(
          g->trajectory.points[i].accelerations.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].accelerations.size();
          j++) {
        trj_ptr->points[i].accelerations[j] = g->trajectory.points[i]
            .accelerations[remapTable_[j]];
      }

      trj_ptr->points[i].time_from_start = g->trajectory.points[i]
          .time_from_start;
    }

    // Sprawdzenie czasu w nagłówku OLD_HEADER_TIMESTAMP
    if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
      RTT::Logger::log(RTT::Logger::Debug) << "Old header timestamp"
                                           << RTT::endlog();
      res.error_code =
          control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP;
      gh.setRejected(res, "");
    }

    trajectory_finish_time_ = g->trajectory.header.stamp
        + g->trajectory.points[g->trajectory.points.size() - 1].time_from_start;

    activeGoal_ = gh;
    goal_active_ = true;

    bool ok = true;

    RTT::TaskContext::PeerList peers = this->getPeerList();
    for (size_t i = 0; i < peers.size(); i++) {
      RTT::Logger::log(RTT::Logger::Debug) << "Starting peer : " << peers[i]
                                           << RTT::endlog();
      ok = ok && this->getPeer(peers[i])->start();
    }

    if (ok) {
      trajectory_msgs::JointTrajectoryConstPtr trj_cptr =
          trajectory_msgs::JointTrajectoryConstPtr(trj_ptr);

      trajectory_ptr_port_.write(trj_cptr);

      gh.setAccepted();
      goal_active_ = true;
    } else {
      gh.setRejected();
      goal_active_ = false;
    }
  } else {
    gh.setRejected();
  }
}

  void GripperAction::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;
          std::cout << "canceled current active goal" << std::endl;
      }

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

      ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort);

      ROS_INFO("setFingersValues");
    
      std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle();

      std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl;

        target_position = active_goal_.getGoal()->command.position;
        target_effort = active_goal_.getGoal()->command.max_effort;

        //determine if the gripper is supposed to be opened or closed
        double current_position = jaco_->getFingersJointAngle()[0];
        if(current_position > target_position){
            opening = true;
            std::cout << "Opening gripper" << std::endl;
        }else{
            opening = false;
            std::cout << "Closing gripper" << std::endl;
        }

        

        fingerPositionsRadian[0] = target_position;
        fingerPositionsRadian[1] = target_position;
        fingerPositionsRadian[2] = target_position;

        

        std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl;


      double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])};

      if(!jaco_->setFingersValues(fingerPositionsDegree))
      {
    	  active_goal_.setCanceled();
    	  ROS_ERROR("Cancelling goal: moveJoint didn't work.");
          std::cout << "Cancelling goal because setFingers didn't work" << std::endl;
      }

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

	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;
//.........这里部分代码省略.........