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

int main(int argc, char **argv){
	ros::init(argc, argv, "mobots_teleop_bridge");
	ros::NodeHandle n;
	client = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state"); //true für persistente Verbindung <> GEHT NICHT IN DIESEM FALL >_<
	client.waitForExistence();
	getClient = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state", true);
	getClient.waitForExistence();
	ros::Subscriber sub = n.subscribe("/cmd_vel", 100, teleopCallback);  //wichtig: Wenn das zurückgegebene Objekt nicht mehr referenziert wird, wird der tatsächliche Subscriber zerstört
	ros::spin();
}

    PickPlaceDemo() : 
        arm("arm"), 
        gripper("gripper")
    {
        planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
        planning_scene_diff_client.waitForExistence();
        get_planning_scene_client = node_handle.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
        get_planning_scene_client.waitForExistence();

    }

        bool wait_for_services() {
            state_machine_client.waitForExistence();
            ROS_INFO("State machine service is ready, waiting for init");
            hdpc_com::ChangeStateMachine change_state;
            ros::Rate rate(1);
            do {
                rate.sleep();
                change_state.request.event = EVENT_READ_STATE;
                if (!state_machine_client.call(change_state)) {
                    ROS_WARN("Change state machine: request state failed");
                    return false;
                }
            } while (ros::ok() && (change_state.response.state != SM_STOP));
            ROS_INFO("HDPC Drive: ROVER is ready");

            change_state.request.event = EVENT_START;
            if (!state_machine_client.call(change_state)) {
                ROS_WARN("Change state machine: starting failed");
                return false;
            }

            watchdog = 0;
            control_mode = HDPCModes::MODE_INIT;
            commands.header.stamp = ros::Time::now();
            for (unsigned int i=0;i<10;i++) {
                commands.isActive[i] = false;
                commands.velocity[i] = 0;
                commands.position[i] = 0.0;
            }
            
            return true;
        }

int
main (int argc, char** argv)
{
  ros::init (argc, argv, "al_viz_objects_features");
  nh_ = new ros::NodeHandle ("~");

  sub_entities_ = nh_->subscribe ("/perception/entities", 1, &entitiesCallback);
  srv_cl_get_entities_visual_features_
      = nh_->serviceClient<al_srvs::GetEntitiesVisualFeatures> ("/get_entities_visual_features", true);
  srv_cl_get_entities_visual_features_.waitForExistence ();
  srv_cl_get_entities_visual_features_
      = nh_->serviceClient<al_srvs::GetEntitiesVisualFeatures> ("/get_entities_visual_features", true);

  ep_ = engOpen ("matlab -nodesktop -nosplash -nojvm");
  if (ep_ == NULL)
    ROS_ERROR("Constructor: engOpen failed");
  else
    ROS_DEBUG(" Matlab engine is working!");

  //in any case surface normals should be calculated
  pcl::PointCloud<pcl::Normal>::Ptr pointcloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
  ne_.setSearchMethod (tree);
  ne_.setViewPoint (0, 0, 1.2);//this could be done more informed by getting this information from a topic

  ros::Rate r (30);//objects can be refreshed at most @30hz
  while (nh_->ok ())
  {
    if (msg_entities_rcvd_)
    {
      msg_entities_rcvd_ = false;
      vizFeatures ();
    }
    ros::spinOnce ();
    r.sleep ();
  }
  engClose (ep_);

  return 0;
}

  // Close end effector
  bool closeEndEffector()
  {
    // Error check - servos are alive and we've been recieving messages
    if( !endEffectorResponding() )
    {
      return false;
    }

    // Check if end effector is already close and arm is stil
    if( ee_status_.target_position == END_EFFECTOR_CLOSE_VALUE_MAX &&
        ee_status_.moving == false &&
        ee_status_.position > END_EFFECTOR_CLOSE_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
        ee_status_.position < END_EFFECTOR_CLOSE_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE )
    {
      // Consider the ee to already be in the correct position
      ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector already closed within tolerance, unable to close further");
      return true;
    }

    // Set the velocity for the end effector to a low value
    ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity low");
    velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
    if( !velocity_client_.waitForExistence(ros::Duration(5.0)) )
    {
      ROS_ERROR_NAMED("clam_gripper_controller","Timed out waiting for velocity client existance");
      return false;
    }

    dynamixel_hardware_interface::SetVelocity set_velocity_srv;
    set_velocity_srv.request.velocity = END_EFFECTOR_MEDIUM_VELOCITY;
    if( !velocity_client_.call(set_velocity_srv) )
    {
      ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
      return false;
    }

    std_msgs::Float64 joint_value;
    double timeout_sec = 10.0; // time before we declare an error
    const double CHECK_INTERVAL = 0.1; // how often we check the load
    const double FIRST_BACKOUT_AMOUNT = -0.25;
    const double SECOND_BACKOUT_AMOUNT = -0.0075;
    const double BACKOUT_AMOUNT[2] = {FIRST_BACKOUT_AMOUNT, SECOND_BACKOUT_AMOUNT};

    // Grasp twice - to reduce amount of slips and ensure better grip
    for(int i = 0; i < 2; ++i)
    {
      timeout_sec = 10; // reset timeout;
      ROS_DEBUG_STREAM("Grasping with end effector - grasp number " << i + 1);

      // Tell servos to start closing slowly to max amount
      joint_value.data = END_EFFECTOR_CLOSE_VALUE_MAX;
      end_effector_pub_.publish(joint_value);

      // Wait until end effector is done moving
      while( ee_status_.position < joint_value.data - END_EFFECTOR_POSITION_TOLERANCE ||
                                   ee_status_.position > joint_value.data + END_EFFECTOR_POSITION_TOLERANCE )
      {
        ros::spinOnce(); // Allows ros to get the latest servo message - we need the load

        // Check if load has peaked
        if( ee_status_.load < END_EFFECTOR_LOAD_SETPOINT ) // we have touched object!
        {
          // Open the gripper back up a little to reduce the amount of load.
          // the first time open it a lot to help with grasp quality
          joint_value.data = ee_status_.position + BACKOUT_AMOUNT[i];

          // Check that we haven't passed the open limit
          if( joint_value.data < END_EFFECTOR_OPEN_VALUE_MAX )
            joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;

          ROS_DEBUG_NAMED("clam_gripper_controller","Setting end effector setpoint to %f when it was %f", joint_value.data, ee_status_.position);
          end_effector_pub_.publish(joint_value);

          if( i == 0 ) // give lots of time to pull out the first time
          {
            ros::Duration(1.00).sleep();
            ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Sleeping as we publish joint value " << joint_value.data);

            set_velocity_srv.request.velocity = END_EFFECTOR_SLOW_VELOCITY;
            if( !velocity_client_.call(set_velocity_srv) )
            {
              ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
              return false;
            }
            ros::Duration(1.0).sleep();
          }
          break;
        }

        // Debug output
        ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","" << joint_value.data - END_EFFECTOR_POSITION_TOLERANCE << " < " <<
                               ee_status_.position << " < " << joint_value.data + END_EFFECTOR_POSITION_TOLERANCE
                               << " -- LOAD: " << ee_status_.load );

        // Feedback
        feedback_.position = ee_status_.position;
        //TODO: fill in more of the feedback
        action_server_->publishFeedback(feedback_);

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

  // Open end effector
  bool openEndEffector()
  {
    // Error check - servos are alive and we've been recieving messages
    if( !endEffectorResponding() )
    {
      return false;
    }

    // Check if end effector is already open and arm is still
    if( ee_status_.target_position == END_EFFECTOR_OPEN_VALUE_MAX &&
        ee_status_.moving == false &&
        ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
        ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE )
    {
      // Consider the ee to already be in the corret position
      ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector open: already in position");
      return true;
    }

    // Set the velocity for the end effector servo
    ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity");
    velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
    while(!velocity_client_.waitForExistence(ros::Duration(10.0)))
    {
      ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
      return false;
    }
    dynamixel_hardware_interface::SetVelocity set_velocity_srv;
    set_velocity_srv.request.velocity = END_EFFECTOR_VELOCITY;
    if( !velocity_client_.call(set_velocity_srv) )
    {
      ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
      return false;
    }

    // Publish command to servos
    std_msgs::Float64 joint_value;
    joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
    end_effector_pub_.publish(joint_value);

    // Wait until end effector is done moving
    int timeout = 0;
    while( ee_status_.moving == true &&
           ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
           ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
           ros::ok() )
    {
      // Feedback
      feedback_.position = ee_status_.position;
      //TODO: fill in more of the feedback
      action_server_->publishFeedback(feedback_);

      // Looping
      ros::Duration(0.25).sleep();
      ++timeout;
      if( timeout > 16 )  // wait 4 seconds
      {
        ROS_ERROR_NAMED("clam_gripper_controller","Unable to open end effector: timeout on goal position");
        return false;
      }
    }

    // It worked!
    ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished end effector action");
    return true;
  }

int
main (int argc, char** argv)
{
  ros::init (argc, argv, "al_object_affordances");
  nh_ = new ros::NodeHandle ();
  //  ros::ServiceClient srv_cl_get_entity_;
  //  ros::ServiceClient srv_cl_get_entity_features_;

  pub_affordances_ = nh_->advertise<al_msgs::Affordances> ("/affordances", 1);

  srv_cl_affordances_ = nh_->serviceClient<al_srvs::GetLearnedEffects> ("/get_learned_effects", true);
  while (nh_->ok () && !srv_cl_affordances_.waitForExistence (ros::Duration (0.1)))
  {
    ros::spinOnce ();
  }

  //  srv_cl_get_entity_ = nh_->serviceClient<al_srvs::GetEntity> ("/memory/get_entity", true);
  //  while (nh_->ok () && !srv_cl_get_entity_.exists ())
  //  {
  //    srv_cl_get_entity_.waitForExistence (ros::Duration (0.1));
  //    ros::spinOnce ();
  //  }
  //
  //  srv_cl_get_entity_features_ = nh_->serviceClient<al_srvs::GetEntityVisualFeatures> ("/get_entity_visual_features",
  //                                                                                      true);
  //  while (nh_->ok () && !srv_cl_get_entity_features_.exists ())
  //  {
  //    srv_cl_get_entity_features_.waitForExistence (ros::Duration (0.1));
  //    ros::spinOnce ();
  //  }

  al::system::changeInputMode (1);
  getAllScene (entities_);

  //  ros::Rate r (5);
  //  std::cout << "Enter an object id to track its affordances" << std::endl;
  //  int id = -1;
  //  while (nh_->ok ())
  //  {
  //    char c_id;
  //    if (al::system::getKey (c_id))
  //    {
  //      std::cin >> c_id;
  //      id = atoi (&c_id);
  //      std::cout << "Enter an object id to track its affordances" << std::endl;
  //    }
  //
  //    if (id != -1)
  //    {
  //      //get object
  //      al_srvs::GetEntity::Request req_entity;
  //      al_srvs::GetEntity::Response res_entity;
  //      req_entity.id = al::facilities::toString (id);
  //      req_entity.latest_scene_required = true;
  //      if (srv_cl_get_entity_.call (req_entity, res_entity))
  //      {
  //        //get object features
  //        al_srvs::GetEntityVisualFeatures::Request req_features;
  //        al_srvs::GetEntityVisualFeatures::Response res_features;
  //
  //        req_features.entity = res_entity.entity;
  //        if (srv_cl_get_entity_features_.call (req_features, res_features))
  //        {
  //          al_srvs::GetLearnedEffects::Request req_learned_effects;
  //          al_srvs::GetLearnedEffects::Response res_learned_effects;
  //
  //          req_learned_effects.feature_vector = res_features.feature_vector;
  //          if (srv_cl_affordances_.call (req_learned_effects, res_learned_effects))
  //          {
  //            for (uint i = 0; i < res_learned_effects.effects.size (); i++)
  //            {
  //              std::cout << al::facilities::behaviorEnumToString (res_learned_effects.effects[i].arg) << "\t";
  //              std::cout << al::facilities::effectEnumToString (res_learned_effects.effects[i].effect) << "\t";
  //              std::cout << res_learned_effects.effects[i].prob << std::endl;
  //              //              std::cout << res_learned_effects.effects[i] << std::endl;
  //              map_effects[res_learned_effects.effects[i].prob] = res_learned_effects.effects[i];
  //            }
  //
  //            //now pick top 5 different affordances
  //            //convert into an affordance message and publish
  //            it = map_effects.end ();
  //            //            affordances_.effects.resize (TOP_N_EFFECTS);
  //            while (it != map_effects.begin () && affordances_.effects.size () < TOP_N_EFFECTS)
  //            {
  //              --it;
  //              bool same_effect_exists = false;
  //              for (uint i = 0; i < affordances_.effects.size (); i++)
  //              {
  //                if (it->second.effect == affordances_.effects[i].effect)
  //                {
  //                  same_effect_exists = true;
  //                  if (it->first > affordances_.effects[i].prob)
  //                  {
  //                    affordances_.effects[i].effect = it->second.effect;
  //                    break;
  //                  }
  //                }
  //              }
  //              if (!same_effect_exists)
  //              {
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pendulum_controller");
    //ros::init(argc, argv, "pendulum");
    ros::NodeHandle n("pendulum");
    ros::NodeHandle n_state("pendulum");

    command_pub = n.advertise<pendulum::command>( "command", 1000 );

    state_client = n_state.serviceClient<pendulum::state>( "state" );
    //state_client = n.serviceClient<pendulum::state>( "state" );
    //state_client = n.serviceClient<pendulum::state>( "state", true );
    if( !state_client.isValid( ) ) {
	ROS_INFO( "state_client not valid" );
        state_client.waitForExistence( );
    } 

    ros::Rate loop_rate( 25 );  
    //ros::Rate loop_rate( 10 );  

    ROS_INFO("Ready to standup pendulum.");

    command.time = 0.0;
    command.torque = 0.0;
    state.request.timestamp = 0.0;

    pendulum::state s;

    if( !state_client.isValid( ) ) {
	ROS_INFO( "state_client no longer valid" );
        state_client.waitForExistence( );
    } 

    int count = 0;

    ros::Time calibrate_time, start_time;

    while( ros::ok( ) ) {
    	calibrate_time = ros::Time::now( );

	if( calibrate_time.sec != 0 || calibrate_time.nsec != 0 )
	    break;
	
        ros::spinOnce( );		
    }

    start_time = ros::Time::now( );
    //ROS_INFO( "start_time: sec[%u] nsec[%u]", start_time.sec, start_time.nsec );
    ROS_INFO( "time, position, velocity, torque" );

    while( ros::ok( ) ) {

	//if( count == 1 ) break;

        if( state_client && state_client.exists( ) && state_client.isValid( ) ) {
            //ROS_INFO("Calling service");
            if( state_client.call( s ) ) {
                //ROS_INFO("Successful call");

    		ros::Time sim_time = ros::Time::now( );

    		//ROS_INFO( "sim_time: sec[%u] nsec[%u]", sim_time.sec, sim_time.nsec );

		//ros::Duration current_time = sim_time - start_time;

		//Real time = (Real) count / 1000.0;
		Real time = (sim_time - start_time).toSec();

	        command.torque = control( time, s.response.position, s.response.velocity );
  	        command.time = time;

		ROS_INFO( "%10.9f, %10.9f, %10.9f, %10.9f", time, s.response.position, s.response.velocity, command.torque );

                command_pub.publish( command );

	        //command.torque = control( state.response.time, state.response.position, state.response.velocity );
  	        //command.time = state.response.time;
            } else {
               ROS_INFO("Failed to call");
	    }
        } else {
            ROS_INFO("Persistent client is invalid");
        }
/*
        if( !state_client.isValid( ) ) {
	    ROS_INFO( "state_client not valid" );
	}

        if( !state_client.exists( ) )
            state_client.waitForExistence( );
	    
	if( state_client.call( s ) ) {
	//if( state_client.call( state ) ) {
	    ROS_INFO( "called state" );

	    command.torque = control( s.response.time, s.response.position, s.response.velocity );
  	    command.time = s.response.time;

	    //command.torque = control( state.response.time, state.response.position, state.response.velocity );
  	    //command.time = state.response.time;
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
	pending_bricks = std::vector<bpMsgs::robot_pick>();

	/* ros messages */

	/* parameters */
	std::string brick_subscriber_topic;
	std::string brick_publisher_topic;
	std::string emergency_stop_subscriber_topic;
	int loop_rate_param;

	/* initialize ros usage */
	ros::init(argc, argv, "robot_motion_controller");
	ros::Publisher brick_publisher;
	ros::Subscriber brick_subscriber;
	ros::Subscriber emergency_stop_subscriber;
	bpPLCController::plc_command plcCommand;
	plcCommand.request.command_number = bpPLCController::plc_command::Request::GET_GRIPPER_SENSOR_STATUS;

	/* private nodehandlers */
	ros::NodeHandle nh;
	ros::NodeHandle n("~");

	/* read parameters from ros parameter server if available otherwise use default values */
	n.param<std::string> ("brick_subscriber_topic", brick_subscriber_topic, "/bpRobotMotionController/brick_command_topic");
	n.param<std::string> ("brick_publisher_topic", brick_publisher_topic, "/bpRobotMotionController/brick_response_topic");
	n.param<std::string> ("emergency_stop_subscriber_topic", emergency_stop_subscriber_topic, "/emergency_stop");
	n.param<int> ("loop_rate", loop_rate_param, 1);


	brick_publisher = nh.advertise<bpMsgs::robot_pick> (brick_publisher_topic.c_str(), 10);
	brick_subscriber = nh.subscribe<bpMsgs::robot_pick> (brick_subscriber_topic.c_str(), 20, brickHandler);
	emergency_stop_subscriber = nh.subscribe<std_msgs::Bool> (emergency_stop_subscriber_topic.c_str(),20,emergencyStopHandler);

	setKnownConfs();

	robot_client = n.serviceClient<bpDrivers::command>("/bpDrivers/rx60_controller/rx60_command");
	ros::ServiceClient plc_client = n.serviceClient<bpPLCController::plc_command>("/plc_controller/plc_command");

	std::cout << "RobotMotionPlanner node started! - waiting for RX60 controller service" << std::endl;
	robot_client.waitForExistence();
	std::cout << "RX60 controller service found! - Going to idle position" << std::endl;

	robot_state = idle;

	// Go to Idle position
	robot_client.call(cmdIdle,cmdRes);
	robot_client.call(cmdOpenGripper, cmdRes);

	bpMsgs::robot_pick brickToPick;
	bpMsgs::robot_pick returnMessage;

	ros::Rate loop_rate(loop_rate_param);
	while (ros::ok())
	{
		ros::spinOnce();

		switch (robot_state) {
			case idle:
				if (pending_bricks.size() > 0)
				{
					brickToPick = pending_bricks[0];
					pending_bricks.erase(pending_bricks.begin());
					if (!calcPositions(brickToPick))
					{
						// Cant reach brick
						robot_state = failed;
					}
					else
					{
						robot_client.call(cmdWaitPosition,cmdRes);
						ros::Duration(0.05).sleep();
						robot_state = wait_for_wait_position;
					}
				}
				else
				{
					robot_client.call(cmdIdle,cmdRes);
				}
				break;
			case wait_for_wait_position:
				cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
				robot_client.call(cmdReq,cmdRes);
				if (cmdRes.is_settled)
				{
					robot_state = go_down_to_pick_brick;
				}
				break;

			case go_down_to_pick_brick:
				if (brickToPick.header.stamp.toSec() < (ros::Time::now().toSec()) )
				{
					robot_state = failed;
				}
				else
				{
					ros::Duration( (brickToPick.header.stamp.toSec() - ros::Time::now().toSec()) - 0.20).sleep();
					robot_client.call(cmdGripPosition,cmdRes);
					robot_state = wait_for_brick;//wait_for_grip_position;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
          ROS_WARN("[iob] couldn't find a I_CLAMP param for %s", joint_ns.c_str());
        }
        if (!rosnode->getParam(vp_str, vp_val)) {
          ROS_WARN("[iob] couldn't find a VP param for %s", joint_ns.c_str());
        }
        // store these directly on altasState, more efficient for pub later
        initial_jointcommand.kp_position[i] = p_val;
        initial_jointcommand.ki_position[i] = i_val;
        initial_jointcommand.kd_position[i] = d_val;
        initial_jointcommand.i_effort_min[i] = -i_clamp_val;
        initial_jointcommand.i_effort_max[i] = i_clamp_val;
        initial_jointcommand.velocity[i]     = 0;
        //initial_jointcommand.effort[i]       = 0;
        initial_jointcommand.kp_velocity[i]  = vp_val;
      } else {
        // velocity feedback
        double p_v_val = 0, vp_v_val = 0;
        std::string p_v_str  = std::string(joint_ns) + "p_v";
        std::string vp_v_str = std::string(joint_ns) + "vp_v";
        if (!rosnode->getParam(p_v_str, p_v_val)) {
          ROS_WARN("[iob] couldn't find a P_V param for %s", joint_ns.c_str());
        }
        if (!rosnode->getParam(vp_v_str, vp_v_val)) {
          ROS_WARN("[iob] couldn't find a VP_V param for %s", joint_ns.c_str());
        }
        initial_jointcommand.kpv_position[i] = p_v_val;
        initial_jointcommand.kpv_velocity[i] = vp_v_val;
      }
    }

    initial_jointcommand.desired_controller_period_ms =
      static_cast<unsigned int>(g_period_ns * 1e-6);

    if (iob_synchronized) {
      serv_command =
        ros::service::createClient<hrpsys_gazebo_msgs::SyncCommand> (robotname + "/iob_command", true); // persistent = true,
      ROS_INFO_STREAM("[iob] waiting service " << robotname << "/iob_command");
      serv_command.waitForExistence();
      ROS_INFO_STREAM("[iob] found service " <<  robotname << "/iob_command");
    } else {
      pub_joint_command = rosnode->advertise <JointCommand> (robotname + "/joint_command", 1, true);

      // ros topic subscribtions
      ros::SubscribeOptions jointStatesSo =
        ros::SubscribeOptions::create<RobotState>(robotname + "/robot_state", 1, setJointStates,
                                                  ros::VoidPtr(), rosnode->getCallbackQueue());
      // Because TCP causes bursty communication with high jitter,
      // declare a preference on UDP connections for receiving
      // joint states, which we want to get at a high rate.
      // Note that we'll still accept TCP connections for this topic
      // (e.g., from rospy nodes, which don't support UDP);
      // we just prefer UDP.
      // temporary use TCP / Using UDP occured some problem when message size more than 1500.
      //jointStatesSo.transport_hints =
      //ros::TransportHints().maxDatagramSize(3000).unreliable().reliable().tcpNoDelay(true);
      jointStatesSo.transport_hints =
        ros::TransportHints().reliable().tcpNoDelay(true);
      sub_robot_state = rosnode->subscribe(jointStatesSo);
    }

    for (int i=0; i < number_of_joints(); i++){
        command[i] = 0.0;
        power[i] = OFF;
        servo[i] = OFF;
    }
    clock_gettime(CLOCK_MONOTONIC, &g_ts);
    rg_ts = ros::Time::now();

    jointcommand = initial_jointcommand;
    std::cerr << "[iob]  " << number_of_joints() << " / " << initial_jointcommand.position.size() << " / " << NUM_OF_REAL_JOINT << std::endl;

    if (iob_synchronized) {
      hrpsys_gazebo_msgs::SyncCommandRequest req;
      req.joint_command = jointcommand;
      hrpsys_gazebo_msgs::SyncCommandResponse res;
      std::cerr << "[iob] first service call" << std::endl;
      serv_command.call(req, res);
      std::cerr << "[iob] first service returned" << std::endl;
      js = res.robot_state;
      init_sub_flag = true;
    } else {
      std::cerr << "[iob] block until subscribing first robot_state";
      ros::Time start_tm = ros::Time::now();
      ros::Rate rr(100);
      ros::spinOnce();
      while (!init_sub_flag) {
        if ((ros::Time::now() - start_tm).toSec() > 5.0) {
          std::cerr << "[iob] timeout for waiting robot_state";
          break;
        }
        std::cerr << ".";
        rr.sleep();
        ros::spinOnce();
      }
    }

    std::cerr << std::endl << "[iob] Open IOB / finish " << std::endl;

    return TRUE;
}