int open_iob(void)
{
    static bool isInitialized = false;
    if ( isInitialized ) return TRUE;
    isInitialized = true;

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

    std::string node_name;
    {
      char *ret = getenv("HRPSYS_GAZEBO_IOB_NAME");
      if (ret != NULL) {
        node_name.assign(ret);
      } else {
        node_name = "hrpsys_gazebo_iob";
      }
      std::cerr << "[iob] set node name : " << node_name << std::endl;
    }

    std::map<std::string, std::string> arg;
    ros::init(arg, "hrpsys_gazebo_iob", ros::init_options::NoSigintHandler);
    rosnode = new ros::NodeHandle();
    ros::WallDuration(0.5).sleep(); // wait for initializing ros

    std::string controller_name;
    { // setting configuration name
      char *ret = getenv("HRPSYS_GAZEBO_CONFIGURATION");
      if (ret != NULL) {
        controller_name.assign(ret);
      } else {
        controller_name = "hrpsys_gazebo_configuration";
      }
      ROS_INFO_STREAM( "[iob] set controller_name : " << controller_name);
    }
    std::string robotname;
    { // setting configuration name
      char *ret = getenv("HRPSYS_GAZEBO_ROBOTNAME");
      if (ret != NULL) {
        robotname.assign(ret);
        // controller_name -> robotname/controller_name
        controller_name = robotname + "/" + controller_name;
      } else {
        std::string rname_str = std::string(controller_name) + "/robotname";
        rosnode->getParam(rname_str, robotname);
      }
      if (robotname.empty()) {
        ROS_ERROR("[iob] did not find robot_name");
        robotname = "default";
      }
      ROS_INFO_STREAM( "[iob] set robot_name : " << robotname);
    }

    { // setting synchronized
      char *ret = getenv("HRPSYS_GAZEBO_IOB_SYNCHRONIZED");
      if (ret != NULL) {
        std::string ret_str(ret);
        if (ret_str.size() > 0) {
          iob_synchronized = true;
        }
      } else {
        iob_synchronized = false;
      }
      if(rosnode->hasParam(controller_name + "/use_synchronized_command")) {
        rosnode->getParam(controller_name + "/use_synchronized_command", iob_synchronized);
      }
      if(iob_synchronized) ROS_INFO("[iob] use synchronized command");
    }
    { // setting substeps
      char *ret = getenv("HRPSYS_GAZEBO_IOB_SUBSTEPS");
      if (ret != NULL) {
        int num = atoi(ret);
        if (num > 0) {
          num_of_substeps = num;
          ROS_INFO("[iob] use substeps %d", num);
        }
      }
      if(rosnode->hasParam(controller_name + "/iob_substeps")) {
        rosnode->getParam(controller_name + "/iob_substeps", num_of_substeps);
        ROS_INFO("[iob] use substeps %d", num_of_substeps);
      }
    }
    { // settting rate
      if(rosnode->hasParam(controller_name + "/iob_rate")) {
        double rate = 0;
        rosnode->getParam(controller_name + "/iob_rate", rate);
        overwrite_g_period_ns = (long) ((1000 * 1000 * 1000) / rate);
        fprintf(stderr, "iob::period %d\n", overwrite_g_period_ns);
        ROS_INFO("[iob] period_ns %d", overwrite_g_period_ns);
      }
    }

    joint_real2model_vec.resize(0);

    if (rosnode->hasParam(controller_name + "/joint_id_list")) {
      XmlRpc::XmlRpcValue param_val;
      rosnode->getParam(controller_name + "/joint_id_list", param_val);
      if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
        for(int i = 0; i < param_val.size(); i++) {
          int num = param_val[i];
          joint_real2model_vec.push_back(num);
//.........这里部分代码省略.........

bool testDummyAction(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
  ROS_INFO("Beginning dummy action test");
  dummy_action(5);
  return true;
}

int pcd_from_ply_main(int argc, char **argv)
{

	if (pcl::console::find_switch (argc, argv, "-h"))
	{
		show_help (argv[0]);
		exit (0);
	}
  		
	// parse the command line
	std::vector<int> ply_idx = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
	std::string ply_file_path( argv[ply_idx[0]] );
	
	std::vector<int> pcd_idx = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
	std::string pcd_file_path( argv[pcd_idx[0]] );
	
	// default options
	double vpx = 1.0, vpy = 0.0, vpz = 0.0;
	double tpx = 0.0, tpy = 0.0, tpz = 0.0;
	int coord = 1; 
	bool org = false, add_noise = false, ascii = false;
	double sub_leaf_size = -1.0;
	
	// command line options
	pcl::console::parse_3x_arguments (argc, argv, "-vp", vpx, vpy, vpz);
	pcl::console::parse_3x_arguments (argc, argv, "-tp", tpx, tpy, tpz);
	pcl::console::parse_argument (argc, argv, "-coord", coord);
	pcl::console::parse_argument (argc, argv, "-add_noise", add_noise);
	pcl::console::parse_argument (argc, argv, "-org", org);
	pcl::console::parse_argument (argc, argv, "-ascii", ascii);
	pcl::console::parse_argument (argc, argv, "-subsamp", sub_leaf_size);
	
	// Construct and initialize the virtual kinect
	vkin_offline vko( Eigen::Vector3d(0,0,0), Eigen::Vector4d(0,0,0,1), coord, org, add_noise );
	vko.init_vkin( ply_file_path );
	
	// get a noiseless cloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr( vko.sense( Eigen::Vector3d(vpx, vpy, vpz), 
															Eigen::Vector3d(tpx, tpy, tpz) ) );
	
	
	if( sub_leaf_size > 0 )
		cld_ptr = pcd_utils::voxel_grid_subsample( cld_ptr, static_cast<float>(sub_leaf_size) );
	
															 
	// save the scan to file
	pcl::PCDWriter writer;
	if(ascii)
		writer.writeASCII( pcd_file_path.c_str(), *cld_ptr );
	else
    	writer.writeBinaryCompressed( pcd_file_path.c_str(), *cld_ptr );
    
    // Extract the tree scores if requested
    std::string score_save_path;
    if (pcl::console::parse_argument (argc, argv, "-save_scores", score_save_path) != -1)
    {
    	// Create and initialize an nbv tree
    	std::string vision_module_path(ros::package::getPath("vision_module"));
    	vtree_user vt("/load", vision_module_path + "/data", vision_module_path + "/../data/cloud_data");
		vt.start();
    	
    	/*
    	ros::init(argc, argv, "ply2pcd_node");
		ros::NodeHandle node_handle("~");
		NBVTree nbv_tree(node_handle);
		nbv_tree.start();
		*/
		
		// Get the score list
		ROS_INFO("Getting matches...");
		std::vector<std::pair<float,std::string> > match_names;
		vt.match_list( cld_ptr->getMatrixXfMap(), match_names, 100 );
		
		
		// write to file
		std::ofstream outstr;
		io_utils::open_out_file( outstr, score_save_path );
		
		if( !outstr )
			throw std::runtime_error("Cannot open scores save file...\n");
		
		for( size_t i = 0; i < match_names.size(); ++i)
		{
			outstr << match_names[i].first << " " << match_names[i].second << std::endl;
			//outstr << cluster_match_names[i].first << " " << cluster_match_names[i].second << std::endl;
		}
		
		outstr.close();
    }
    
    return 0;
}

void help()
{
  ROS_INFO("#####################################################");
  ROS_INFO("RVIZ SETUP");
  ROS_INFO("----------");
  ROS_INFO("  Global options:");
  ROS_INFO("    FixedFrame = /base_footprint");
  ROS_INFO("  Add a RobotState display:");
  ROS_INFO("    RobotDescription = robot_description");
  ROS_INFO("    RobotStateTopic  = interactive_robot_state");
  ROS_INFO("  Add a Marker display:");
  ROS_INFO("    MarkerTopic = interactive_robot_markers");
  ROS_INFO("  Add an InteractiveMarker display:");
  ROS_INFO("    UpdateTopic = interactive_robot_imarkers/update");
  ROS_INFO("  Add a MarkerArray display:");
  ROS_INFO("    MarkerTopic = interactive_robot_marray");
  ROS_INFO("#####################################################");
}

//test get the current time
bool testGetTime(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
  ROS_INFO("Getting the current local time");
  std::string t = getCurrentStringTime();
  ROS_INFO("current time: %s, seconds since midnight: %ld",t.c_str(),secondsFromStringTime(t));
  return true;
}

void Square::sensorCallback(const create_fundamentals::SensorPacket::ConstPtr& msg){
	ROS_INFO("left encoder: %f, right encoder: %f", msg->encoderLeft, msg->encoderRight);
}

DvsRosDriver::DvsRosDriver(ros::NodeHandle & nh, ros::NodeHandle nh_private) :
    nh_(nh), parameter_update_required_(false)
{
  // load parameters
  nh_private.param<std::string>("serial_number", device_id_, "");
  nh_private.param<bool>("master", master_, true);
  double reset_timestamps_delay;
  nh_private.param<double>("reset_timestamps_delay", reset_timestamps_delay, -1.0);

  // start driver
  bool device_is_running = false;
  while (!device_is_running)
  {
    const char* serial_number_restrict = (device_id_ == "") ? NULL : device_id_.c_str();
    dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, serial_number_restrict);

    //dvs_running = driver_->isDeviceRunning();
    device_is_running = !(dvs128_handle == NULL);

    if (!device_is_running)
    {
      ROS_WARN("Could not find DVS. Will retry every second.");
      ros::Duration(1.0).sleep();
      ros::spinOnce();
    }
    else
    {
      // configure as master or slave
      caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_DVS, DVS128_CONFIG_DVS_TS_MASTER, master_);
    }

    if (!ros::ok())
    {
      return;
    }
  }

  dvs128_info_ = caerDVS128InfoGet(dvs128_handle);
  device_id_ = "DVS128-V1-" + std::string(dvs128_info_.deviceString).substr(15, 4);

  ROS_INFO("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", dvs128_info_.deviceString,
           dvs128_info_.deviceID, dvs128_info_.deviceIsMaster, dvs128_info_.dvsSizeX, dvs128_info_.dvsSizeY,
           dvs128_info_.logicVersion);

  current_config_.streaming_rate = 30;
  delta_ = boost::posix_time::microseconds(1e6/current_config_.streaming_rate);

  // set namespace
  std::string ns = ros::this_node::getNamespace();
  if (ns == "/")
    ns = "/dvs";
  event_array_pub_ = nh_.advertise<dvs_msgs::EventArray>(ns + "/events", 1);
  camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(ns + "/camera_info", 1);

  // camera info handling
  ros::NodeHandle nh_ns(ns);
  camera_info_manager_ = new camera_info_manager::CameraInfoManager(nh_ns, device_id_);

  // reset timestamps is publisher as master, subscriber as slave
  if (master_)
  {
    reset_pub_ = nh_.advertise<std_msgs::Time>((ns + "/reset_timestamps").c_str(), 1);
  }
  else
  {
    reset_sub_ = nh_.subscribe((ns + "/reset_timestamps").c_str(), 1, &DvsRosDriver::resetTimestampsCallback, this);
  }

  // initialize timestamps
  resetTimestamps();

  // spawn threads
  running_ = true;
  parameter_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::changeDvsParameters, this)));
  readout_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::readout, this)));

  // Dynamic reconfigure
  dynamic_reconfigure_callback_ = boost::bind(&DvsRosDriver::callback, this, _1, _2);
  server_.reset(new dynamic_reconfigure::Server<dvs_ros_driver::DVS_ROS_DriverConfig>(nh_private));
  server_->setCallback(dynamic_reconfigure_callback_);

  // start timer to reset timestamps for synchronization
  if (reset_timestamps_delay > 0.0 && master_)
  {
    timestamp_reset_timer_ = nh_.createTimer(ros::Duration(reset_timestamps_delay), &DvsRosDriver::resetTimerCallback, this);
    ROS_INFO("Started timer to reset timestamps on master DVS for synchronization (delay=%3.2fs).", reset_timestamps_delay);
  }
}

 BumperToMTSA(ros::NodeHandle node_handle,ros::NodeHandle private_node_handle )
   : nh(node_handle), private_nh(private_node_handle)
 {
   ROS_INFO("BumperToMTSA");
   pickput_detect = 0;
 }

// Main code:
int main(int argc,char* argv[])
{
	// (when V-REP launches this executable, V-REP will also provide the argument list)
	//numero de argumentos que mande (se excluye el fantasma que el manda solo)
	int Narg=0, handleArana=0, k=0;
	int PataTipHandle[Npatas],FuerzaSensorHandle[Npatas],Pata_Motor1Handle[Npatas];
	float periodo, f;
	camina::UbicacionRobot ubicacionRobot;
    geometry_msgs::PoseStamped CuerpoPose;
    geometry_msgs::PoseStamped PataTipPose[Npatas];
    tf::Quaternion CuerpoOrientacion_Q;
    tfScalar roll, pitch, yaw;

	Narg=13;

	if (argc>=Narg)
	{
        handleArana=atoi(argv[1]);
        for (k=0;k<Npatas;k++) PataTipHandle[k] = atoi(argv[2+k]);
        for (k=0;k<Npatas;k++) FuerzaSensorHandle[k] = atoi(argv[2+Npatas+k]);
        for (k=0;k<Npatas;k++) Pata_Motor1Handle[k] = atoi(argv[2+2*Npatas+k]);
	}
	else
	{
		ROS_ERROR("Nodo6:Indique argumentos completos!\n");
		return (0);
	}
    // Inicializacion de variables del mensaje
    for (k=0;k<Npatas;k++) {
        ubicacionRobot.coordenadaPata_y.push_back(0);
        ubicacionRobot.coordenadaPata_x.push_back(0);
        ubicacionRobot.coordenadaPata_z.push_back(0);
        ubicacionRobot.coordenadaPataSistemaPata_y.push_back(0);
        ubicacionRobot.coordenadaPataSistemaPata_x.push_back(0);
        ubicacionRobot.coordenadaPataSistemaPata_z.push_back(0);
        ubicacionRobot.pataTipFuerza_z.push_back(0);
        ubicacionRobot.pataApoyo.push_back(0);
    }
	// Create a ROS node:
	int _argc = 0;
	char** _argv = NULL;
	ros::init(_argc,_argv,"Nodo6_UbicacionArana");
	if(!ros::master::check()) return(0);

	ros::NodeHandle node;
	ROS_INFO("Nodo6_UbicacionArana just started\n");

    periodo=0.1;
    f=1/periodo;
    ros::Rate loop_rate(f);  //Frecuencia [Hz]

    //Topicos susbcritos y publicados
    ros::Subscriber subInfo=node.subscribe("/vrep/info",1,infoCallback);
    ros::Publisher chatter_pub = node.advertise<camina::UbicacionRobot>("UbicacionRobot", 100);
    //Clientes y Servicios
    client_simRosGetObjectPose=node.serviceClient<vrep_common::simRosGetObjectPose>("/vrep/simRosGetObjectPose");
    client_simRosReadForceSensor=node.serviceClient<vrep_common::simRosReadForceSensor>("/vrep/simRosReadForceSensor");
    srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;

    // Creamos archivo para guardar posiciones de patas
//    fp = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/XYPatas.txt","w+");
//    int cuentaPataApoyo=0, pataApoyo[Npatas];
	while (ros::ok() && simulationRunning)
	{
		ros::spinOnce();
		loop_rate.sleep();
		// Primero buscamos posicion del cuerpo de Arana
		srv_simRosGetObjectPose.request.handle=handleArana;
		srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;
	    if (client_simRosGetObjectPose.call(srv_simRosGetObjectPose)&&(srv_simRosGetObjectPose.response.result!=-1))
        {
            CuerpoPose = srv_simRosGetObjectPose.response.pose;
//            ROS_INFO("posicion: x=%.3f; y=%.3f; z=%.3f\n", CuerpoPose.pose.position.x,CuerpoPose.pose.position.y,CuerpoPose.pose.position.z);
            ubicacionRobot.coordenadaCuerpo_x = CuerpoPose.pose.position.x;
            ubicacionRobot.coordenadaCuerpo_y = CuerpoPose.pose.position.y;
            tf::quaternionMsgToTF(CuerpoPose.pose.orientation,CuerpoOrientacion_Q);
            tf::Matrix3x3(CuerpoOrientacion_Q).getRPY(roll, pitch, yaw);
            ubicacionRobot.orientacionCuerpo_roll = roll;
            ubicacionRobot.orientacionCuerpo_pitch = pitch;
            ubicacionRobot.orientacionCuerpo_yaw = yaw;
//            ROS_INFO("orientacion: z=%.3f\n", ubicacionRobot.orientacionCuerpo_yaw);
        } else {
             ROS_ERROR("Nodo 6: servicio de posicion cuerpo no funciona\n");
            }

        for (k=0;k<Npatas;k++){
            ros::spinOnce();
//            loop_rate.sleep();
            srv_simRosGetObjectPose.request.handle=PataTipHandle[k];
        //--------------------------------------------------------------
        //---- Obtengo posicion de tip de pata en el mundo
            srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;
            if (client_simRosGetObjectPose.call(srv_simRosGetObjectPose)&&(srv_simRosGetObjectPose.response.result!=-1))
            {
                PataTipPose[k] = srv_simRosGetObjectPose.response.pose;
                ubicacionRobot.coordenadaPata_y[k] = PataTipPose[k].pose.position.y;
                ubicacionRobot.coordenadaPata_x[k] = PataTipPose[k].pose.position.x;
                ubicacionRobot.coordenadaPata_z[k] = PataTipPose[k].pose.position.z;
//                printf("Nodo6: [%d]: y= %.3f, x= %.3f, z= %.3f\n",k+1,ubicacionRobot.coordenadaPata_y[k],ubicacionRobot.coordenadaPata_x[k],ubicacionRobot.coordenadaPata_z[k]);
//.........这里部分代码省略.........

 void BumperToMTSA::subscriber_init(){
   ROS_INFO("subscriber_init");
   std::string bumper_topic_name = "/mobile_base/events/bumper";
   bumper_sub = nh.subscribe<kobuki_msgs::BumperEvent::ConstPtr>(bumper_topic_name,10, &BumperToMTSA::bumper_callback, this);
   pickput_sub = nh.subscribe("chatter",1000,&BumperToMTSA::pickput_callback,this);
 }

// Main loop
int main(int argc, char** argv)
{
	// ROS Initialization
	ros::init(argc, argv, "Roomba_Driver");		// creates ROS node
	ros::NodeHandle node;				// creates the node handle
	ros::Rate loop_rate(100); 			// testing rate (Hz)
	
	// ROS subscribers
	ros::Subscriber joy_sub;			// create subscriber to listen to joystick
	joy_sub = node.subscribe("joy",1,joy_callback); // this tells our node what to subscribe to, 1 is buffer size of 1
	ros::Subscriber track_sub = node.subscribe("UAV_Trackee_RelPos", 1, tracking_callback);
	// ROS publishers
/*	
	ros::Publisher pos_pub = node.advertise<geometry_msgs::Vector3>("position",1);
	ros::Publisher yaw_pub = node.advertise<std_msgs::Float32>("yaw",1);
	ros::Publisher u_pub = node.advertise<geometry_msgs::Vector3>("control_input",1);
*/
	// ROS Service client
	ros::ServiceClient client = node.serviceClient<irobot_create_2_1::Tank>("tank");

	// Initialize service
	irobot_create_2_1::Tank srv;

	// loop until time or ros not ok
	while(ros::ok())
	{
		ros::spinOnce();	// Receive callbacks	
		merge_new_msgs();   	// Merge joysticks
		
		if(joy_a) // check error in x
			{
			merge_new_msgs();
			output = mixer(tag_y,0.0);
			srv.request.left = output.x;;
			srv.request.right= output.y;
			srv.request.clear= 1;
			client.call(srv);
			
			ROS_INFO("Auto Roomba Track x: %f" ,output.x);
			ros::spinOnce();
			loop_rate.sleep();
			}
/*
		if(joy_b) // Stop when B is pressed
			start_flag = false;

		if(start_flag) // If A has been pressed, run main loop
				{
*/
else{
			output = mixer(joy_x, joy_y);
			
			// set srv values
			srv.request.left = output.x;
			srv.request.right= output.y;
			srv.request.clear= 1;
			// send out value to robot
			if(client.call(srv))
			{
				//ROS_INFO("Sending stuff");
				ROS_INFO("Roomba Joy %f %f",output.x,output.y);
				//ROS_INFO("service %d %d",srv.request.left,srv.request.right);
				if (srv.response.success)
					{//ROS_INFO("Response");
}
				else
					{ROS_INFO("No Response");}
			}
			else
				ROS_INFO("NOT Sending stuff");
	}

}//rosok

}//main

//引用传参比较好，不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
      ros::NodeHandle node_handle;
    //添加物体

    // Advertise the required topic
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // Note that this topic may need to be remapped in the launch file
      ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
      while(planning_scene_diff_publisher.getNumSubscribers() < 1)
      {
        ros::WallDuration sleep_t(0.5);
        sleep_t.sleep();
      }

    // Define the attached object message
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // We will use this message to add or
    // subtract the object from the world
    // and to attach the object to the robot
      moveit_msgs::AttachedCollisionObject attached_object;
      attached_object.link_name = "lLink7";
      /* The header must contain a valid TF frame*/
      attached_object.object.header.frame_id = group.getPlanningFrame();

      /* The id of the object */
      attached_object.object.id = "box1";

      /* A default left pose */
      geometry_msgs::Pose pose1;
      pose1.orientation.w = 1.0;
      pose1.position.x=0.4;
      pose1.position.y=0.64;
      pose1.position.z=0.6;
      /* Define a left box to be attached */
      shape_msgs::SolidPrimitive primitive1;
      primitive1.type = primitive1.BOX;
      primitive1.dimensions.resize(3);
      primitive1.dimensions[0] = 0.03;
      primitive1.dimensions[1] = 0.03;
      primitive1.dimensions[2] = 0.2;

      /* A default right pose */
      geometry_msgs::Pose pose2;
      pose2.orientation.w = 1.0;
      pose2.position.x=-0.4;
      pose2.position.y=0.7;
      pose2.position.z=0.4;
      /* Define a right box to be attached */
      shape_msgs::SolidPrimitive primitive2;
      primitive2.type = primitive2.BOX;
      primitive2.dimensions.resize(3);
      primitive2.dimensions[0] = 0.3;
      primitive2.dimensions[1] = 0.3;
      primitive2.dimensions[2] = 0.4;

      //容器使用push_back进行添加元素
      attached_object.object.primitives.push_back(primitive1);
      attached_object.object.primitive_poses.push_back(pose1);
      attached_object.object.primitives.push_back(primitive2);
      attached_object.object.primitive_poses.push_back(pose2);

    // Note that attaching an object to the robot requires
    // the corresponding operation to be specified as an ADD operation
      attached_object.object.operation = attached_object.object.ADD;


    // Add an object into the environment
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // Add the object into the environment by adding it to
    // the set of collision objects in the "world" part of the
    // planning scene. Note that we are using only the "object"
    // field of the attached_object message here.
      ROS_INFO("Adding the object into the world ");
      moveit_msgs::PlanningScene planning_scene;
      planning_scene.world.collision_objects.push_back(attached_object.object);
      planning_scene.is_diff = true;
      planning_scene_diff_publisher.publish(planning_scene);
      sleep(2);
}

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

    ros::AsyncSpinner spinner(1);
    spinner.start();

    //sleep(20.0);//等rviz起来，如果没在一个lanuch中，可以省去这个

    moveit::planning_interface::MoveGroup group("dual_arms");

    // Getting Basic Information
    ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());//打印坐标系
    ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());//打印末端链节

    add_object(group);//添加左右两个物体

    geometry_msgs::PoseStamped current_pose = group.getCurrentPose("lLink7");
    ROS_INFO("current left pose:x=%f,y=%f,z=%f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    ROS_INFO("current let qurternion:x=%f,y=%f,z=%f,w=%f",current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w);
    std::cout<<"左臂初始RPY=";
    float r,p,y;
    xyzw_to_rpy(-1,0,0,0,r,p,y);
    current_pose = group.getCurrentPose("rLink7");
    ROS_INFO("current right pose:x=%f,y=%f,z=%f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    ROS_INFO("current right quaternion:x=%f,y=%f,z=%f,w=%f",current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w);
    std::cout<<"右臂初始RPY=";
    xyzw_to_rpy(-1,0,0,0,r,p,y);

    //给左臂末端赋值
    geometry_msgs::Pose testPose1 = current_pose.pose;
    geometry_msgs::Pose pose1;//和上面添加物体函数中给水杯添加的位置一样
    pose1.orientation.w = 1.0;
    pose1.position.x=0.4;
    pose1.position.y=0.64;
    pose1.position.z=0.6;
    testPose1.position = pose1.position;
    testPose1.position.y = pose1.position.y - 0.14;//不能碰杯子否则规划失败，所以留出一些空间
    std::cout<<"左臂goal的欧拉角：";
    xyzw_to_rpy(0.699477,0.714655,0.000155,0.000255,r,p,y);//显示下左臂goal的欧拉角
    std::cout<<"水杯的欧拉角";
    xyzw_to_rpy(pose1.orientation.x,pose1.orientation.y,pose1.orientation.z,pose1.orientation.w,r,p,y);//得到水杯的欧拉角
    rpy_to_xyzw(3.14+r,0+p,1.6+y,testPose1);
    std::cout<<"左臂goal的四元数：";
    std::cout<<testPose1.orientation.x<<testPose1.orientation.y<<testPose1.orientation.z<<testPose1.orientation.w<<std::endl;
    //给右臂末端赋值
    geometry_msgs::Pose testPose2 = current_pose.pose;
    testPose2.position.x = -0.385743;
    testPose2.position.y = 0.467167;
    testPose2.position.z = 0.674107;
//    testPose2.orientation.x = 0.702235;
//    testPose2.orientation.y =-0.710211;
//    testPose2.orientation.z = -0.033619;
//    testPose2.orientation.w = 0.036564;
    xyzw_to_rpy(0.70223,-0.710211,-0.03361,0.036564,r,p,y);
    rpy_to_xyzw(3.14,0,-1.5,testPose2);
    std::cout<<testPose2.orientation.x<<testPose2.orientation.y<<testPose2.orientation.z<<testPose2.orientation.w<<std::endl;


    //设定home位置
    std::vector<double> group_variable_values;
    group_variable_values = group.getCurrentJointValues();
    //c++98不支持，c++11新特性
    //for(double d : group_variable_values){
    //std::cout << d << std::endl;
    //    }
    std::cout<<"打印初始位姿的各个关节角度值:"<<std::endl;
    std::vector<double>::iterator d = group_variable_values.begin();
    while(d != group_variable_values.end()) {
        std::cout << *(d++) << std::endl;

    }
    for(int i=0; i<14; i++){
        group_variable_values[i] = 0.0;
    }

//group.setMaxVelocityScalingFactor(0.1);


    //规划运动到目标位置
    group.setPoseTarget(testPose1,"lLink7");
    group.setPoseTarget(testPose2,"rLink7");
    //group.asyncMove();
    moveit::planning_interface::MoveGroup::Plan plan_goal;
    group.plan(plan_goal);
    group.asyncExecute(plan_goal);

    //通过命令去控制规划到home还是goal位置
    std::string s1 = "go home";
    std::string command;
    std::string s2 = "go to the goal";
    moveit::planning_interface::MoveGroup::Plan plan_home;   
    while(1){
        std::cout << "Please input command(Eg:go home,go to the goal):";
        std::getline(std::cin, command);
        if(command == s1){
            group.setJointValueTarget(group_variable_values);
            group.plan(plan_home);
            group.asyncExecute(plan_home);
        }else if(command == s2){
//.........这里部分代码省略.........

int ConfigManager::readConfigFile(int fd, std::istream* stream)
{
    YAML::Parser p;
    YAML::Node doc;
    int ret = 0;
    std::vector<v4l2_ext_control> controls;

    try
    {
        p.Load(*stream);
    }
    catch(YAML::Exception& e)
    {
        ROS_FATAL("Could not parse config file: %s", e.what());
        return -1;
    }

    if(!p.GetNextDocument(doc))
    {
        ROS_FATAL("Could not get YAML document");
        return -1;
    }

    for(YAML::Iterator it = doc.begin(); it != doc.end(); ++it)
    {
        std::string key, value;

        try
        {
            (*it).begin().first() >> key;
            (*it).begin().second() >> value;
        }
        catch(YAML::Exception& e)
        {
            ROS_FATAL("Invalid parameter specification: %s", e.what());
            return -1;
        }

        int ivalue = -1;
        calibration::CameraParam* info = getParamInfo(key);
        if(!info)
        {
            ROS_FATAL("Unknown parameter '%s' in config file, ignoring...", key.c_str());
            continue;
        }

        ivalue = atoi(value.c_str());

        info->value = ivalue;
        v4l2_ext_control control;
        memset(&control, 0, sizeof(control));
        control.id = info->id;
        control.value = ivalue;

        controls.push_back(control);
    }

    v4l2_ext_controls request;
    memset(&request, 0, sizeof(request));
    request.controls = &controls[0];
    request.count = controls.size();
    request.ctrl_class = V4L2_CTRL_CLASS_USER;

    if(ioctl(fd, VIDIOC_S_EXT_CTRLS, &request) != 0)
    {
        ROS_FATAL("Control setting failed. This may have happened at control 0x%X with value %d",
                  controls[request.error_idx].id,
                  controls[request.error_idx].value
                 );
        return -1;
    }
    ROS_INFO("Controls set successfully from config file");

    return ret;
}

//.........这里部分代码省略.........
      z1   =atof(argv[5]);
      gx1  =atof(argv[6]);
      gy1  =atof(argv[7]);
      gz1  =atof(argv[8]);

      mode2=atof(argv[9]);
      x2   =atof(argv[10]);
      y2   =atof(argv[11]);
      z2   =atof(argv[12]);
    }

  // Both Controllers with SetPoint and Gains
  else if(argc==16 && numCtrls==2)
    {
      mode1=atof(argv[2]);      
      x1   =atof(argv[3]);
      y1   =atof(argv[4]);
      z1   =atof(argv[5]);
      gx1  =atof(argv[6]);
      gy1  =atof(argv[7]);
      gz1  =atof(argv[8]);

      mode2=atof(argv[9]);
      x2   =atof(argv[10]);
      y2   =atof(argv[11]);
      z2   =atof(argv[12]);
      gx2  =atof(argv[13]);
      gy2  =atof(argv[14]);
      gz2  =atof(argv[15]);
    }

  else
    {
      ROS_INFO("Wrong number of arguments in the commmand line.");
    }

  // B. Initialize ROS data
  // Get side Parameter
  std::string side_="";
  n.param<std::string>("side", side_, "right");

  // Create the publisher 
  ros::Publisher setPoint_pub = n.advertise<force_controller::setPoint>("/" + side_ + "/force_control/setPoint",1); 

  // Populate the setPoint msg
  force_controller::setPoint sP;

  // Set Control Type, Desired set-point and gains for 1 or 2 controllers. 
  if(numCtrls < 1 || numCtrls > 2)
    {
      ROS_ERROR("Wrong number of controllers given");
      return -1;
    }
  else
    {
      // Number of Controllers
      sP.num_ctrls=numCtrls;

      /********** Dominant Controller ************/
      /*** CONTROL TYPE ***/
      // Dominant Controller Type
      if(mode1==0)           sP.domType="force";
      else if(mode1==1)      sP.domType="moment";

      /*** SETPOINT ***/
      geometry_msgs::Vector3 d;

int ConfigManager::init(int fd, const std::vector<std::string>& controls)
{
    v4l2_queryctrl q;
    memset (&q, 0, sizeof (q));

    m_controls.params.clear();
    m_controls.params.resize(controls.size());

    q.id = V4L2_CTRL_FLAG_NEXT_CTRL;
    while(1)
    {
        if(ioctl(fd, VIDIOC_QUERYCTRL, &q) != 0)
        {
            if(errno == EINVAL)
                break;

            ROS_FATAL("Could not query control: %d: %s", errno, strerror(errno));
            return -1;
        }

        if (q.flags & V4L2_CTRL_FLAG_DISABLED)
        {
            ROS_INFO("skipping disabled control '%s'", (const char*)q.name);
            continue;
        }

        v4l2_control control;
        memset(&control, 0, sizeof(control));
        control.id = q.id;

        if(ioctl(fd, VIDIOC_G_CTRL, &control) != 0)
        {
            ROS_FATAL("Could not get control value");
            return -1;
        }

//		ROS_INFO("(id=0x%X) Control '%s' (type %s): %d", q.id, q.name, v4l2_type_to_str(q.type), control.value);

        calibration::CameraParam param;
        param.id = q.id;
        param.label = (const char*)q.name;
        param.maximum = q.maximum;
        param.minimum = q.minimum;
        param.value = control.value;

        if(q.type == V4L2_CTRL_TYPE_MENU)
        {
            if(enumerateMenu(fd, &param, q) != 0)
            {
                ROS_FATAL("Could not query menu");
                return -1;
            }
        }

        std::vector<std::string>::const_iterator it = std::find(
                    controls.begin(), controls.end(), param.label
                );

        if(it != controls.end())
        {
            int idx = it - controls.begin();
            m_controls.params[idx] = param;
        }
        else
            ROS_INFO("Ignoring unknown parameter '%s'", q.name);

        q.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    }

    bool error = false;
    for(size_t i = 0; i < m_controls.params.size(); ++i)
    {
        if(m_controls.params[i].label == "")
        {
            ROS_ERROR("Could not find whitelisted camera parameter '%s'", controls[i].c_str());
            error = true;
        }
    }

    if(error)
        return -1;

    return 0;
}

//printing the asynchronous data;
void asyncDataListener(Vn100* vn100,Vn100CompositeData* data)
{
	ros::Time timestamp=ros::Time::now();
	static int seq =0;
	ROS_INFO("\nASYNC DATA:\n"
	         "YPR.Yaw: %+#7.2f\n"
	         "YPR.pitch: %+#7.2f\n"
	         "YPR.roll: %+#7.2f\n",
	         data->ypr.yaw,
	         data->ypr.pitch, 
	         data->ypr.roll);
	ROS_INFO(
	     "\n quaternion.X:  %+#7.2f\n"
	     "   quaternion.Y:  %+#7.2f\n"
	     "   quaternion.Z:  %+#7.2f\n"
	     "   quaternion.W:  %+#7.2f\n", 
	        data->quaternion.x,
	        data->quaternion.y,
	        data->quaternion.z,
	        data->quaternion.w);
	ROS_INFO(
	       "\n            {Value,Voltage}\n"
	       " magnetic X:  %+#7.2f, %+#7.2f\n"
	       " magnetic Y:  %+#7.2f, %+#7.2f\n"
	       " magnetic Z:  %+#7.2f, %+#7.2f\n",
	         data->magnetic.c0,data->magneticVoltage.c0, 
	         data->magnetic.c1,data->magneticVoltage.c1,
	         data->magnetic.c2,data->magneticVoltage.c2);
	ROS_INFO(
	       "\n acceleration X:    %+#7.2f,%+#7.2f\n"
	       "  acceleration Y:     %+#7.2f,%+#7.2f\n"
	       "  acceleration Z:     %+#7.2f,%+#7.2f\n",
	          data->acceleration.c0,
	          data->accelerationVoltage.c0,
	          data->acceleration.c1,
	          data->accelerationVoltage.c1,
	          data->acceleration.c2,
	          data->accelerationVoltage.c2);
	ROS_INFO(
	       "\n                   {Value ,Voltage,Bias,BiasVariance}"
       	"  angularRate X:          %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n"
	  "  angularRate Y:          %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n"
	  "  angularRate Z:          %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n",
		  data->angularRate.c0,
	          data->angularRateVoltage.c0, 
		  data->angularRateBias.c0,
	          data->angularRateBiasVariance.c0, 
		  data->angularRate.c1,    
	          data->angularRateVoltage.c1, 
		  data->angularRateBias.c1, 
	          data->angularRateBiasVariance.c1, 
		  data->angularRate.c2,    
	          data->angularRateVoltage.c2,
		  data->angularRateBias.c2, data->angularRateBiasVariance.c2);
	 ROS_INFO(
		  "\n  Attitude Variance X:    %+#7.2f\n"
		  "  Attitude Variance Y:    %+#7.2f\n"
		  "  Attitude Variance Z:    %+#7.2f\n",
		  data->attitudeVariance.c0, 
		  data->attitudeVariance.c1, 
		  data->attitudeVariance.c2);
	
	 ROS_INFO(
		  "\n  Direction Cosine Matrix:\n"
		  "    %+#7.2f, %+#7.2f, %+#7.2f\n"
		  "    %+#7.2f, %+#7.2f, %+#7.2f\n"
		  "    %+#7.2f, %+#7.2f, %+#7.2f\n",
		  data->dcm.c00, data->dcm.c01, data->dcm.c02,
		  data->dcm.c10, data->dcm.c11, data->dcm.c12,
		  data->dcm.c20, data->dcm.c21, data->dcm.c22);
	
	    ROS_INFO(
		  "\n  Temperature:            %+#7.2f\n"
		  "  Temperature Voltage:    %+#7.2f\n",
		  data->temperature,
		  data->temperatureVoltage);
	
}

bool I2cImu::ImuSettings::loadSettings()
{
	ROS_INFO("%s: reading IMU parameters from param server", __FUNCTION__);
	int temp_int;

	// General
	settings_nh_->getParam("imu_type", m_imuType);
	settings_nh_->getParam("fusion_type", m_fusionType);

	if(settings_nh_->getParam("i2c_bus", temp_int))
		m_I2CBus = (unsigned char) temp_int;

	if(settings_nh_->getParam("i2c_slave_address", temp_int))
			m_I2CSlaveAddress = (unsigned char) temp_int;


	//double declination_radians;
	//settings_nh_->param("magnetic_declination", declination_radians, 0.0);
	//m_compassAdjDeclination = angles::to_degrees(declination_radians);

	//MPU9150
	settings_nh_->getParam("mpu9150/gyro_accel_sample_rate", m_MPU9150GyroAccelSampleRate);
	settings_nh_->getParam("mpu9150/compass_sample_rate", m_MPU9150CompassSampleRate);
	settings_nh_->getParam("mpu9150/accel_full_scale_range", m_MPU9150AccelFsr);
	settings_nh_->getParam("mpu9150/gyro_accel_low_pass_filter", m_MPU9150GyroAccelLpf);
	settings_nh_->getParam("mpu9150/gyro_full_scale_range", m_MPU9150GyroFsr);

	//MPU9250
	settings_nh_->getParam("mpu9250/gyro_accel_sample_rate", m_MPU9250GyroAccelSampleRate);
	settings_nh_->getParam("mpu9250/compass_sample_rate", m_MPU9250CompassSampleRate);
	settings_nh_->getParam("mpu9250/accel_full_scale_range", m_MPU9250AccelFsr);
	settings_nh_->getParam("mpu9250/accel_low_pass_filter", m_MPU9250AccelLpf);
	settings_nh_->getParam("mpu9250/gyro_full_scale_range", m_MPU9250GyroFsr);
	settings_nh_->getParam("