int main(int argc, char **argv) {
    ros::init (argc, argv, "move_arm_pose_goal_test");
    ros::NodeHandle nh;
    actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true);
    move_arm.waitForServer();
    ROS_INFO("Connected to server");
    arm_navigation_msgs::MoveArmGoal goalA;

    double	x = 0.5,
            y = 0.1,
            z = 0.5,
            R = -M_PI_2, // greifer 'gerade' drehen
            P = M_PI_2, // M_PI_2 negativ: greifer zeigt nach oben. und umgekehrt
            Y = 0;

    // 0 0 0.6   -M_PI_2  M_PI_4 0
    // 0.3 0 0.4   -M_PI_2  M_PI_2 0
    // 0.5 0 0.3   -M_PI_2  M_PI_2 0
    // 0.4 0.1 0.4   -M_PI_2  M_PI_2 -M_PI_2

    if(argc >= 1+6) { // cmd name + args
        x = parseArg(argv[1]);
        y = parseArg(argv[2]);
        z = parseArg(argv[3]);
        R = parseArg(argv[4]);
        P = parseArg(argv[5]);
        Y = parseArg(argv[6]);
    }

    ROS_INFO_STREAM("x="<<x<<" y="<<y<<" z="<<z<<" R="<<R<<" P="<<P<<" Y="<<Y);

    goalA.motion_plan_request.group_name = "SchunkArm";
    goalA.motion_plan_request.num_planning_attempts = 2;
    goalA.motion_plan_request.planner_id = std::string("");
    goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
    goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

    arm_navigation_msgs::SimplePoseConstraint desired_pose;
    desired_pose.header.frame_id = "arm_base_link";
    desired_pose.link_name = "gripper";
    desired_pose.pose.position.x = x;
    desired_pose.pose.position.y = y;
    desired_pose.pose.position.z = z;

    double yaw = tan(desired_pose.pose.position.y / desired_pose.pose.position.x);
    ROS_INFO_STREAM("yaw="<<yaw);

    if(argc >= 1+7 && !strcmp(argv[7], "calcyaw")) {
        ROS_INFO("Using calculated yaw");
        Y = yaw;
    }

    desired_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(R, P, Y);

    ROS_INFO_STREAM(desired_pose.pose.orientation);


    desired_pose.absolute_position_tolerance.x = 0.02;
    desired_pose.absolute_position_tolerance.y = 0.02;
    desired_pose.absolute_position_tolerance.z = 0.02;

    desired_pose.absolute_roll_tolerance = 0.04;
    desired_pose.absolute_pitch_tolerance = 0.04;
    desired_pose.absolute_yaw_tolerance = 0.04;

    arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);

    if (nh.ok())
    {
        bool finished_within_time = false;
        move_arm.sendGoal(goalA);
        finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
        if (!finished_within_time)
        {
            move_arm.cancelGoal();
            ROS_INFO("Timed out achieving goal A");
        }
        else
        {
            actionlib::SimpleClientGoalState state = move_arm.getState();
            bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
            if(success)
                ROS_INFO("Action finished: %s",state.toString().c_str());
            else
                ROS_INFO("Action failed: %s",state.toString().c_str());
        }
    }
    ros::shutdown();
}

  void callback(const sensor_msgs::ImageConstPtr &img,
                const sensor_msgs::CameraInfoConstPtr &info) {
    boost::mutex::scoped_lock lock(mutex_);
    ros::Time now = ros::Time::now();

    static boost::circular_buffer<double> in_times(100);
    static boost::circular_buffer<double> out_times(100);
    static boost::circular_buffer<double> in_bytes(100);
    static boost::circular_buffer<double> out_bytes(100);

    ROS_DEBUG("resize: callback");
    if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
      ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
      return;
    }

    in_times.push_front((now - last_subscribe_time_).toSec());
    in_bytes.push_front(img->data.size());
    //
    try {
        int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
        int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
        double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
        double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;

        cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);

        cv::Mat tmpmat(height, width, cv_img->image.type());
        cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
        cv_img->image = tmpmat;

        sensor_msgs::CameraInfo tinfo = *info;
        tinfo.height = height;
        tinfo.width = width;
        tinfo.K[0] = tinfo.K[0] * scale_x; // fx
        tinfo.K[2] = tinfo.K[2] * scale_x; // cx
        tinfo.K[4] = tinfo.K[4] * scale_y; // fy
        tinfo.K[5] = tinfo.K[5] * scale_y; // cy

        tinfo.P[0] = tinfo.P[0] * scale_x; // fx
        tinfo.P[2] = tinfo.P[2] * scale_x; // cx
        tinfo.P[3] = tinfo.P[3] * scale_x; // T
        tinfo.P[5] = tinfo.P[5] * scale_y; // fy
        tinfo.P[6] = tinfo.P[6] * scale_y; // cy

        if ( !use_messages_ || now - last_publish_time_  > period_ ) {
            cp_.publish(cv_img->toImageMsg(),
                        boost::make_shared<sensor_msgs::CameraInfo> (tinfo));

            out_times.push_front((now - last_publish_time_).toSec());
            out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());

            last_publish_time_ = now;
        }
    } catch( cv::Exception& e ) {
        ROS_ERROR("%s", e.what());
    }


    float duration =  (now - last_rosinfo_time_).toSec();
    if ( duration > 2 ) {
        int in_time_n = in_times.size();
        int out_time_n = out_times.size();
        double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
        double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;

        std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
        in_time_mean /= in_time_n;
        in_time_rate /= in_time_mean;
        std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
        in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
        if ( in_time_n > 1 ) {
            in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
            in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
        }

        std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
        out_time_mean /= out_time_n;
        out_time_rate /= out_time_mean;
        std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
        out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
        if ( out_time_n > 1 ) {
            out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
            out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
        }

        double in_byte_mean = 0, out_byte_mean = 0;
        std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
        std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
        in_byte_mean /= duration;
        out_byte_mean /= duration;

        ROS_INFO_STREAM(" in  bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << in_byte_mean/1000*8
                        << " Kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
                        << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
                        << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
        ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << out_byte_mean/1000*8
                        << " kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
//.........这里部分代码省略.........

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

      // Calculate the quad_control_references
      if(wjse.stick_x!=0)
      {

         arm_control_references.arm_control_references.position_ref[2] = arm_state_estimation.arm_state_estimation.position[2] + wjse.stick_x/6000.0;
      }

      if(wjse.stick2_x!=0)
      {
      arm_control_references.arm_control_references.position_ref[3] = arm_state_estimation.arm_state_estimation.position[3] + wjse.stick2_x/6000.0;
      }

      if(wjse.stick_y!=0)
      {
      arm_control_references.arm_control_references.position_ref[1] = arm_state_estimation.arm_state_estimation.position[1] + wjse.stick_y/6000.0;
      }


      if( wjse.stick2_y!=0)
      {
         arm_control_references.arm_control_references.position_ref[0] = arm_state_estimation.arm_state_estimation.position[0] + wjse.stick2_y/6000.0;
      }


      if(wjse.stick3_x!=0)
      {
      arm_control_references.arm_control_references.position_ref[4] = arm_state_estimation.arm_state_estimation.position[4] + wjse.stick3_x/12000.0;
      }

      if(wjse.stick3_y!=0)
      {
      arm_control_references.arm_control_references.position_ref[5] = arm_state_estimation.arm_state_estimation.position[5] + wjse.stick3_y/12000.0;
      }


/*
      if(joystick.get_joystick_status(&wjse)!=-1)
      {
         command_to_send.linear.x = wjse.stick_x/2500.0;
         //std::cout << "Pitch control: " << command_to_send.linear.x << std::endl;

         command_to_send.linear.y = wjse.stick2_x/2500.0;
         //std::cout << "Roll control: " << command_to_send.linear.y << std::endl;

         command_to_send.linear.z = wjse.stick_y/4000.0;
         //std::cout << "Thrust control: " << command_to_send.linear.z << std::endl;

         command_to_send.angular.z = wjse.stick2_y/4000.0;
         //std::cout << "Yaw control: " <<  command_to_send.angular.z << std::endl;

      }
      else
      {
         ROS_ERROR_STREAM("Problems with joystick!");
      }
*/

      // Implement take-off
      if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[0] == 1))
      {
         ROS_INFO_STREAM("Extracting Sent");
         aalExtensionActionWrapper.extendArm();
         usleep(100000); // In order to not send it twice accidentally
      }

      // Implement landing
      if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[1] == 1))
      {
         ROS_INFO_STREAM("Contracting Sent");
         aalExtensionActionWrapper.contractArm();
         usleep(100000); // In order to not send it twice accidentally
      }

      if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[2] == 1))
      {

         arm_control_references.arm_control_references.position_ref[6] = 3.14159/5;

         usleep(100000); // In order to not send it twice accidentally
      }
      if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[3] == 1))
      {

         arm_control_references.arm_control_references.position_ref[6] =0.0;

         usleep(100000); // In order to not send it twice accidentally
      }


    //joystick_publisher.publish(command_to_send);
    arm_control_ref_pub.publish(arm_control_references);

    //~20Hz
    usleep(50000);
   }

   return 0;
}

 /*
  * Get the list of controller names.
  */
 virtual void getControllersList(std::vector<std::string> &names) {
   for (std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
     names.push_back(it->first);
   ROS_INFO_STREAM("Returned " << names.size() << " controllers in list");
 }

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

  ros::init( argc, argv, "example2" );

  ros::NodeHandle n;

  const double val = 3.14;

  // Basic messages:
  ROS_INFO( "My INFO message." );
  ROS_INFO( "My INFO message with argument: %f", val );
  ROS_INFO_STREAM(
    "My INFO stream message with argument: " << val
  );

  // Named messages:
  ROS_INFO_STREAM_NAMED(
    "named_msg",
    "My named INFO stream message; val = " << val
  );

  // Conditional messages:
  ROS_INFO_STREAM_COND(
    val < 0.,
    "My conditional INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_COND(
    val >= 0.,
    "My conditional INFO stream message; val (" << val << ") >= 0"
  );

  // Conditional Named messages:
  ROS_INFO_STREAM_COND_NAMED(
    val < 0., "cond_named_msg",
    "My conditional INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_COND_NAMED(
    val >= 0., "cond_named_msg",
    "My conditional INFO stream message; val (" << val << ") >= 0"
  );

  // Filtered messages:
  struct MyLowerFilter : public ros::console::FilterBase {
    MyLowerFilter( const double& val ) : value( val ) {}

    inline virtual bool isEnabled()
    {
      return value < 0.;
    }

    double value;
  };

  struct MyGreaterEqualFilter : public ros::console::FilterBase {
    MyGreaterEqualFilter( const double& val ) : value( val ) {}

    inline virtual bool isEnabled()
    {
      return value >= 0.;
    }
  
    double value;
  };

  MyLowerFilter filter_lower( val );
  MyGreaterEqualFilter filter_greater_equal( val );

  ROS_INFO_STREAM_FILTER(
    &filter_lower,
    "My filter INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_FILTER(
    &filter_greater_equal,
    "My filter INFO stream message; val (" << val << ") >= 0"
  );

  // Once messages:
  for( int i = 0; i < 10; ++i ) {
    ROS_INFO_STREAM_ONCE(
      "My once INFO stream message; i = " << i
    );
  }

  // Throttle messages:
  for( int i = 0; i < 10; ++i ) {
    ROS_INFO_STREAM_THROTTLE(
      2,
      "My throttle INFO stream message; i = " << i
    );
    ros::Duration( 1 ).sleep();
  }

  ros::spinOnce();

  return EXIT_SUCCESS;

}

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

    //ros::Subscriber imu_data = nh.subscribe();
    ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("imudata",1);
    //串口读取数据
    //    serial::Serial serport;

    //    try
    //    {
    //        //打开串口0
    //        serport.setPort("/dev/ttyUSB0");
    //        serport.setBaudrate(115200);
    //        serial::Timeout to = serial::Timeout::simpleTimeout(0);

    //        serport.setTimeout(to);
    //        serport.open();

    //    }
    //    catch(serial::IOException & e)
    //    {
    //        ROS_ERROR_STREAM("Unable to open serial port");
    //        return -1;
    //    }

    //    //判断串口是否打开
    //    if(serport.isOpen())
    //    {
    //        ROS_INFO_STREAM("Serial Port initialized");
    //    }
    //    else
    //    {
    //        return -1;
    //    }

    //    serport.flushInput();


    //uint8 dataPacket[PKGSIZE];
    m_serial.init();

    sensor_msgs::Imu imu_msg;

    //判断是否已经标定过
    bool calibrated = false;
    double zeroshift[3];
    int times = 0;

    ROS_INFO_STREAM("Reading from serial port");
    std::cout << "start calibrate gyro ";

    ros::Rate loop_rate(200);
    double starttime, endtime;

    while(ros::ok())
    {
        starttime = ros::Time::now().toSec();
        ros::spinOnce();

        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
        tf::Quaternion q;
        q.setRPY(0, 0, 0);
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "imu"));

        //        int datasize = serport.read(dataPacket, packetLen);
        m_serial.Read();

        if (m_serial.isReadOK == 1)
        {
            m_serial.isReadOK = 0;
            if(parseimumsg(m_serial.readbuff, &imu_msg) == true)
            {

                //解析imu的datapacket
                if(!calibrated)
                {
                    ROS_INFO_STREAM("Calibrating");
                    if(times == 200)
                    {
                        std::cout << std::endl;
                        calibrated = true;
                        zeroshift[0] /= 200;
                        zeroshift[1] /= 200;
                        zeroshift[2] /= 200;
                        Global_Initialise(roll_init/200.0,pitch_init/200.0,yaw_init/200.0);
                    }
                    else
                    {
                        zeroshift[0] += imu_msg.angular_velocity.x;
                        zeroshift[1] += imu_msg.angular_velocity.y;
                        zeroshift[2] += imu_msg.angular_velocity.z;
                        GetInitAng(&imu_msg);
                        std::cout << ".";
                    }
                    times += 1;
//.........这里部分代码省略.........

  void GripperAction::goalCB(GoalHandle gh)
  {
	  // Cancels the currently active goal.

      if (has_active_goal_)
      {
    	  // Marks the current goal as canceled.
    	  active_goal_.setCanceled();
    	  has_active_goal_ = false;
          std::cout << "canceled current active goal" << std::endl;
      }

      gh.setAccepted();
      active_goal_ = gh;
      has_active_goal_ = true;
      goal_received_ = ros::Time::now();
      min_error_seen_ = 1e10;

      ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort);

      ROS_INFO("setFingersValues");
    
      std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle();

      std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl;

        target_position = active_goal_.getGoal()->command.position;
        target_effort = active_goal_.getGoal()->command.max_effort;

        //determine if the gripper is supposed to be opened or closed
        double current_position = jaco_->getFingersJointAngle()[0];
        if(current_position > target_position){
            opening = true;
            std::cout << "Opening gripper" << std::endl;
        }else{
            opening = false;
            std::cout << "Closing gripper" << std::endl;
        }

        

        fingerPositionsRadian[0] = target_position;
        fingerPositionsRadian[1] = target_position;
        fingerPositionsRadian[2] = target_position;

        

        std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl;


      double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])};

      if(!jaco_->setFingersValues(fingerPositionsDegree))
      {
    	  active_goal_.setCanceled();
    	  ROS_ERROR("Cancelling goal: moveJoint didn't work.");
          std::cout << "Cancelling goal because setFingers didn't work" << std::endl;
      }

      last_movement_time_ = ros::Time::now();
  }

int getCamCalibration(string dir_root, string camera_name, double* K,std::vector<double> & D,double *R,double* P){
/*
 *   from: http://kitti.is.tue.mpg.de/kitti/devkit_raw_data.zip
 *   calib_cam_to_cam.txt: Camera-to-camera calibration
 *   --------------------------------------------------
 *
 *     - S_xx: 1x2 size of image xx before rectification
 *     - K_xx: 3x3 calibration matrix of camera xx before rectification
 *     - D_xx: 1x5 distortion vector of camera xx before rectification
 *     - R_xx: 3x3 rotation matrix of camera xx (extrinsic)
 *     - T_xx: 3x1 translation vector of camera xx (extrinsic)
 *     - S_rect_xx: 1x2 size of image xx after rectification
 *     - R_rect_xx: 3x3 rectifying rotation to make image planes co-planar
 *     - P_rect_xx: 3x4 projection matrix after rectification
*/

    //    double K[9];         // Calibration Matrix
    //    double D[5];         // Distortion Coefficients
    //    double R[9];         // Rectification Matrix
    //    double P[12];        // Projection Matrix Rectified (u,v,w) = P * R * (x,y,z,q)

    string calib_cam_to_cam=(fs::path(dir_root) / fs::path("calib_cam_to_cam.txt")).string();
    ifstream file_c2c(calib_cam_to_cam.c_str());
    if (!file_c2c.is_open())
        return false;

    ROS_INFO_STREAM("Reading camera" << camera_name << " calibration from " << calib_cam_to_cam);

    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep{" "};

    string line="";
    char index=0;
    tokenizer::iterator token_iterator;

    while (getline(file_c2c,line))
    {
        // Parse string phase 1, tokenize it using Boost.
        tokenizer tok(line,sep);

        // Move the iterator at the beginning of the tokenize vector and check for K/D/R/P matrices.
        token_iterator=tok.begin();
        if (strcmp((*token_iterator).c_str(),((string)(string("K_")+camera_name+string(":"))).c_str())==0) //Calibration Matrix
        {
            index=0; //should be 9 at the end
            ROS_DEBUG_STREAM("K_" << camera_name);
            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
            {
                //std::cout << *token_iterator << '\n';
                K[index++]=boost::lexical_cast<double>(*token_iterator);
            }
        }

        // EXPERIMENTAL: use with unrectified images

        //        token_iterator=tok.begin();
        //        if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients
        //        {
        //            index=0; //should be 5 at the end
        //            ROS_DEBUG_STREAM("D_" << camera_name);
        //            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
        //            {
        ////                std::cout << *token_iterator << '\n';
        //                D[index++]=boost::lexical_cast<double>(*token_iterator);
        //            }
        //        }

        token_iterator=tok.begin();
        if (strcmp((*token_iterator).c_str(),((string)(string("R_")+camera_name+string(":"))).c_str())==0) //Rectification Matrix
        {
            index=0; //should be 12 at the end
            ROS_DEBUG_STREAM("R_" << camera_name);
            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
            {
                //std::cout << *token_iterator << '\n';
                R[index++]=boost::lexical_cast<double>(*token_iterator);
            }
        }

        token_iterator=tok.begin();
        if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified
        {
            index=0; //should be 12 at the end
            ROS_DEBUG_STREAM("P_rect_" << camera_name);
            for (token_iterator++; token_iterator != tok.end(); token_iterator++)
            {
                //std::cout << *token_iterator << '\n';
                P[index++]=boost::lexical_cast<double>(*token_iterator);
            }
        }

    }
    ROS_INFO_STREAM("... ok");
    return true;
}

	void GSPipe::publish_stream() {
		ROS_INFO_STREAM("Publishing stream...");

		// Pre-roll camera if needed
		if (preroll_) {
		  ROS_DEBUG("Performing preroll...");

		  //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
		  //I am told this is needed and am erring on the side of caution.
		  gst_element_set_state(pipeline_, GST_STATE_PLAYING);
		  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
			ROS_ERROR("Failed to PLAY during preroll.");
			return;
		  } else {
			ROS_DEBUG("Stream is PLAYING in preroll.");
		  }

		  gst_element_set_state(pipeline_, GST_STATE_PAUSED);
		  if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
			ROS_ERROR("Failed to PAUSE.");
			return;
		  } else {
			ROS_INFO("Stream is PAUSED in preroll.");
		  }
		}

		if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
		  ROS_ERROR("Could not start stream!");
		  return;
		}
		ROS_INFO("Started stream.");

		// Poll the data as fast a spossible
		while(ros::ok()) {
		  // This should block until a new frame is awake, this way, we'll run at the
		  // actual capture framerate of the device.
		  ROS_DEBUG("Getting data...");
		  GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));


		  GstFormat fmt = GST_FORMAT_TIME;
		  gint64 current = -1;

		  // Query the current position of the stream
		  //if (gst_element_query_position(pipeline_, &fmt, &current)) {
			//ROS_INFO_STREAM("Position "<<current);
		  //}

		  // Stop on end of stream
		  if (!buf) {
			ROS_INFO("Stream ended.");
			break;
		  }

		  ROS_DEBUG("Got data.");

		  // Get the image width and height
		  GstPad* pad = gst_element_get_static_pad(sink_, "sink");
		  const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
		  GstStructure *structure = gst_caps_get_structure(caps,0);
		  gst_structure_get_int(structure,"width",&width_);
		  gst_structure_get_int(structure,"height",&height_);

		  // Complain if the returned buffer is smaller than we expect
		  const unsigned int expected_frame_size =
			  image_encoding_ == sensor_msgs::image_encodings::RGB8
				  ? width_ * height_ * 3
				  : width_ * height_;

		  if (buf->size < expected_frame_size) {
			ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
				<< expected_frame_size << " bytes but got only "
				<< (buf->size) << " bytes. (make sure frames are correctly encoded)");
		  }

		  // Construct Image message
		  sensor_msgs::ImagePtr img(new sensor_msgs::Image());
		  sensor_msgs::CameraInfoPtr cinfo;

		  // Update header information
		  cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
		  cinfo->header.stamp = ros::Time::now();
		  cinfo->header.frame_id = frame_id_;
		  img->header = cinfo->header;

		  // Image data and metadata
		  img->width = width_;
		  img->height = height_;
		  img->encoding = image_encoding_;
		  img->is_bigendian = false;
		  img->data.resize(expected_frame_size);

		  ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_);
		  // Copy only the data we received
		  // Since we're publishing shared pointers, we need to copy the image so
		  // we can free the buffer allocated by gstreamer
		  if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
			img->step = width_ * 3;
		  } else {
			img->step = width_;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
  // Check the parameter server and initialize RDW
  if(!private_nh_.getParam("DW_R", RDW)) {
    ROS_WARN_STREAM("No DW_R found. Using default.");
  }
  // Check to see if the size is not equal to 16
  if (RDW.size() != 16) {
    ROS_WARN_STREAM("DW_R isn't 16 elements long!");
  }
  // And initialize the Matrix
  typedef Matrix<double, 4, 4, RowMajor> Matrix44;
  Matrix44 RDWMat(RDW.data());
  std::cout << "RDecaWave = " << std::endl;
  std::cout << RDWMat << std::endl;

  // Create temp array to initialize beacon locations for DecaWave
  double DWBL_temp[] = {0, 0,
		       5, 0,
		       5, 15,
		       0, 15};
  // And a std::vector, which will be used to initialize an Eigen Matrix
  std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
  // Check the parameter server and initialize DWBL
  if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
    ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
  }
  // Check to see if the size is not equal to 8
  if (DWBL.size() != 8) {
    ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
  }
  // And initialize the Matrix
  typedef Matrix<double, 4, 2, RowMajor> Matrix42;
  Matrix42 DWBLMat(DWBL.data());
  std::cout << "DW_Beacon_Loc = " << std::endl;
  std::cout << DWBLMat << std::endl;

  MatrixXd DecaWaveBeaconLoc(4,2);
  MatrixXd DecaWaveOffset(2,1);
  double DW_offset_x;
  double DW_offset_y;
  if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
    DW_offset_x = 0.0;
  if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
    DW_offset_y = 0.0;
  DecaWaveOffset << DW_offset_x, DW_offset_y;
  std::cout << "DecaWaveOffset = " << std::endl;
  std::cout << DecaWaveOffset << std::endl;

  ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
  // Decawave is not used in odom, so no need to initialize

  /**************************************************************************
   * Initialize Encoders for ekf_odom_ and ekf_map_
   **************************************************************************/
  // Create temp array to initialize R for DecaWave
  double REnc_temp[] = {0.01, 0,
			0, 0.01};
  // And a std::vector, which will be used to initialize an Eigen Matrix
  std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
  // Check the parameter server and initialize RDW
  if(!private_nh_.getParam("Enc_R", REnc)) {
    ROS_WARN_STREAM("No Enc_R found. Using default.");
  }
  // Check to see if the size is not equal to 4
  if (REnc.size() != 4) {
    ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
  }
  // And initialize the Matrix
  typedef Matrix<double, 2, 2, RowMajor> Matrix22;
  Matrix22 REncMat(REnc.data());
  std::cout << "R_Enc = " << std::endl;
  std::cout << REncMat << std::endl;

  double b, tpmRight, tpmLeft;
  if(!private_nh_.getParam("track_width", b))
    b = 1;
  if(!private_nh_.getParam("tpm_right", tpmRight))
    tpmRight = 1;
  if(!private_nh_.getParam("tpm_left", tpmLeft))
    tpmLeft = 1;
  ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
  ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
  std::cout << "track_width = " << b << std::endl;
  std::cout << "tpm_right   = " << tpmRight << std::endl;
  std::cout << "tpm_left    = " << tpmLeft << std::endl;

  /**************************************************************************
   * Initialize IMU for ekf_odom_ and ekf_map_
   **************************************************************************/
  double RIMU;
  if(!private_nh_.getParam("R_IMU", RIMU))
    RIMU = 0.01;
  ekf_map_.initIMU(RIMU);
  ekf_odom_.initIMU(RIMU);
  std::cout << "R_IMU = " << RIMU << std::endl;

  // Publish the initialized state and exit initialization.
  publishState(currTime);
  ROS_INFO_STREAM("Finished with initialization.");
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "example_cart_move_action_client"); // name this node 
    ros::NodeHandle nh; //standard ros node handle     
    ArmMotionCommander arm_motion_commander(&nh);
    CwruPclUtils cwru_pcl_utils(&nh);
    while (!cwru_pcl_utils.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 xvec_des,yvec_des,zvec_des,origin_des;
    geometry_msgs::PoseStamped rt_tool_pose;
    
    A_sensor_wrt_torso = cwru_pcl_utils.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=arm_motion_commander.plan_move_to_pre_pose();
    
    //send command to execute planned motion
    rtn_val=arm_motion_commander.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
            //construct a goal affine pose:
            for (int i=0;i<3;i++) {
                origin_des[i] = centroid[i]; // convert to double precision
                zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
                xvec_des[i] = major_axis[i];
            }
            origin_des[2]+=0.02; //raise up 2cm
            yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
            Rmat.col(0)= xvec_des;
            Rmat.col(1)= yvec_des;
            Rmat.col(2)= zvec_des;
            Affine_des_gripper.linear()=Rmat;
            Affine_des_gripper.translation()=origin_des;
            cout<<"des origin: "<<Affine_des_gripper.translation().transpose()<<endl;
            cout<<"orientation: "<<endl;
            cout<<Rmat<<endl;
            //convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
            rt_tool_pose.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
            //could fill out the header as well...
            
            // send move plan request:
            rtn_val=arm_motion_commander.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=arm_motion_commander.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;
}

  // Publish the state as an odom message on the topic odom_ekf. Alos well broadcast a transform.
void EkfNode::publishState(ros::Time now){
  // Create an Odometry message to publish
  nav_msgs::Odometry state_msg;
  /**************************************************************************
   * Populate Odometry message for ekf_map_
   **************************************************************************/
  // Populate timestamp, position frame, and velocity frame
  state_msg.header.stamp = now;
  state_msg.header.frame_id = map_frame_;
  state_msg.child_frame_id = base_frame_;
  // Populate the position and orientation
  state_msg.pose.pose.position.x = ekf_map_.state_(0); // x
  state_msg.pose.pose.position.y = ekf_map_.state_(1); // y
  state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_map_.state_(2)); // theta
  boost::array<double,36> temp;
  // Populate the covariance matrix
  state_msg.pose.covariance = temp;
  // Populate the linear and angular velocities
  state_msg.twist.twist.linear.x = ekf_map_.state_(3); // v
  state_msg.twist.twist.angular.z = ekf_map_.state_(4); // omega
  // Populate the covariance matrix
  state_msg.twist.covariance = temp;
  // Publish the message!
  stateMapPub_.publish(state_msg);
  // Print for debugging purposes
  ROS_INFO_STREAM("\nstate = \n" << ekf_map_.state_);
  ROS_INFO_STREAM("\ncovariance = \n" << ekf_map_.cov_);
  /**************************************************************************
   * Populate Odometry message for ekf_odom_
   **************************************************************************/
  // Populate timestamp, position frame, and velocity frame
  state_msg.header.stamp = now;
  state_msg.header.frame_id = odom_frame_;
  state_msg.child_frame_id = base_frame_;
  // Populate the position and orientation
  state_msg.pose.pose.position.x = ekf_odom_.state_(0); // x
  state_msg.pose.pose.position.y = ekf_odom_.state_(1); // y
  state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_odom_.state_(2)); // theta
  // Populate the covariance matrix
  state_msg.pose.covariance = temp;
  // Populate the linear and angular velocities
  state_msg.twist.twist.linear.x = ekf_odom_.state_(3); // v
  state_msg.twist.twist.angular.z = ekf_odom_.state_(4); // omega
  // Populate the covariance matrix
  state_msg.twist.covariance = temp;
  // Publish the message!
  stateOdomPub_.publish(state_msg);
  /**************************************************************************
   * Create and broadcast transforms for map->odom and odom->base_link
   **************************************************************************/
  static tf::TransformBroadcaster br;
  // First create and broadcast odom->base_link since we already have that.
  tf::Transform odom2base;
  odom2base.setOrigin(tf::Vector3(ekf_odom_.state_(0), ekf_odom_.state_(1), 0.0));
  tf::Quaternion q;
  q.setRPY(0.0, 0.0, ekf_odom_.state_(2));
  odom2base.setRotation(q);
  br.sendTransform(tf::StampedTransform(odom2base, now, odom_frame_, base_frame_));
  // Now create and broadcast map->odom. But, first we must calculate this from
  // map->base_link and odom->base_link.
  tf::Transform map2odom;
  double x2 = ekf_odom_.state_(0);
  double y2 = ekf_odom_.state_(1);
  double theta2 = ekf_odom_.state_(2);
  double x3 = ekf_map_.state_(0);
  double y3 = ekf_map_.state_(1);
  double theta3 = ekf_map_.state_(2);
  double x1 = cos(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
             -sin(theta3)*( x2*sin(theta2)-y2*cos(theta2))
             +x3;
  double y1 = sin(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
             +cos(theta3)*( x2*sin(theta2)-y2*cos(theta2))
             +y3;
  double theta1 = theta3 - theta2;
  map2odom.setOrigin(tf::Vector3(x1, y1, 0.0));
  q.setRPY(0.0, 0.0, theta1);
  map2odom.setRotation(q);
  br.sendTransform(tf::StampedTransform(map2odom, now, map_frame_, odom_frame_));
} 

  // Processes the point cloud with OpenCV using the PCL cluster indices
  void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
                        //                        const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
                        const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr  cloud_transformed,
                        const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
                        const pcl::PointCloud<pcl::PointXYZRGB>&  cloud )
  {

    // -------------------------------------------------------------------------------------------------------
    // Convert image
    ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");

    try
    {
      sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
      //      pcl::toROSMsg (*pointcloud_msg, *image_msg);
      pcl::toROSMsg (*cloud_transformed, *image_msg);
      cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
      full_input_image = input_bridge->image;
    }
    catch (cv_bridge::Exception& ex)
    {
      ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
      return;
    }

    // -------------------------------------------------------------------------------------------------------
    // Process Image

    // Convert image to gray
    cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
    //cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);

    // Blur image - reduce noise with a 3x3 kernel
    cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );

    ROS_INFO_STREAM_NAMED("perception","Finished coverting");

    // -------------------------------------------------------------------------------------------------------
    // Check OpenCV and PCL image height for errors
    int image_width = cloud.width;
    int image_height = cloud.height;
    ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
    int image_width_cv = full_input_image.size.p[1];
    int image_height_cv = full_input_image.size.p[0];
    ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");

    if( image_width != image_width_cv || image_height != image_height_cv )
    {
      ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
      return;
    }

    // -------------------------------------------------------------------------------------------------------
    // GUI Stuff

    // First window
    const char* opencv_window = "Source";
    /*
      cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
      cv::imshow( opencv_window, full_input_image_gray );
    */


    //    while(true)  // use this when we want to tweak the image
    {

      output_image = full_input_image.clone();

      // -------------------------------------------------------------------------------------------------------
      // Start processing clusters
      ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");

      int top_image_overlay_x = 0; //