C++ SimpleActionServer::isActive方法代码示例

  void processGoal(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose )
  {
    // Change the goal constraints on the servos to be less strict, so that the controllers don't die. this is a hack
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/elbow_pitch_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/shoulder_pan_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/gripper_roll_joint/goal", 2); // originally it was 0.45
    nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45

    if( !pickAndPlace(start_block_pose, end_block_pose) )
    {
      ROS_ERROR_STREAM_NAMED("pick_place_moveit","Pick and place failed");

      if(action_server_.isActive()) // Make sure we haven't sent a fake goal
      {
        // Report failure
        result_.success = false;
        action_server_.setSucceeded(result_);
      }
    }
    else
    {
      if(action_server_.isActive()) // Make sure we haven't sent a fake goal
      {
        // Report success
        result_.success = true;
        action_server_.setSucceeded(result_);
      }
    }

    // TODO: remove
    ros::shutdown();
  }

  void trackBucketPointCB(const geometry_msgs::Point::ConstPtr& msg)
  {
	  // make sure that the action hasn't been canceled
	  if (!as_.isActive())
		  return;
	  bucket_detected_ = true;
  }

 void ultraSonicsCB(const std_msgs::Int32::ConstPtr& dist)
 {
     // make sure that the action hasn't been canceled
     if (!as_.isActive())
         return;
     obstacle_detected_ = false;
     if ( dist->data > 0 ){
         obstacle_detected_ = true;
     }
 }

 void trackAreaCB(const std_msgs::Int32::ConstPtr& area)
 {
     // make sure that the action hasn't been canceled
     if (!as_.isActive())
         return;
     drop_flag = false;
     if ( area->data > 38000 ){
         drop_flag = true;
     }
 }

    /**
     * @brief Preempt callback for the server, cancels the current running goal and all associated movement actions.
     */
    void preemptCb(){

        boost::unique_lock<boost::mutex> lock(move_client_lock_);
        move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
        ROS_WARN("Current exploration task cancelled");

        if(as_.isActive()){
            as_.setPreempted();
        }

    }

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

  void trackIRCB(const std_msgs::Int32::ConstPtr& msg)
  {
	  // make sure that the action hasn't been canceled
	  if (!as_.isActive())
		  return;
      if ( msg->data == 1 )
      {
          entered_ir = true;
      }
      if (entered_ir && (msg->data == 0))
      {
          ir_detected_ = true;
          entered_ir = false;
      }
  }

  void addBlocks(const geometry_msgs::PoseArrayConstPtr& msg)
  {
    interactive_m_server_.clear();
    interactive_m_server_.applyChanges();

    ROS_INFO("[block logic] Got block detection callback. Adding blocks.");
    geometry_msgs::Pose block;
    bool active = action_server_.isActive();

    for (unsigned int i=0; i < msg->poses.size(); i++)
    {
      block = msg->poses[i];
      addBlock(block, i, active, msg->header.frame_id);
    }
    ROS_INFO("[block logic] Added %d blocks to Rviz", int(msg->poses.size()));

    interactive_m_server_.applyChanges();

    msg_ = msg;
    initialized_ = true;
  }

  // Move the real block!
  void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
  {
    if (!action_server_.isActive())
    {
      ROS_INFO("[block logic] Got feedback but not active!");
      return;
    }
    switch ( feedback->event_type )
    {
    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
      ROS_INFO_STREAM("[block logic] Staging " << feedback->marker_name);
      old_pose_ = feedback->pose;
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
      ROS_INFO_STREAM("[block logic] Now moving " << feedback->marker_name);
      moveBlock(old_pose_, feedback->pose);
      break;
    }

    interactive_m_server_.applyChanges();
  }

  void trackPointCB(const geometry_msgs::Point::ConstPtr& msg)
  {
	  // make sure that the action hasn't been canceled
	  if (!as_.isActive())
	  {
		  twist_.angular.z = 0;
		  twist_.linear.x = 0;
		  twist_pub_.publish(twist_);
		  return;
	  }

	  if( !has_goal_)
	  {
		  twist_.angular.z = 0;
		  twist_.linear.x = 0;
		  twist_pub_.publish(twist_);
		  return;
	  }

	  if( obstacle_detected_ && msg->y < SVTIUS)
	  {
		  result_.success = false;
		  ROS_INFO("%s: Obsticle Detected", action_name_.c_str());
		  // set the action state to succeeded
		  as_.setSucceeded(result_);
	  }

	  feedback_.success = false;
	  twist_.angular.z = 0;
	  twist_.linear.x = 0;

	  if ( msg->y >= TOP_T
		&& msg->x <= RIGHT_T
		&& msg->x >= LEFT_T )
	  {
		  twist_pub_.publish(twist_);
		  ROS_INFO("%s: Succeeded", action_name_.c_str());
		  // set the action state to succeeded
		  result_.success = true;
		  as_.setSucceeded(result_);
	  }

	  if(drop_flag) {
	  	twist_pub_.publish(twist_);
		  ROS_INFO("%s: Succeeded", action_name_.c_str());
		  // set the action state to succeeded
		  result_.success = true;
		  drop_flag = false;
		  as_.setSucceeded(result_);
	  }

	  if (msg->y < TOP_T)
	  {
		  ROS_INFO("MOVE FORWARD");
		  twist_.linear.x = getLinVel(msg->y, TOP_T);
	  }

	  if (msg->x < LEFT_T)
	  {
		  ROS_INFO("TURN RIGHT");
		  twist_.angular.z = getAngVel(msg->x, LEFT_T);
	  }
	  else if (msg->x  > RIGHT_T)
	  {
		  ROS_INFO("TURN LEFT");
		  twist_.angular.z = -1.0 * getAngVel(msg->x, RIGHT_T);
	  }

	  twist_pub_.publish(twist_);
	  as_.publishFeedback(feedback_);
  }

  void Update(const ros::TimerEvent& e) {
    if(!server_.isActive() ||  is_running)
      return;
    
    if(got_goals)
      {
	is_running = true;
	ROS_INFO("Update--> Got goals of size %d", num_feature_goals_);
	feedback_.features.resize(num_feature_goals_);
	feedback_.signal.resize(num_feature_goals_);
	//	feedback_.signal.resize(num_feature_goals_);

	for(int i=0; i<num_feature_goals_; i++)
	  {
	    feedback_.signal[i].signal_type = "hue";
	    feedback_.signal[i].mean.resize(1);
	    feedback_.signal[i].variance.resize(1);
	    feedback_.signal[i].mean[0] = feature_mean_goals_[i];
	    feedback_.signal[i].variance[0] = feature_var_goals_[i];
	    feedback_.signal[i].size = 1;

	    mean_color = feature_mean_goals_[i];
	    window_size = feature_var_goals_[i];
	    int rangeMin = ((mean_color - window_size + 255)%255);
	    int rangeMax = ((mean_color + window_size + 255)%255);
	    ROS_INFO("Range [%d] : %d,%d",i,rangeMin,rangeMax);
	    ROS_INFO("Threshold : %d,%d",minCCThreshold,maxCCThreshold);
	    
	    if(rangeMin > rangeMax){
	      int temp = rangeMax;
	      rangeMax = rangeMin;
	      rangeMin = temp;
	    }
	    
	    if(minCCThreshold >= maxCCThreshold){
	      //ROS_INFO("Min radius must be smaller than Max radius");
	      is_running = false;
	      return;
	    }
	    
	    if(fabs(rangeMin - rangeMax) <= 2*window_size){
	      //ROS_INFO("REG rangeMin %d rangeMax %d", rangeMin, rangeMax);
	      inRange(*hue_image, Scalar((double)((uchar)rangeMin)),Scalar((double)((uchar)rangeMax)), *back_img);
	    }
	    else if ((mean_color + window_size) > 255){
	      //ROS_INFO("BIG rangeMin %d rangeMax %d", rangeMin, rangeMax);
	      inRange(*hue_image, Scalar((double)((uchar)rangeMax)),Scalar((double)((uchar)255)), *back_img);
	    }
	    else {
	      //ROS_INFO("SML rangeMin %d rangeMax %d", rangeMin, rangeMax);
	      inRange(*hue_image, Scalar((double)((uchar)0)),Scalar((double)((uchar)rangeMin)), *back_img);
	    }  
	    
	    Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
	    ROS_INFO("Adding Gaussian Blur");
	    GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);
	    
	    ROS_INFO("Thresholding --->");
	    threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY); 
	    convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
	    
	    //Find Connected Components
	    ROS_INFO("Finding Connected Comps for feature %d",i);
	    getConnectedComponents(i);
	    ROS_INFO("After finding conn comps, num[%d] : %d",i,numCC[i]);
	    feedback_.features[i].num = numCC[i];
	    if(numCC[i] > 0)
	      feedback_.features[i].moments.resize(numCC[i]);
	  
	    ROS_INFO("Getting Moments");
	    if(numCC[i] > 0)
	      getMoments(i);

	    ROS_INFO("Updating KF ");
	    blobTracker[i].updateKalmanFiltersConnectedComponents();
	    if (numCC[i] > 0)
	      blobTracker[i].getFilteredBlobs(true);
	    else
	      blobTracker[i].getFilteredBlobs(false);
	    
	    RotatedRect box;
	    Point pt;
	    	    
	    for(int j=0; j<maxNumComponents; j++)
	      {
		FeatureBlob ftemp;
		blobTracker[i].filters_[j].getFiltered(ftemp);
		if(ftemp.getValid() && display_image)
		  {
		    Mat firstTemp, secondTemp;
		    ftemp.getValues(firstTemp, secondTemp);
		    pt.x = firstTemp.at<float>(0,0); pt.y = firstTemp.at<float>(1,0);
		    blobTracker[i].getBoxFromCov(pt, secondTemp, box);
		    
		    if (box.size.width > 0 && box.size.height > 0 && box.size.width < width && box.size.height < height)
		      {
			ellipse(*rgb_image, box, CV_RGB(255,255,255), 3, 8);
			circle(*rgb_image, pt, 3, CV_RGB(255, 255, 255), -1, 8);
		      }	    
		    
//.........这里部分代码省略.........

  bool turnOdom(double radians)
  {
    // If the distance to travel is negligble, don't even try.
    if (fabs(radians) < 0.01)
      return true;
  
    while(radians < -M_PI) radians += 2*M_PI;
    while(radians > M_PI) radians -= 2*M_PI;

    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

    try
    {
      //wait for the listener to get the first message
      listener_.waitForTransform(base_frame, odom_frame, 
                                 ros::Time::now(), ros::Duration(1.0));

      //record the starting transform from the odometry to the base frame
      listener_.lookupTransform(base_frame, odom_frame, 
                                ros::Time(0), start_transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      return false;
    }
    
    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;
    //the command will be to turn at 0.75 rad/s
    base_cmd.linear.x = base_cmd.linear.y = 0.0;
    base_cmd.angular.z = turn_rate;
    if (radians < 0)
      base_cmd.angular.z = -turn_rate;
    
    //the axis we want to be rotating by
    tf::Vector3 desired_turn_axis(0,0,1);
    
    ros::Rate rate(25.0);
    bool done = false;
    while (!done && nh_.ok() && as_.isActive())
    {
      //send the drive command
      cmd_vel_pub_.publish(base_cmd);
      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform(base_frame, odom_frame, 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;
      tf::Vector3 actual_turn_axis = 
        relative_transform.getRotation().getAxis();
      double angle_turned = relative_transform.getRotation().getAngle();
      
      // Update feedback and result.
      feedback_.turn_distance = angle_turned;
      result_.turn_distance = angle_turned;
      as_.publishFeedback(feedback_);
      
      if ( fabs(angle_turned) < 1.0e-2) continue;

      //if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) 
      //  angle_turned = 2 * M_PI - angle_turned;

      if (fabs(angle_turned) > fabs(radians)) done = true;
    }
    if (done) return true;
    return false;
  }

/*
void TrajActionServer::cmd_pose_right(Vectorq7x1 qvec) {
    //member var right_cmd_ already has joint names populated
    for (int i = 0; i < 7; i++) {
        right_cmd.command[i] = qvec[i];
    }
    joint_cmd_pub_right.publish(right_cmd);
}
*/
void TrajActionServer::executeCB(const actionlib::SimpleActionServer<davinci_traj_streamer::trajAction>::GoalConstPtr& goal) {
    double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
    int isegment;
    trajectory_msgs::JointTrajectoryPoint trajectory_point0;

    //Vectorq7x1 qvec, qvec0, qvec_prev, qvec_new;
    Eigen::VectorXd qvec, qvec0, qvec_prev, qvec_new;
    // TEST TEST TEST
    //Eigen::VectorXd q_vec;
    //q_vec<<0.1,0.2,0.15,0.4,0.5,0.6,0.7;    

    ROS_INFO("in executeCB");
    //ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes

    //....

    // for illustration, populate the "result" message with two numbers:
    // the "input" is the message count, copied from goal->input (as sent by the client)
    // the "goal_stamp" is the server's count of how many goals it has serviced so far
    // if there is only one client, and if it is never restarted, then these two numbers SHOULD be identical...
    // unless some communication got dropped, indicating an error
    // send the result message back with the status of "success"

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.return_val = g_count; // we'll use the member variable result_, defined in our class
    result_.traj_id = goal->traj_id;
    cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
    // copy trajectory to global var:
    new_trajectory = goal->trajectory; // 
    // insist that a traj have at least 2 pts
    int npts = new_trajectory.points.size();
    if (npts  < 2) {
        ROS_WARN("too few points; aborting goal");
        as_.setAborted(result_);
    } else { //OK...have a valid trajectory goal; execute it
        //got_new_goal = true;
        //got_new_trajectory = true;
        ROS_INFO("Cb received traj w/ npts = %d",npts);
        //cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
        //trajectory_msgs::JointTrajectoryPoint trajectory_point0;
        //trajectory_point0 = new_trajectory.points[0];  
        //trajectory_point0 =  tj_msg.points[0];   
        //cout<<new_trajectory.points[0].positions.size()<<" =  new_trajectory.points[0].positions.size()"<<endl;
        //cout<<"size of positions[]: "<<trajectory_point0.positions.size()<<endl;
        cout << "subgoals: " << endl;
        int njnts; 
        for (int i = 0; i < npts; i++) {
            njnts = new_trajectory.points[i].positions.size();
            cout<<"njnts: "<<njnts<<endl;
            for (int j = 0; j < njnts; j++) { //copy from traj point to 7x1 vector
                cout << new_trajectory.points[i].positions[j] << ", ";
            }
            cout<<endl;
            cout<<"time from start: "<<new_trajectory.points[i].time_from_start.toSec()<<endl;
            cout << endl;
        }

        as_.isActive();

        working_on_trajectory = true;
        //got_new_trajectory=false;
        traj_clock = 0.0; // initialize clock for trajectory;
        isegment = 0;
        trajectory_point0 = new_trajectory.points[0];
        njnts = new_trajectory.points[0].positions.size();
        int njnts_new;
        qvec_prev.resize(njnts);
        qvec_new.resize(njnts);
        ROS_INFO("populating qvec_prev: ");
        for (int i = 0; i < njnts; i++) { //copy from traj point to Eigen type vector
            qvec_prev[i] = trajectory_point0.positions[i];
        }
        //cmd_pose_right(qvec0); //populate and send out first command  
        //qvec_prev = qvec0;
        cout << "start pt: " << qvec_prev.transpose() << endl;
    }
    while (working_on_trajectory) {
        traj_clock += dt_traj;
        // update isegment and qvec according to traj_clock; 
        //if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false 
        ROS_INFO("traj_clock = %f; updating qvec_new",traj_clock);
        working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
        //cmd_pose_right(qvec_new); // use qvec to populate object and send it to robot
        ROS_INFO("publishing qvec_new as command");
        davinciJointPublisher.pubJointStatesAll(qvec_new);
;
        qvec_prev = qvec_new;


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

	bool isActionServerActive(){return as_.isActive();};

//this is where the bulk of the work is done, interpolating between potentially coarse joint-space poses
// using the specified arrival times
void trajActionServer::executeCB(const actionlib::SimpleActionServer<baxter_trajectory_streamer::trajAction>::GoalConstPtr& goal) {
    double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
    int isegment;
    trajectory_msgs::JointTrajectoryPoint trajectory_point0;

    Vectorq7x1 qvec, qvec0, qvec_prev, qvec_new;

    //ROS_INFO("in executeCB");
    //ROS_INFO("goal input is: %d", goal->input);

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.return_val = g_count; // we'll use the member variable result_, defined in our class
    //result_.traj_id = goal->traj_id;
    //cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
    // copy trajectory to global var:
    new_trajectory = goal->trajectory; // 
    // insist that a traj have at least 2 pts
    int npts = new_trajectory.points.size();
    if (npts  < 2) {
        ROS_WARN("too few points; aborting goal");
        as_.setAborted(result_);
    } else { //OK...have a valid trajectory goal; execute it
        //got_new_goal = true;
        //got_new_trajectory = true;
        ROS_INFO("Cb received traj w/ npts = %d",npts);
        //cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
        //debug output...
        /*
        cout << "subgoals: " << endl;
        for (int i = 0; i < npts; i++) {
            for (int j = 0; j < 7; j++) { //copy from traj point to 7x1 vector
                cout << new_trajectory.points[i].positions[j] << ", ";
            }
            cout << endl;
        }
        */
        as_.isActive();

        working_on_trajectory = true;

        traj_clock = 0.0; // initialize clock for trajectory;
        isegment = 0; //initialize the segment count
        trajectory_point0 = new_trajectory.points[0]; //start trajectory from first point...should be at least close to
          //current state of system; SHOULD CHECK THIS
        for (int i = 0; i < 7; i++) { //copy from traj point to 7x1 Eigen-type vector
            qvec0[i] = trajectory_point0.positions[i];
        }
        cmd_pose_left(qvec0); //populate and send out first command  
        qvec_prev = qvec0;
        cout << "start pt: " << qvec0.transpose() << endl;
    }
    while (working_on_trajectory) {
        traj_clock += dt_traj;
        // update isegment and qvec according to traj_clock; 
        //if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false          
        working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
        cmd_pose_left(qvec_new); // use qvec to populate object and send it to robot
        qvec_prev = qvec_new;
        //cout << "traj_clock: " << traj_clock << "; vec:" << qvec_new.transpose() << endl;
        ros::spinOnce();
        ros::Duration(dt_traj).sleep(); //update the outputs at time-step resolution specified by dt_traj
    }
    ROS_INFO("completed execution of a trajectory" );
    as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message 
}