C++ ServiceClient::call方法代码示例

int main(int argc, char **argv)
{
	ros::init(argc, argv, "rps_robot_commander");
	ros::NodeHandle n;

	//~ ros::Subscriber	rps_map_subscriber = n.subscribe("rps_map_data", 1, set_RPS_MAP);
	ros::ServiceServer server_robot_command = n.advertiseService("rps_robot_command", start_robot_commander);

	commander_to_get_robots_info = n.serviceClient<tms_msg_db::tmsdb_get_robots_info>("tmsdb_get_robots_info");
#ifdef SEND_COMMAND
	client_smartpal5_control = n.serviceClient<tms_msg_rc::smartpal_control>("sp5_control");
#endif
	client_smartpal5_speak = n.serviceClient<tms_msg_rc::smartpal_speak>("sp5_speak");
	
	////////////////////////////
#ifdef USE_TMS_DB
	srv_r.request.robots_id = 2;
	if(commander_to_get_robots_info.call(srv_r))
		ROS_INFO("Success robots_x = %lf, y = %lf, theta = %lf", srv_r.response.robots_x,srv_r.response.robots_y,srv_r.response.robots_theta);
	else{
		ROS_ERROR("Failed to call service get_robots_info\n");
		return 0;
	}
	srv_smartpal_control.request.unit = UNIT_VEHICLE;
	srv_smartpal_control.request.cmd  = CMD_setPose;
	srv_smartpal_control.request.arg.resize(3);
	srv_smartpal_control.request.arg[0] = srv_r.response.robots_x;
	srv_smartpal_control.request.arg[1] = srv_r.response.robots_y;
	srv_smartpal_control.request.arg[2] = srv_r.response.robots_theta;
#else
	srv_smartpal_control.request.unit = UNIT_VEHICLE;
	srv_smartpal_control.request.cmd  = CMD_setPose;
	srv_smartpal_control.request.arg.resize(3);
	srv_smartpal_control.request.arg[0] = 1000;
	srv_smartpal_control.request.arg[1] = 1000;
	srv_smartpal_control.request.arg[2] = 0.0;
#endif
	
#ifdef SEND_COMMAND
	if(client_smartpal5_control.call(srv_smartpal_control)){
		ROS_INFO("result: %d", srv_smartpal_control.response.result);
	}	
	else{
		ROS_ERROR("Failed to call service sp5_control");
		return 0;
	}
#endif
	////////////////////////////
	
	ros::spin();

	return 0;
}

void emergencyStopHandler(std_msgs::Bool msg)
{
	if (msg.data)
	{
		pending_bricks.clear();
		robot_client.call(cmdResetMotion,cmdRes);
		robot_state = emergency_stop;
	}
	else
	{
		robot_client.call(cmdOpenGripper,cmdRes);
		robot_state = idle;
	}
}

void joint_state_cb(const sensor_msgs::JointStateConstPtr& js){

    if (js->position.size() > 4){ //Message from the base or the arm
        js_cur = *js;
        
        last_status = sbs;
		if(!checkIfSafe())
			sbs.status = sbs.PAUSED;
		else
			sbs.status = sbs.RUNNING;
			
		sb_req.status = sbs;
		sb_req.requester = "mico_safety_node";
		
		if(sbs.status != last_status.status){
			if(stopbase_client.call(sb_req,sb_resp)){
				ROS_DEBUG("Made stop_base call");
			}
			else{
				ROS_ERROR("Stop base service call failed!");
				ros::shutdown();
			}	
		}
    }

};

/*
 * Check the busy flag
 *
 * Poll for the busy flag returns true if busy, false if not
 */
bool isADCBusy(ADC adc)
{
    //Define and fill in a service request
    hardware_interface::ReadI2CRegister read;
    read.request.addr = adc;
    read.request.reg = ADC_BUSY;
    read.request.size = 1;

    //Send request
    if(read_register_svr.call(read))
    {
        //Sussecful read
        if(read.response.data[0])
        {
            //Still busy
            return true;
        }
        else
        {
            //Not busy!
            return false;
        }
    }
    else
    {
        //Unsussesful read!!!
        //Do error handeling
    }

    return false;
}

void moveToJointState(const float* js){
	
	/*moveit::planning_interface::MoveGroup group("arm");
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;   
    group.setPlanningTime(5.0); //10 second maximum for collision computation*/
	
	
	
	moveit_utils::MicoMoveitJointPose::Request req;
	moveit_utils::MicoMoveitJointPose::Response res;
	
	for(int i = 0; i < 6; i++){
        switch(i) {
            case 0  :    req.target.joint1 = js[i]; break;
            case 1  :    req.target.joint2 =  js[i]; break;
            case 2  :    req.target.joint3 =  js[i]; break;
            case 3  :    req.target.joint4 =  js[i]; break;
            case 4  :    req.target.joint5 =  js[i]; break;
            case 5  :    req.target.joint6 =  js[i]; break;
        }
	//ROS_INFO("Requested angle: %f", q_vals.at(i));
    }
	
	if(client_joint_command.call(req, res)){
 		ROS_INFO("Call successful. Response:");
 		ROS_INFO_STREAM(res);
 	} else {
 		ROS_ERROR("Call failed. Terminating.");
 		//ros::shutdown();
 	}
}

  void jointsCall()
  {
    //ここをどうするか？
    for(int i = 0; i < okao_id.size(); ++i)
      {

	humans_msgs::HumanImgSrv hs;
	//hs.request.rule = rule;
       	
	hs.request.src.max_okao_id = okao_id[i];
	
	//okao_client::OkaoStack okao_img;
	//okao_img.request.person.okao_id = okao_id;
	
	if( srv.call( hs ) )
	  {	
	    //cout << "okao_id:" << okao_id[i] << ", x,y =" 
	    //	 << hs.response.dst.p.x << ","<< hs.response.dst.p.y <<  endl ;
	    markerPublisher( hs.response.dst, hs.response.img );
	  }
	else
	  {
	    cout << "not found!"<<endl;
	  }
      }


  }

/* processData is the main decision function that
 * will publish commands to the motor controller
 * node and possibly ask for arduino data.
 */
void processData() {

   //geometry_msgs::Twist command;
   beagle_pkg::status status;
   if(ard_client.call(ard_data)) {

      ROS_INFO("lidar data was %f, arduino reading was %f\n", scan_data.range_min, ard_data.response.distance);

      if(ard_data.response.distance > 5 && scan_data.range_min > 5) {
         status.command = 0;
      }
      else if(ard_data.response.distance <= 0 && scan_data.range_min <= 0) {
         status.command = 0;
      }
      else if(ard_data.response.distance > scan_data.range_min) {
         status.command = 1;
      }
      else {
         status.command = 2;
      }
      
      /* TODO: look at global data (scan_data, gps_data, ard_data)
       * and make decision of what to send to the motor node.
       */
      vel_pub.publish(status);
   }
   else {
    ROS_ERROR("Failed to call ard service");
   }
}

//=============================================================
//名前：TurnScan
//説明：首振りの方向折り返し
//=============================================================
void ThermalScanning::TurnScan(void)
{
	//std_msgs::Float64 tilt_controller_command_msg; 

	if(cw == -1)	// 左回りの場合
	{
		start_angle = MIN_YAW_ANGLE_CAM;
		end_angle   = MAX_YAW_ANGLE_CAM;
		pm = 1;
	}else if(cw == 1){	// 右回りの場合
		start_angle = MAX_YAW_ANGLE_CAM;
		end_angle   = MIN_YAW_ANGLE_CAM;
		pm = -1;
	}

	// ダイナミクセルのスピードの設定
	if(Scan_state == 0 || Scan_state == 5 || Scan_state == 6)	// 通常探索
	{ 
		srv.request.speed = MOTOR_SPEED;
	}
	else if(Scan_state == 1)	// 強化探索
	{
		srv.request.speed = MOTOR_SPEED2;
	}	
	dyna_client.call(srv);
		
	// ダイナミクセルの角度を初期位置に移動する
	// TODO ダイナミクセルの角度をDyna_angleに変更する処理を入れる
	Dyna_angle = start_angle;

	tilt_controller_command_msg.data = Dyna_angle;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "run_pose_estimation_for_scene_screenshot");

    ros::NodeHandle nodeHandle = ros::NodeHandle("~");
    global = nodeHandle.serviceClient<MsgT>("/inSceneDetector/detect");

    if (!once){


        //get screen shot pose
        ROS_INFO("Subscribing to detection service for screen shot pose...");
        ros::service::waitForService("/inSceneDetector/detect");
        MsgT msgglobal01;
        msgglobal01.request.scenario = "detect_screen_shot";

            if(global.call(msgglobal01)) {
			//printResults(msgglobal01);
                    pcl::console::print_value("Screenshot detected!\n");
            } else pcl::console::print_error("Screenshot not detected!\n");

	once = true;
    }
    ros::spinOnce();

return 0;
}

std::vector<Brick> getBricks()
{
    std::vector<Brick> retVec;
    rc_vision::getBricks obj;

    if(!_serviceGetBricks.call(obj))
    {
        printConsole("Failed to call the 'serviceGetBricks'");
    }
    else
    {
        for(unsigned int i=0; i<obj.response.size.size(); i++)
        {
            Brick brick;
            brick.color = obj.response.color[i];
            brick.posX = obj.response.posX[i];
            brick.posY = obj.response.posY[i];
            brick.size = obj.response.size[i];
            brick.theta = obj.response.theta[i];
            retVec.push_back(brick);
        }
    }

    return retVec;
}

void anyBrickCallback(std_msgs::Bool msg)
{
    // Fetch robot position
    kuka_rsi::getConfiguration getQObj;

    // Call service
    if(!_serviceGetConf.call(getQObj))
       printConsole("Failed to call the 'serviceKukaGetConfiguration'");

    // Get information
    bool same = true;
    for(int i=0; i<6; i++)
    {
        if(fabs(getQObj.response.q[i]*DEGREETORAD - _qIdle[i]) > 0.1)
        {
            same = false;
            break;
        }
    }

    _anyBricksMutex.lock();
    _anyBricks = msg.data;
    _anyBricksMutex.unlock();

    _qMutex.lock();
    _positionQIdle = same;
    _qMutex.unlock();
}

void changeDirConveyerBelt(bool dir) // true = reverse
{
    rc_plc::ChangeDirection obj;
    obj.request.direction = dir;
    if(!_serviceChangeDir.call(obj))
        printConsole("Failed to call the 'serviceChangeDirConveyer'");
}

void startConveyerBelt(bool dir = false)
{
    rc_plc::StartConv obj;
    obj.request.direction = dir;
    if(!_serviceStart.call(obj))
        printConsole("Failed to call the 'serviceStartConveyer'");
}

/**
 * @brief Callback functions for datalink routing, channel 1 
 *        
 */
void sub1_callback(const uav_control::DFrame msg)
{
  // message from GCS, to be relayed to quadcopter
  if(msg.target_id != SYSID){
    uav_control::datalink_send ss;
    ss.request.source_id = msg.source_id;
    ss.request.target_id = msg.target_id;
    ss.request.route = msg.route;
    ss.request.len = msg.len;
    int i;
    for(i=0; i<msg.len; i++)
      ss.request.payload[i] = msg.payload[i];

    if(client_sub1.call(ss)){
      switch(ss.response.err){
        case 0: //ROS_INFO("successful");
                break;
        case 1: ROS_ERROR("relay error: invalid channel");
                break;
        case 2: ROS_ERROR("relay error: channel not initialized");
                break;
        case 3: ROS_ERROR("relay error: sending failed");
                break;
        default: ROS_ERROR("relay error: unknown failure");
                break;
      }
    }
  }

}

bool plotSummary(ros::ServiceClient& sum_client)
{
    PlanningSummary planning_sum;
    if (!sum_client.call(planning_sum))
    {
        return false;
    }

    unsigned int cols = planning_sum.response.score_labels.size();
    for (unsigned int i = 0; i < planning_sum.response.score_labels.size(); i++)
    {
        cout << planning_sum.response.score_labels[i] << "\t";
    }
    cout << endl;

    for (unsigned int i = 0; i < planning_sum.response.score_values.size(); i++)
    {
        if (i % cols == 0)
        {
            cout << i / cols << ": ";
        }
        cout << planning_sum.response.score_values[i];
        cout << "\t";
        if (i % cols == cols - 1)
        {
            cout << endl;
        }
    }

    return true;
}