C++ TrajClient::waitForServer方法代码示例

  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_r_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
    traj_client_l_ = new TrajClient("l_arm_controller/joint_trajectory_action", true);

    // wait for action server to come up
    while(!traj_client_l_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
    // wait for action server to come up
    while(!traj_client_r_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  IKTrajectoryExecutor()
  {

    //tell the joint trajectory action client that we want 
    //to spin a thread by default
    action_client = new TrajClient("joint_trajectory_action", true);

    //wait for the action server to come up
    while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
    {
      ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
    }

    //register a service to input desired Cartesian trajectories
    service = node.advertiseService("execute_cartesian_ik_trajectory", 
        &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    //have to specify the order of the joints we're sending in our 
    //joint trajectory goal, even if they're already on the param server
    goal.trajectory.joint_names.push_back("shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("elbow_joint");
    goal.trajectory.joint_names.push_back("wrist_1_joint");
    goal.trajectory.joint_names.push_back("wrist_2_joint");
    goal.trajectory.joint_names.push_back("wrist_3_joint");

  }

  IKTrajectoryExecutor(){

    //create a client function for the IK service
    ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
      (ARM_IK_NAME, true);    

    //wait for the various services to be ready
    ROS_INFO("Waiting for services to be ready");
    ros::service::waitForService(ARM_IK_NAME);
    ROS_INFO("Services ready");

    //tell the joint trajectory action client that we want 
    //to spin a thread by default
    action_client = new TrajClient("l_arm_controller/joint_trajectory_action", true);

    //wait for the action server to come up
    while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
    }

    //register a service to input desired Cartesian trajectories
    service = node.advertiseService("left_execute_cartesian_ik_trajectory", 
        &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    //have to specify the order of the joints we're sending in our 
    //joint trajectory goal, even if they're already on the param server
    goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("l_wrist_roll_joint");

  }

	// Initialize the action client and wait for the action server to come up
	RobotArm()
	{
		// tell the action client that we want to spin a thread by default
		traj_client_elbow_flex_ = new TrajClient("r_elbow_flex_controller/follow_joint_trajectory", true);
		traj_client_torso_ = new TrajClient("/torso_lift_controller/follow_joint_trajectory", true);
		
		// wait for action server to come up
		while (!traj_client_elbow_flex_->waitForServer(ros::Duration(5.0)))
		{
			ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
		}

		while (!traj_client_torso_->waitForServer(ros::Duration(5.0)))
		{
			ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
		}
		ROS_INFO("Works for now");
	}	

   //! Initialize the action client and wait for action server to come up
	MoveInJoints() 
	{
		// tell the action client that we want to spin a thread by default
		traj_client_ = new TrajClient("/move_iri_wam", true);
		
		// wait for action server to come up
		while(!traj_client_->waitForServer(ros::Duration(5.0))){
		ROS_INFO("Waiting for the move_iri_wam server");
		}
	}

        //! Initialize the action client and wait for action server to come up
        RobotArm():srv_name_("/iri_wam/iri_wam_controller/joints_move") {
            client = n.serviceClient<iri_common_drivers_msgs::QueryJointsMovement>(this->srv_name_.c_str());
            // tell the action client that we want to spin a thread by default
            traj_client_ = new TrajClient("/iri_wam/iri_wam_controller/follow_joint_trajectory", true);

            // wait for action server to come up
            while(!traj_client_->waitForServer(ros::Duration(5.0))){
                ROS_INFO("Waiting for the joint_trajectory_action server");
            }
        }

  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("/wam_wrapper/DMPTracker", true);

    //initialize the topic to send new goals
    
    pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/wam_wrapper/DMPTrackerNewGoal", 1);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  void init (ros::NodeHandle* nh) {

    ik_client = nh->serviceClient<moveit_msgs::GetPositionIK>
      (ARM_IK_NAME, true);    

    ROS_INFO("Waiting for services to be ready");
    ros::service::waitForService(ARM_IK_NAME);
    ROS_INFO("Services ready");

//    action_client = new TrajClient("joint_trajectory_action", true);
    action_client = new TrajClient("follow_joint_trajectory", true);

    while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
    {
      ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
    }

 //   service = node.advertiseService("execute_cartesian_ik_trajectory", 
 //       &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    const std::string input_topic = "execute_cartesian_ik_trajectory";

    joint_states_sub = nh->subscribe( "joint_states", 1, 
        &IKTrajectoryExecutor::set_current_joint_angles, this);

    ros::Duration(1.0).sleep();    

    trajectory_sub = nh->subscribe( input_topic, 1, 
        &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);

    ros::spinOnce();

    goal.trajectory.joint_names.push_back("shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("elbow_joint");
    goal.trajectory.joint_names.push_back("wrist_1_joint");
    goal.trajectory.joint_names.push_back("wrist_2_joint");
    goal.trajectory.joint_names.push_back("wrist_3_joint");

  }