int main(int argc, char** argv) {
    ros::init(argc, argv, "merry_motionplanner_test"); // name this node 
    ros::NodeHandle nh; //standard ros node handle

    MerryMotionplanner merry_motionplanner(&nh);
    CwruPclUtils cwru_pcl_utils(&nh);


    while (!merry_motionplanner.got_kinect_cloud()) {
        ROS_INFO("did not receive pointcloud");
        ros::spinOnce();
        ros::Duration(1.0).sleep();
    }
    ROS_INFO("got a pointcloud");
    tf::StampedTransform tf_sensor_frame_to_torso_frame; //use this to transform sensor frame to torso frame
    tf::TransformListener tf_listener; //start a transform listener

    //let's warm up the tf_listener, to make sure it get's all the transforms it needs to avoid crashing:
    bool tferr = true;
    ROS_INFO("waiting for tf between kinect_pc_frame and torso...");
    while (tferr) {
        tferr = false;
        try {

            //The direction of the transform returned will be from the target_frame to the source_frame. 
            //Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction
            tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame);
        } catch (tf::TransformException &exception) {
            ROS_ERROR("%s", exception.what());
            tferr = true;
            ros::Duration(0.5).sleep(); // sleep for half a second
            ros::spinOnce();
        }
    }
    ROS_INFO("tf is good"); //tf-listener found a complete chain from sensor to world; ready to roll
    //convert the tf to an Eigen::Affine:
    Eigen::Affine3f A_sensor_wrt_torso;
    Eigen::Affine3d Affine_des_gripper;
    Eigen::Vector3d origin_des;
    geometry_msgs::PoseStamped rt_tool_pose;
    
    A_sensor_wrt_torso = merry_motionplanner.transformTFToEigen(tf_sensor_frame_to_torso_frame);
    Eigen::Vector3f plane_normal, major_axis, centroid;
    Eigen::Matrix3d Rmat;
    int rtn_val;    
    double plane_dist;
    
    //send a command to plan a joint-space move to pre-defined pose:
    rtn_val = merry_motionplanner.plan_move_to_pre_pose();
    
    //send command to execute planned motion
    rtn_val = merry_motionplanner.rt_arm_execute_planned_path();
    
    while (ros::ok()) {
        if (cwru_pcl_utils.got_selected_points()) {
            ROS_INFO("transforming selected points");
            cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso);

            //fit a plane to these selected points:
            cwru_pcl_utils.fit_xformed_selected_pts_to_plane(plane_normal, plane_dist);
            ROS_INFO_STREAM(" normal: " << plane_normal.transpose() << "; dist = " << plane_dist);
            major_axis= cwru_pcl_utils.get_major_axis();
            centroid = cwru_pcl_utils.get_centroid();
            cwru_pcl_utils.reset_got_selected_points();   // reset the selected-points trigger

            //input: centroid, plane_normal, major_axis
            //output: Rmat, origin_des
            origin_des = merry_motionplanner.compute_origin_des(centroid);
            Rmat = merry_motionplanner.compute_orientation(plane_normal, major_axis);

            //construct a goal affine pose:
            Affine_des_gripper = merry_motionplanner.construct_affine_pose(Rmat, origin_des);

            //convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
            rt_tool_pose.pose = merry_motionplanner.transformEigenAffine3dToPose(Affine_des_gripper);
            //could fill out the header as well...

            // send move plan request:
            rtn_val = merry_motionplanner.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
            if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS)  { 
                //send command to execute planned motion
                rtn_val = merry_motionplanner.rt_arm_execute_planned_path();
            }
            else {
                ROS_WARN("Cartesian path to desired pose not achievable");
            }
        }
        ros::Duration(0.5).sleep(); // sleep for half a second
        ros::spinOnce();
    }

    return 0;
}

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

    // Initial ROS
    ros::init(argc,argv,"lrf_server_node");

    // Create the ROS node
    ros::NodeHandle node("~");

    if(!node.hasParam("laser_topic"))
        ROS_WARN("Usage : rosrun robot_controller robot_controller_node _laser_topic:=<laser_topic>");
    node.param<std::string>("laser_topic",laser_topic,"/scan");

    if(!node.hasParam("socket_port"))
        ROS_WARN("Usage : rosrun robot_controller robot_controller_node _socket_port:=<socket_port>");
    node.param<int>("socket_port",socket_port,25650);

    // Subscribe the Topic of sensor_msgs/IMU
    ros::Subscriber sub_lrf = node.subscribe(laser_topic,100,callback_laser);

    socklen_t clilen;
    char buffer[256];
    struct sockaddr_in serv_addr, cli_addr;
    int n;

    if (argc < 2) errormsg_print(SOCKET_ERROR_NOPROVIDE_PORT);
    sockfd = socket(AF_INET, SOCK_STREAM, 0);

    // Check is there socket create error
    if(sockfd < 0) {
        fprintf(stdout, "ERROR opening socket\n");
    } else {
        fprintf(stdout, "SUCCESSFUL opening socket:%d\n",sockfd);
    }

    bzero((char*) &serv_addr, sizeof(serv_addr));


    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = INADDR_ANY;
    serv_addr.sin_port = htons(socket_port);

    // SET SOCKET REUSE Address
    int sock_opt = 1;
    if (setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,(void*)&sock_opt, sizeof(sock_opt)) == -1) {
        ROS_ERROR("ERROR ON SETUP SOCKET CONFIG\n");
        return -1;
    } else {
        ROS_INFO("SUCCESSFUL ON SETUP SOCKET CONFIG\n");
    }

    ROS_INFO("Binding socket at port:%d\n",socket_port);
    if(bind(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0)
        errormsg_print(SOCKET_ERROR_EVENT_ONBINDING);
    else
        ROS_INFO("SUCCESSFUL on binding");


    listen(sockfd, 5);
    clilen = sizeof(cli_addr);

    RECONNECT:

    newsockfd = accept(sockfd,(struct sockaddr*)&cli_addr,&clilen);

    if(newsockfd < 0) errormsg_print(SOCKET_ERROR_EVENT_ONACCEPT);


    bzero(buffer,256);

    n = read(newsockfd, buffer,255);


    if( n < 0) errormsg_print(SOCKET_ERROR_EVENT_ONREAD);
    printf("Here is the message: %s \n", buffer);




    // Into the Message loop, while there is a topic be subscribed exist then callback.
    ros::Rate r(10);
    while(ros::ok()) {
        if(is_rst_connect) {
            is_rst_connect = false;
            goto RECONNECT;
        }
        r.sleep();
        ros::spinOnce();
    }

    close(newsockfd);
    close(sockfd);
}

void
vtree_color_user::compute_features( const std::string& cloud_filename,
                                    FeatureVector &feature_vector )
{
    typedef pcl::PointXYZRGB nx_PointT;
    typedef pcl::Normal nx_Normal;
    typedef pcl::PointCloud<nx_PointT> nx_PointCloud;
    typedef pcl::PointCloud<nx_Normal> nx_PointCloud_normal;
    typedef pcl::PointCloud<int> nx_PointCloud_int;

    typedef pcl::UniformSampling<nx_PointT> nx_Sampler;
    typedef pcl::search::KdTree<nx_PointT> nx_SearchMethod;
    typedef pcl::NormalEstimation<nx_PointT, nx_Normal> nx_NormalEst;

#if FEATURE == 1
    typedef pcl::PFHRGBSignature250 nx_FeatureType;
    typedef pcl::PFHRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 2
    typedef pcl::PPFRGBSignature nx_FeatureType;
    typedef pcl::PPFRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#else
#error A valid feature definition is required!
#endif
    typedef pcl::PointCloud<nx_FeatureType> nx_PointCloud_feature;

    // load the file
    nx_PointCloud::Ptr cld_ptr(new nx_PointCloud);
    pcl::io::loadPCDFile<nx_PointT> ( cloud_filename, *cld_ptr);

    ROS_INFO("[vtree_color_user] Starting keypoint extraction...");
    clock_t tic = clock();
    nx_PointCloud::Ptr keypoints( new nx_PointCloud);
    nx_PointCloud_int::Ptr keypoint_idx(new nx_PointCloud_int);
    nx_Sampler uniform_sampling;
    uniform_sampling.setInputCloud ( cld_ptr );
    uniform_sampling.setRadiusSearch ( keypoint_radius_ );
    uniform_sampling.compute( *keypoint_idx );

    pcl::copyPointCloud ( *cld_ptr, keypoint_idx->points, *keypoints);

    ROS_INFO("[vtree_color_user] No of Keypoints found %d", static_cast<int>(keypoint_idx->size()) );
    ROS_INFO("[vtree_color_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );

    if( keypoints->empty() )
    {
        ROS_WARN("[vtree_color_user] No keypoints were found...");
        return;
    }

    // Compute normals for the input cloud
    ROS_INFO("[vtree_color_user] Starting normal extraction...");
    tic = clock();
    nx_PointCloud_normal::Ptr normals (new nx_PointCloud_normal);
    nx_SearchMethod::Ptr search_method_xyz (new nx_SearchMethod);
    nx_NormalEst norm_est;
    norm_est.setInputCloud ( cld_ptr );
    norm_est.setSearchMethod ( search_method_xyz );
    norm_est.setRadiusSearch ( normal_radius_ );
    norm_est.compute ( *normals );
    ROS_INFO("[vtree_color_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );

    // Get features at the computed keypoints
    ROS_INFO("[vtree_color_user] Starting feature computation...");
    tic = clock();
    nx_PointCloud_feature::Ptr features(new nx_PointCloud_feature);
    nx_FeatureEst feat_est;
    feat_est.setInputCloud ( keypoints );
    feat_est.setSearchSurface ( cld_ptr );
    feat_est.setInputNormals ( normals );

    search_method_xyz.reset(new nx_SearchMethod);
    feat_est.setSearchMethod ( search_method_xyz );
    feat_est.setRadiusSearch ( feature_radius_ );
    feat_est.compute ( *features );
    ROS_INFO("[vtree_color_user] No of Features found %d", static_cast<int>(features->size()) );
    ROS_INFO("[vtree_color_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );

    // Rectify the historgram values to ensure they are in [0,100] and create a document
    for( nx_PointCloud_feature::iterator iter = features->begin();
         iter != features->end(); ++iter)
    {
        rectify_histogram( iter->histogram );
        feature_vector.push_back( FeatureHist( iter->histogram ) );
    }
}

    bool ITRCartesianImpedanceController::init(hardware_interface::PositionCartesianInterface *robot, ros::NodeHandle &n)
    {
        ROS_INFO("THIS CONTROLLER USES THE TCP INFORMATION SET ON THE FRI SIDE... SO BE SURE YOU KNOW WHAT YOU ARE DOING!");

        if (!ros::param::search(n.getNamespace(),"robot_name", robot_namespace_))
        {
            ROS_WARN_STREAM("ITRCartesianImpedanceController: No robot name found on parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
            robot_namespace_ = n.getNamespace();
            //return false;
        }
        if (!n.getParam("robot_name", robot_namespace_))
        {
            ROS_WARN_STREAM("ITRCartesianImpedanceController: Could not read robot name from parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
            robot_namespace_ = n.getNamespace();
            //return false;
        }

        // stuff to read KDL chain in order to get the transform between base_link and robot
        if( !(KinematicChainControllerBase<hardware_interface::PositionCartesianInterface >::init(robot, n)) )
        {
            ROS_ERROR("Couldn't execute init of the KinematikChainController to get the KDL chain.");
            return false;
        }
        KDL::Chain mount_chain;
        //cout << "reading segment 0 name:" << endl;
        kdl_tree_.getChain(kdl_tree_.getRootSegment()->first, this->robot_namespace_ + "_base_link", mount_chain);
        //cout << "KDL segment 0 name: " << mount_chain.getSegment(0).getName() << endl;
        base_link2robot_ = mount_chain.getSegment(0).getFrameToTip();
        //cout << "base_link2robot transform: " << endl << base_link2robot_ << endl;

        joint_names_.push_back( robot_namespace_ + std::string("_0_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_1_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_2_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_3_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_4_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_5_joint") );
        joint_names_.push_back( robot_namespace_ + std::string("_6_joint") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xx") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yx") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zx") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_pos_x") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xy") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yy") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zy") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_pos_y") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xz") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yz") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zz") );
        cart_12_names_.push_back( robot_namespace_ + std::string("_pos_z") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_X") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_Y") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_Z") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_A") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_B") );
        cart_6_names_.push_back( robot_namespace_ + std::string("_C") );

        // now get all handles, 12 for cart pos, 6 for stiff, 6 for damp, 6 for wrench, 6 for joints
        for(int c = 0; c < 30; ++c)
        {
            if(c < 12)
                cart_handles_.push_back(robot->getHandle(cart_12_names_.at(c)));
            if(c > 11 && c < 18)
                cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-12) + std::string("_stiffness")));
            if(c > 17 && c < 24)
                cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-18) + std::string("_damping")));
            if(c > 23 && c < 30)
                cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-24) + std::string("_wrench")));
        }
        for(int j = 0; j < 6; ++j)
        {
            joint_handles_.push_back(robot->getHandle(joint_names_.at(j)));
        }

        sub_pose_ = n.subscribe(n.resolveName("pose"), 1, &ITRCartesianImpedanceController::pose, this);
        sub_pose_world_ = n.subscribe(n.resolveName("pose_base_link"), 1, &ITRCartesianImpedanceController::pose_base_link, this);

        sub_gains_ = n.subscribe(n.resolveName("gains"), 1, &ITRCartesianImpedanceController::gains, this);
        sub_addFT_ = n.subscribe(n.resolveName("wrench"), 1, &ITRCartesianImpedanceController::additionalFT, this);

        srv_command_ = n.advertiseService("set_command", &ITRCartesianImpedanceController::command_cb, this);
        //sub_ft_measures_ = n.subscribe(n.resolveName("ft_measures"), 1, &ITRCartesianImpedanceController::updateFT, this);
        //pub_goal_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("goal"),0);

        pub_msr_pos_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("measured_cartesian_pose"),0);

        // Initial position
        KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
                            cart_handles_.at(1).getPosition(),
                            cart_handles_.at(2).getPosition(),
                            cart_handles_.at(4).getPosition(),
                            cart_handles_.at(5).getPosition(),
                            cart_handles_.at(6).getPosition(),
                            cart_handles_.at(8).getPosition(),
                            cart_handles_.at(9).getPosition(),
                            cart_handles_.at(10).getPosition());
        KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
                            cart_handles_.at(7).getPosition(),
                            cart_handles_.at(11).getPosition());
        KDL::Frame cur_T( cur_R, cur_p );
        x_ref_ = cur_T;
//.........这里部分代码省略.........

    bool SiftNode::detect(posedetection_msgs::Feature0D& features, const sensor_msgs::Image& imagemsg,
                          const sensor_msgs::Image::ConstPtr& mask_ptr)
    {
        boost::mutex::scoped_lock lock(_mutex);
        Image imagesift = NULL;
        cv::Rect region;
        try {
            cv::Mat image;
            cv_bridge::CvImagePtr framefloat;

            if (!(framefloat = cv_bridge::toCvCopy(imagemsg, "mono8")) )
                return false;
            
            if(imagesift != NULL && (imagesift->cols!=imagemsg.width || imagesift->rows!=imagemsg.height)) {
                ROS_INFO("clear sift resources");
                DestroyAllImages();
                imagesift = NULL;
            }
            
            image = framefloat->image;

            if (mask_ptr) {
                cv::Mat mask = cv_bridge::toCvShare(mask_ptr, mask_ptr->encoding)->image;
                region = jsk_perception::boundingRectOfMaskImage(mask);
                image = image(region);
            }
            else {
                region = cv::Rect(0, 0, imagemsg.width, imagemsg.height);
            }
            
            if(imagesift == NULL)
                imagesift = CreateImage(imagemsg.height,imagemsg.width);

            for(int i = 0; i < imagemsg.height; ++i) {
                uint8_t* psrc = (uint8_t*)image.data+image.step*i;
                float* pdst = imagesift->pixels+i*imagesift->stride;
                for(int j = 0; j < imagemsg.width; ++j)
                    pdst[j] = (float)psrc[j]*(1.0f/255.0f);
                //memcpy(imagesift->pixels+i*imagesift->stride,framefloat->imageData+framefloat->widthStep*i,imagemsg.width*sizeof(float));
            }
        }
        catch (cv_bridge::Exception error) {
            ROS_WARN("bad frame");
            return false;
        }

        // compute SIFT
        ros::Time siftbasetime = ros::Time::now();
        Keypoint keypts = GetKeypoints(imagesift);
        // write the keys to the output
        int numkeys = 0;
        Keypoint key = keypts;
        while(key) {
            numkeys++;
            key = key->next;
        }

        // publish
        features.header = imagemsg.header;
        features.positions.resize(numkeys*2);
        features.scales.resize(numkeys);
        features.orientations.resize(numkeys);
        features.confidences.resize(numkeys);
        features.descriptors.resize(numkeys*128);
        features.descriptor_dim = 128;
        features.type = "libsiftfast";

        int index = 0;
        key = keypts;
        while(key) {

            for(int j = 0; j < 128; ++j)
                features.descriptors[128*index+j] = key->descrip[j];

            features.positions[2*index+0] = key->col + region.x;
            features.positions[2*index+1] = key->row + region.y;
            features.scales[index] = key->scale;
            features.orientations[index] = key->ori;
            features.confidences[index] = 1.0; // SIFT has no confidence?

            key = key->next;
            ++index;
        }

        FreeKeypoints(keypts);
        DestroyAllImages();

        ROS_INFO("imagesift: image: %d(size=%lu), num: %d, sift time: %.3fs, total: %.3fs", imagemsg.header.seq,
                 imagemsg.data.size(),  numkeys,
                 (float)(ros::Time::now()-siftbasetime).toSec(), (float)(ros::Time::now()-lasttime).toSec());
        lasttime = ros::Time::now();
        return true;
    }

int main(int argc, char** argv) 
{
    char   		*pszGuid = NULL;
    char    	 szGuid[512];
    int			 nInterfaces = 0;
    int			 nDevices = 0;
    int 		 i = 0;
	const char	*pkeyAcquisitionFrameRate[2] = {"AcquisitionFrameRate", "AcquisitionFrameRateAbs"};
    ArvGcNode	*pGcNode;
	GError		*error=NULL;

    global.bCancel = FALSE;
    global.config = global.config.__getDefault__();
    global.idSoftwareTriggerTimer = 0;

    ros::init(argc, argv, "camera");
    global.phNode = new ros::NodeHandle();


    // Service callback for firing nuc's. 
	// Needed since we cannot open another connection to cameras while streaming.
	ros::NodeHandle nh;
	ros::ServiceServer NUCservice = nh.advertiseService("FireNUC", NUCService_callback);

    //g_type_init ();

    // Print out some useful info.
    ROS_INFO ("Attached cameras:");
    arv_update_device_list();
    nInterfaces = arv_get_n_interfaces();
    ROS_INFO ("# Interfaces: %d", nInterfaces);

    nDevices = arv_get_n_devices();
    ROS_INFO ("# Devices: %d", nDevices);
    for (i=0; i<nDevices; i++)
    	ROS_INFO ("Device%d: %s", i, arv_get_device_id(i));
    
    if (nDevices>0)
    {
		// Get the camera guid from either the command-line or as a parameter.
    	if (argc==2)
    	{
    		strcpy(szGuid, argv[1]);
    		pszGuid = szGuid;
    	}
    	else
    	{
    		if (global.phNode->hasParam(ros::this_node::getName()+"/guid"))
    		{
    			std::string		stGuid;
    			
    			global.phNode->getParam(ros::this_node::getName()+"/guid", stGuid);
    			strcpy (szGuid, stGuid.c_str());
        		pszGuid = szGuid;
    		}
    		else
    			pszGuid = NULL;
    	}
    	
    	// Open the camera, and set it up.
    	ROS_INFO("Opening: %s", pszGuid ? pszGuid : "(any)");
		while (TRUE)
		{
			global.pCamera = arv_camera_new(pszGuid);
			if (global.pCamera)
				break;
			else
			{
				ROS_WARN ("Could not open camera %s.  Retrying...", pszGuid);
				ros::Duration(1.0).sleep();
			    ros::spinOnce();
			}
		}

		global.pDevice = arv_camera_get_device(global.pCamera);
		ROS_INFO("Opened: %s-%s", arv_device_get_string_feature_value (global.pDevice, "DeviceVendorName"), arv_device_get_string_feature_value (global.pDevice, "DeviceID"));


		// See if some basic camera features exist.
		pGcNode = arv_device_get_feature (global.pDevice, "AcquisitionMode");
		global.isImplementedAcquisitionMode = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "GainRaw");
		global.isImplementedGain = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
		pGcNode = arv_device_get_feature (global.pDevice, "Gain");
		global.isImplementedGain |= ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "ExposureTimeAbs");
		global.isImplementedExposureTimeAbs = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "ExposureAuto");
		global.isImplementedExposureAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "GainAuto");
		global.isImplementedGainAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "TriggerSelector");
		global.isImplementedTriggerSelector = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;

		pGcNode = arv_device_get_feature (global.pDevice, "TriggerSource");
//.........这里部分代码省略.........

CSrf10Controller::CSrf10Controller(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) :
    CController(name,device_p,nh)
{
    std::string topic;
    nh.param("controllers/"+name+"/topic", topic, std::string("srf10_state"));
    nh.param("controllers/"+name+"/rate", rate_, 15.0);
    if(nh.hasParam("controllers/"+name+"/sensors/front"))
    {
      std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
      std::map< std::string, XmlRpc::XmlRpcValue > value;
      XmlRpc::MyXmlRpcValue sensors;
      nh.getParam("controllers/"+name+"/sensors/front", sensors);
      ROS_ASSERT(sensors.getType() == XmlRpc::XmlRpcValue::TypeStruct);
      value=sensors;
      for(it=value.begin();it!=value.end();it++)
      {
        ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
        if(!nh.hasParam("controllers/"+name+"/sensors/front/"+(*it).first+"/address"))
        {
            ROS_WARN_STREAM("You need to set the address atribute for the sensor " << (*it).first);
            continue;
        }
        if(!nh.hasParam("controllers/"+name+"/sensors/front/"+(*it).first+"/type"))
        {
            ROS_WARN_STREAM("You need to set the type of the sensor " << (*it).first);
            continue;
        }
        int address;
        std::string type;
        std::string frame_id;
        std::string sensor_topic;
        double max_alert_distance;
        double min_alert_distance;
        nh.getParam("controllers/"+name+"/sensors/front/"+(*it).first+"/address", address);
        nh.getParam("controllers/"+name+"/sensors/front/"+(*it).first+"/type", type);
        nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/frame_id", frame_id, std::string(""));
        nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/topic", sensor_topic, topic+"/"+(*it).first);
        nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/min_alert_distance", min_alert_distance, -1.0);
        nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/max_alert_distance", max_alert_distance, -1.0);
        //printf("%f\n",alert_distance);
        if (type.compare("srf10")==0)
        {
            srf10Sensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
            srf10SensorsUpdateGroup_[(uint8_t)address]=1;
        }
        else if (type.compare("gp2d12")==0 || type.compare("gp2d120")==0 || type.compare("GP2Y0A21YK")==0)
        {
            adcSensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
            adcSensorsAddresses_.push_back((uint8_t)address);
        }
        ROS_INFO_STREAM("Sensor " << (*it).first << " of type " << type << " inicializado");
      }
    }
    if(nh.hasParam("controllers/"+name+"/sensors/back"))
    {
      std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
      std::map< std::string, XmlRpc::XmlRpcValue > value;
      XmlRpc::MyXmlRpcValue sensors;
      nh.getParam("controllers/"+name+"/sensors/back", sensors);
      ROS_ASSERT(sensors.getType() == XmlRpc::XmlRpcValue::TypeStruct);
      value=sensors;
      for(it=value.begin();it!=value.end();it++)
      {
        ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
        if(!nh.hasParam("controllers/"+name+"/sensors/back/"+(*it).first+"/address"))
        {
            ROS_WARN_STREAM("You need to set the address atribute for the sensor " << (*it).first);
            continue;
        }
        if(!nh.hasParam("controllers/"+name+"/sensors/back/"+(*it).first+"/type"))
        {
            ROS_WARN_STREAM("You need to set the type of the sensor " << (*it).first);
            continue;
        }
        int address;
        std::string type;
        std::string frame_id;
        std::string sensor_topic;
        double min_alert_distance;
        double max_alert_distance;
        nh.getParam("controllers/"+name+"/sensors/back/"+(*it).first+"/address", address);
        nh.getParam("controllers/"+name+"/sensors/back/"+(*it).first+"/type", type);
        nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/frame_id", frame_id, std::string(""));
        nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/topic", sensor_topic, topic+"/"+(*it).first);
        nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/min_alert_distance", min_alert_distance, -1.0);
        nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/max_alert_distance", max_alert_distance, -1.0);
        //printf("%f\n",alert_distance);
        if (type.compare("srf10")==0)
        {
            srf10Sensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
            srf10SensorsUpdateGroup_[(uint8_t)address]=1;
        }
        else if (type.compare("gp2d12")==0 || type.compare("gp2d120")==0  || type.compare("GP2Y0A21YK")==0)
        {
            adcSensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
            adcSensorsAddresses_.push_back((uint8_t)address);
        }
        ROS_INFO_STREAM("Sensor " << (*it).first << " of type " << type << " inicializado");
      }
    }
//.........这里部分代码省略.........

void OrientGoal::runBehavior(){
  if(!initialized_){
    ROS_ERROR("This object must be initialized before runBehavior is called");
    return;
  }

  if(global_costmap_ == NULL || local_costmap_ == NULL){
    ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
    return;
  }

  ros::Rate r(frequency_);
  ros::NodeHandle n;
  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);

  tf::Stamped<tf::Pose> global_pose;
  global_costmap_->getRobotPose(global_pose);
  double yaw_req = angles::normalize_angle(tf::getYaw(*goal_orientation_));
  double yaw_now = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
  ROS_INFO("The required orientation is %f but the orientation of the robot is %f\n",yaw_req,yaw_now);
  double tol = fabs(yaw_req - yaw_now);
  ROS_INFO("Tolerance with in limits. Allowed tolerance is 0.1 and the difference is %f\n",tol);
  if (tol <= 0.1)
  {
	  ROS_INFO("No Goal Correction is required\n");
	  return;
  }
  ROS_INFO("Goal Correction is required\n");

  double current_angle = -1.0 * M_PI;

  while(n.ok()){
    global_costmap_->getRobotPose(global_pose);

    current_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
    double dist_left = angles::shortest_angular_distance(current_angle,yaw_req);
    ROS_INFO("Current angle = %f Required Angle = %f", current_angle,yaw_req);
    ROS_INFO("Shortest angle = %f",dist_left);

    //update the costmap copy that the world model holds
   // local_costmap_->getCostmapCopy(costmap_);
    global_costmap_->getCostmapCopy(costmap_);

    //check if that velocity is legal by forward simulating
    double sim_angle = 0.0;
    while(sim_angle < dist_left){
      std::vector<geometry_msgs::Point> oriented_footprint;
      double theta = tf::getYaw(global_pose.getRotation())+sim_angle;
      geometry_msgs::Point position;
      position.x = global_pose.getOrigin().x();
      position.y = global_pose.getOrigin().y();

      global_costmap_->getOrientedFootprint(position.x, position.y, theta, oriented_footprint);
     double footprint_cost = 1.0;
      //double footprint_cost = world_model_->footprintCost(position, oriented_footprint, local_costmap_->getInscribedRadius(), local_costmap_->getCircumscribedRadius());
      if(footprint_cost < 0.0){
        ROS_WARN("Rotation towards goal cannot take place because there is a potential collision. Cost: %.2f", footprint_cost);
        return;
      }

      sim_angle += sim_granularity_;
    }

    if (fabs(dist_left) < 0.1)
    	return;
    

    double vel = sqrt(2 * acc_lim_th_ * fabs(dist_left));

    (dist_left <=0.0)?(vel= -vel):(vel = vel);
    vel = std::min(std::max(vel, -0.5),0.5);
    ROS_INFO("The required orientation is %f but the orientation of the robot is %f\n",yaw_req,current_angle);
    ROS_INFO("Velocity:: %f",vel);

    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = vel;

    vel_pub.publish(cmd_vel);

    r.sleep();
  }
}

//.........这里部分代码省略.........
    double timestep = 1; // TODO: time
    
    
    if (goal_orientation)
    {
      goal_orient = *goal_orientation;
    }
    else
    {
      goal_orient = start_orient;
    }
    
    if (!isInit())
    {	
      addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization		

      // we insert middle points now (increase start by 1 and decrease goal by 1)
      std::advance(path_start,1);
      std::advance(path_end,-1);
      unsigned int idx=0;
      for (; path_start != path_end; ++path_start) // insert middle-points
      {
	//Eigen::Vector2d point_to_goal = path.back()-*it;
	//double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
	// Alternative: Direction from last path
	Eigen::Vector2d curr_point = fun_position(*path_start);
	Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use boost::prior(*path_start) for those cases,
								       // where fun_position() does not return a reference or is expensive.
	double diff_norm = diff_last.norm();
	
	double timestep_vel = diff_norm/max_vel_x; // constant velocity
	double timestep_acc;
	if (max_acc_x)
	{
		timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
		if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
		else timestep = timestep_vel;
	}
	else timestep = timestep_vel;
	
	if (timestep<0) timestep=0.2; // TODO: this is an assumption
	
	addPoseAndTimeDiff(curr_point, atan2(diff_last[1],diff_last[0]) ,timestep);
	
	Eigen::Vector2d diff_next = fun_position(*boost::next(path_start))-curr_point; // TODO maybe store the boost::next for the following iteration
	double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
						         -atan2(diff_last[1],diff_last[0]) ) );
	
	timestep_vel = ang_diff/max_vel_theta; // constant velocity
	if (max_acc_theta)
	{
		timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
		if (timestep_vel < timestep_acc) timestep = timestep_acc;
		else timestep = timestep_vel;
	}
	else timestep = timestep_vel;
	
	if (timestep<0) timestep=0.2; // TODO: this is an assumption
	
	addPoseAndTimeDiff(curr_point, atan2(diff_last[1],diff_last[0]) ,timestep);
	
	++idx;
      }
      Eigen::Vector2d diff = goal_position-Pose(idx).position();
      double diff_norm = diff.norm();
      double timestep_vel = diff_norm/max_vel_x; // constant velocity
      if (max_acc_x)
      {
	double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
	if (timestep_vel < timestep_acc) timestep = timestep_acc;
	else timestep = timestep_vel;
      }
      else timestep = timestep_vel;

      
      PoseSE2 goal(goal_position, goal_orient);
      
      // if number of samples is not larger than min_samples, insert manually
      if ( (int)sizePoses() < min_samples-1 )
      {
        ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
        while ((int)sizePoses() < min_samples-1) // subtract goal point that will be added later
        {
          // simple strategy: interpolate between the current pose and the goal
          addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization	
        }
      }
      
      // now add goal
      addPoseAndTimeDiff(goal, timestep); // add goal point
      setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
    }
    else // size!=0
    {
      ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
      ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
      return false;
    }
    return true;
}  

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "scanner2d");
  scanner2d::Scanner2d scanner;
  std::string port_name;
  int scan_rate;
  int sample_rejection;
  int samples_per_scan;
  int min_angle;
  int max_angle;

  ROS_DEBUG("Welcome to scanner2d Node!");

  signal(SIGINT, sig_handler);
  
  ros::NodeHandle private_node_handle_("~");
  private_node_handle_.param("port_name", port_name, std::string("/dev/ttyACM0"));
  private_node_handle_.param("scan_rate", scan_rate, int(3));
  private_node_handle_.param("samples_per_scan", samples_per_scan, int(333));
  private_node_handle_.param("sample_rejection", sample_rejection, int(0));
  private_node_handle_.param("min_angle", min_angle, int(0));
  private_node_handle_.param("max_angle", max_angle, int(360));

  if (scan_rate*samples_per_scan > 1000) {
    ROS_WARN("The scan_rate * samples_per_scan exceeds the max sample rate (1000) of the sensor!");
    ROS_WARN("  you should either alter the settings from the command line invocation,");
    ROS_WARN("  or reconfigure the parameter server directly.");
  }

  scanner.open(port_name);

  switch(scan_rate)
  {
  case 1:
    scanner.setScanPeriod(1000);
    break;
  case 2:
    scanner.setScanPeriod(500);
    break;
  case 3:
    scanner.setScanPeriod(333);
    break;
  case 4:
    scanner.setScanPeriod(250);
    break;
  case 5:
    scanner.setScanPeriod(200);
    break;
  default:
    ROS_WARN("Invalid scan_rate!");
  }
  
  scanner.setSampleRejectionMode((bool)sample_rejection);
  scanner.setSamplesPerScan(samples_per_scan);
  scanner.setMinMaxAngle(min_angle,  max_angle);

  ros::Rate loop_rate(10);

  while (ros::ok()) 
  {
    scanner2d::Scanner2dStatus_t status;
    scanner.getStatus(&status);
    if (status.flags & 0x80) 
    {
      ROS_DEBUG("Got quit from scanner");
      ROS_DEBUG("  Status flags: 0x%x", status.flags);
      break;
    }

    if (got_ctrl_c) {
      ROS_WARN("Got Ctrl-C");
      scanner.stop();
      ros::Duration(0.5).sleep();
      scanner.close();
      break;
    }

    ros::spinOnce();
    loop_rate.sleep();
  }

  ROS_WARN("Exiting.");

  exit(0);
}

//.........这里部分代码省略.........
    bool joint_state_sets = false;
    //see if we need to update any transforms
    std::string parent_frame_id = (*it)->getParentFrameId();
    std::string child_frame_id = (*it)->getChildFrameId();
    if(!parent_frame_id.empty() && !child_frame_id.empty()) {
      std::string err;
      ros::Time tm;
      tf::StampedTransform transf;
      bool ok = false;
      if (tf_->getLatestCommonTime(parent_frame_id, child_frame_id, tm, &err) == tf::NO_ERROR) {
        ok = true;
        try
        {
          tf_->lookupTransform(parent_frame_id, child_frame_id, tm, transf);
        }
        catch(tf::TransformException& ex)
        {
          ROS_ERROR("Unable to lookup transform from %s to %s.  Exception: %s", parent_frame_id.c_str(), child_frame_id.c_str(), ex.what());
          ok = false;
        }
      } else {
        ROS_DEBUG("Unable to lookup transform from %s to %s: no common time.", parent_frame_id.c_str(), child_frame_id.c_str());
        ok = false;
      }
      if(ok) {
        tfSets = (*it)->setJointStateValues(transf);
        if(tfSets) {
          const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
          for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
              it != joint_state_names.end();
              it++) {
            last_joint_update_[*it] = tm;
          }
        }
        // tf::Transform transf = getKinematicModel()->getRoot()->variable_transform;
        // ROS_INFO_STREAM("transform is to " << transf.getRotation().x() << " " 
        //                 << transf.getRotation().y() << " z " << transf.getRotation().z()
        //                 << " w " << transf.getRotation().w());
        have_pose_ = tfSets;
        last_pose_update_ = tm;
      }
    }
    //now we update from joint state
    joint_state_sets = (*it)->setJointStateValues(joint_state_map);
    if(joint_state_sets) {
      const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
      for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
          it != joint_state_names.end();
          it++) {
        last_joint_update_[*it] = joint_state->header.stamp;
      }
    }
  }
  
  state.updateKinematicLinks();
  state.getKinematicStateValues(current_joint_state_map_);

  if(allJointsUpdated()) {
    have_joint_state_ = true;
    last_joint_state_update_ = joint_state->header.stamp;
    
    if(!joint_state_map_cache_.empty()) {
      if(joint_state->header.stamp-joint_state_map_cache_.back().first > ros::Duration(joint_state_cache_allowed_difference_)) {
        ROS_DEBUG_STREAM("Introducing joint state cache sparsity time of " << (joint_state->header.stamp-joint_state_map_cache_.back().first).toSec());
      }
    }

    joint_state_map_cache_.push_back(std::pair<ros::Time, std::map<std::string, double> >(joint_state->header.stamp,
                                                                                          current_joint_state_map_));
  } 

  if(have_joint_state_) {
    while(1) {
      if(joint_state_map_cache_.empty()) {
        ROS_WARN("Empty joint state map cache");
        break;
      }
      if((ros::Time::now()-joint_state_map_cache_.front().first) > ros::Duration(joint_state_cache_time_)) {
        joint_state_map_cache_.pop_front();
      } else {
        break;
      }
    }
  }
    
  first_time = false;

  if(!allJointsUpdated(ros::Duration(1.0))) {
    if(!printed_out_of_date_) {
      ROS_WARN_STREAM("Got joint state update but did not update some joints for more than 1 second.  Turn on DEBUG for more info");
      printed_out_of_date_ = true;
    }
  } else {
    printed_out_of_date_ = false;
  }
  if(on_state_update_callback_ != NULL) {
    on_state_update_callback_(joint_state);
  }
  current_joint_values_lock_.unlock();
}

    //  Callback to register with tf::MessageFilter to be called when transforms are available
    void msgCallback(const boost::shared_ptr<const movingobstaclesrhc::Coil>& coil_ptr)
    {
        tf::StampedTransform transformb, transform_auxiliar;
        geometry_msgs::PointStamped point_out, point_coil;
        double tfx, tfy;
	std::string t_frame;
	if ((coil_ptr->header.frame_id=="metal_detector/right_coil")||(coil_ptr->header.frame_id=="right_coil"))
		t_frame = "right_coil";
	else if((coil_ptr->header.frame_id=="metal_detector/left_coil")||(coil_ptr->header.frame_id=="left_coil"))
		t_frame = "left_coil";
	else
		t_frame = "middle_coil";

        try{
        	tf_.lookupTransform(target_frame_,t_frame.c_str(), ros::Time(0), transformb);
        	point_out.point.x = transformb.getOrigin().x();
		      point_out.point.y = transformb.getOrigin().y();
		      point_out.point.z = transformb.getOrigin().z();
        }catch (tf::TransformException ex){
        	ROS_ERROR("%s",ex.what());
        }
        try{
          tf_.lookupTransform(target_frame_,"/base_link", ros::Time(0), transform_auxiliar);
        }catch (tf::TransformException ex){
        	ROS_ERROR("%s",ex.what());
        }
        try{
          tf_.lookupTransform("/base_link",t_frame.c_str(), ros::Time(0), transformb);
        }catch (tf::TransformException ex){