void MentalPerspectiveTransformer::callback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_in) {
    static tf::TransformListener listener;
    const std::string source_frame = "robot_head";
    const std::string target_frame = "head_origin1";

    // first get cloud in the source frame (it is usually in camera frame)
    pcl::PointCloud<pcl::PointXYZRGB> cloud_in_source_frame;
    pcl_ros::transformPointCloud(source_frame, *cloud_in, cloud_in_source_frame, listener);

    static tf::TransformBroadcaster br;
    try {
        // get transform from source to target frame
        tf::StampedTransform transform_source_target;
        listener.waitForTransform(target_frame, source_frame, pcl_conversions::fromPCL(cloud_in_source_frame.header.stamp), ros::Duration(1.0));
        listener.lookupTransform (target_frame, source_frame, ros::Time(0), transform_source_target);

        ROS_INFO_STREAM("Origin: " << transform_source_target.getOrigin().getX() << " "
                                   << transform_source_target.getOrigin().getY() << " "
                                   << transform_source_target.getOrigin().getZ());
        ROS_INFO_STREAM("Angle: "  << transform_source_target.getRotation().getAngle());
        ROS_INFO_STREAM("Axis: "   << transform_source_target.getRotation().getAxis().getX() << " "
                                   << transform_source_target.getRotation().getAxis().getY() << " "
                                   << transform_source_target.getRotation().getAxis().getZ());

        tf::Quaternion q_origin       = tf::Quaternion::getIdentity();
        tf::Quaternion q_dest         = transform_source_target.getRotation();
        tf::Vector3    v_dest         = transform_source_target.getOrigin();

        //ROS_INFO_STREAM("q_dest: " << q_dest.getX() << " " << q_dest.getY() << " " << q_dest.getZ() << " " << q_dest.getW());

        /*tf::Vector3 v_r               = v_dest/2.0;
        ROS_INFO_STREAM("v_r: " << v_r.getX() << " " << v_r.getY() << " " << v_r.getZ());
        tf::Vector3 c                 = tf::Vector3(-1.0*v_r[1], v_r[0], 0);
        tf::Vector3 p_circle          = v_r + c;
        ROS_INFO_STREAM("p_circle " << p_circle.getX() << " " << p_circle.getY() << " " << p_circle.getZ());
        double radius                 = v_r.length();
        ROS_INFO_STREAM("radius " << radius);
        tf::Vector3 n = p_circle.cross(v_r).normalize();
        if(transform.getRotation().getAngle()>M_PI) {
            n = -1.0*n;
        }

        ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
        tf::Vector3 u                 = v_dest.normalized();
        ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());*/

        // compute circle equation
        double r1x = (pow(v_dest.getX(), 2.0) + pow(v_dest.getY(), 2.0) + pow(v_dest.getZ(), 2.0)) / (2.0*(v_dest.getX()+pow(v_dest.getZ(), 2.0)/v_dest.getX()));
        double r1y = 0.0;
        double r1z = r1x * v_dest.getZ()/v_dest.getX();

        tf::Vector3 r1(r1x, r1y, r1z);
        tf::Vector3 r2(v_dest.getX() - r1x, v_dest.getY() - r1y, v_dest.getZ() - r1z);

        double radius = r1.length();

        tf::Vector3 n = v_dest.cross(2*r1).normalize();
        tf::Vector3 u = r1.normalized();
        tf::Vector3 v_r = r1;

        ROS_INFO_STREAM("u " << u.getX() << " " << u.getY() << " " << u.getZ());
        ROS_INFO_STREAM("n " << n.getX() << " " << n.getY() << " " << n.getZ());
        ROS_INFO_STREAM("r1 " << r1x << " " << r1y << " " << r1z);
        ROS_INFO_STREAM("r2 " << r2.getX() << " " << r2.getY() << " " << r2.getZ());
        ROS_INFO_STREAM("radius " << radius);

        // get theta for head coordinates
        perspective_taking_python::CircleEquation ce;
        ce.request.radius = radius;
        tf::vector3TFToMsg(v_dest, ce.request.h);
        tf::vector3TFToMsg(n, ce.request.n);
        tf::vector3TFToMsg(u, ce.request.u);
        tf::vector3TFToMsg(v_r, ce.request.c);
        if(equationSolverClient_.call(ce)) {
            ROS_INFO_STREAM("response: " << ce.response.theta);
        } else {
            ROS_ERROR("Did not get response from equationSolverClient!");
            return;
        }

        double delta = 0.01; // approximately 10 steps at 1.3 meters distance to human (straight ahead)
        double epsilon = M_PI * delta / radius;
        double resp_theta = ce.response.theta;
        ROS_INFO_STREAM("epsilon: " << epsilon);

        for(double theta = -1.0, step=0; theta<=resp_theta; theta+=epsilon, step+=1) {
            double b = 1.0/(resp_theta+1.0);
            if(transform_source_target.getRotation().getAngle()>M_PI) {
                b = -1.0*b;
            }
            double theta_for_q = b * theta + b;
            tf::Quaternion q_intermediate = q_origin.slerp(q_dest, theta_for_q);
            //tf::Quaternion q_intermediate = q_origin.slerp(q_dest, 1.0-theta);
            //tf::Vector3 v_intermediate    = v_origin.lerp(v_dest, ration);
            tf::Vector3 v_intermediate    = radius*cos(M_PI*theta)*u + radius*sin(M_PI*theta)*n.cross(u)+v_r;

            ROS_INFO_STREAM("theta_for_q: " << theta_for_q);
            ROS_INFO_STREAM("theta: " << theta << ": " << v_intermediate.getX() << " " << v_intermediate.getY() << " " << v_intermediate.getZ());

            tf::Transform intermediate_transform;
//.........这里部分代码省略.........

bool WifiStumbler::stumble()
{
  int pid;
  pid = getpid();

  struct iwreq w_request;
  w_request.u.data.pointer = (caddr_t)&pid;
  w_request.u.data.length = 0;

  if (iw_set_ext(wlan_sock_, wlan_if_.c_str(), SIOCSIWSCAN, &w_request) < 0)
  {
    ROS_ERROR("Couldn't start stumbler.");
    return false;
  }

  timeval time;
  time.tv_sec = 0;
  time.tv_usec = 200000;

  uint8_t *p_buff = NULL;
  int buff_size = IW_SCAN_MAX_DATA;

  bool is_end = false;
  while(!is_end)
  {
    fd_set fds;
    int ret;
    FD_ZERO(&fds);
    ret = select(0, &fds, NULL, NULL, &time);
    if (ret == 0)
    {
      uint8_t *p_buff2;
      while (!is_end)
      {
        p_buff2 = (uint8_t *)realloc(p_buff, buff_size);
        p_buff = p_buff2;
        w_request.u.data.pointer = p_buff;
        w_request.u.data.flags = 0;
        w_request.u.data.length = buff_size;
        if (iw_get_ext(wlan_sock_, wlan_if_.c_str(), SIOCGIWSCAN, &w_request) < 0)
        {
          if (errno == E2BIG && range_.we_version_compiled > 16)
          {
            if (w_request.u.data.length > buff_size)
              buff_size = w_request.u.data.length;
            else
              buff_size *= 2;
            continue;
          }
          else if (errno == EAGAIN)
          {
            time.tv_sec = 0;
            time.tv_usec = 200000;
            break;
          }
        }
        else
          is_end = true;
      }
    }
  }

  // Put wifi data into ROS message
  wifi_tools::AccessPoint ap;
  iw_event event;
  stream_descr stream;

  wifi_stumble_.data.clear();
  wifi_stumble_.header.stamp = ros::Time::now();

  iw_init_event_stream(&stream, (char *)p_buff, w_request.u.data.length);
  is_end = false;
  int value = 0;
  while(!is_end)
  {
    value = iw_extract_event_stream(&stream, &event, range_.we_version_compiled);
    if(!(value>0))
    {
      is_end = true;
    }
    else
    {
      if(event.cmd == IWEVQUAL)
      {
        // quality part of statistics
        //ROS_INFO("command=IWEVQUAL");
        if (event.u.qual.level != 0 || (event.u.qual.updated & (IW_QUAL_DBM | IW_QUAL_RCPI)))
        {
          ap.noise = event.u.qual.noise;
          ap.ss    = event.u.qual.level;
        }
      }
      else if(event.cmd == SIOCGIWAP)
      {
        // get access point MAC addresses
        //ROS_INFO("command=SIOCGIWAP");
        char mac_buff[1024];
        iw_ether_ntop((const struct ether_addr *)&event.u.ap_addr.sa_data,mac_buff);
        ap.mac_address = std::string(mac_buff);
        ROS_INFO("mac_addr=%s",mac_buff);
//.........这里部分代码省略.........

    // Function needs working on
    void mapUpdate(double x_sens_dist, double y_sens_dist, int sensor)
    {

        //x_sens_dist and y_sens_dist are distance from robot to detected object.
        // Always start at center of grid: add/subtract to center_x to all distances
        // according to sign conventions.
        // First, convert sensor readings from robot frame to map frame then and then convert

        int x_sens_cell = floor(x_sens_dist/resolution);
        int y_sens_cell = floor(y_sens_dist/resolution);
        int weight_cell = 5;
        double corner_x, corner_y;
        int corner_x_cell, corner_y_cell;
        tf::StampedTransform transform;
        switch(sensor)
        {
            case 1:
            try
            {
                listener.waitForTransform("/sensor1", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor1", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
                ros::Time::init();
            }
                catch(tf::TransformException ex)
                {
                    ROS_ERROR("%s",ex.what());
                }
            break;


            case 2:
            try
            {
                listener.waitForTransform("/sensor2", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor2", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }
            break;

            case 3:
            try
            {
                listener.waitForTransform("/sensor3", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor3", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }
            break;

            case 4:
            try
            {
                listener.waitForTransform("/sensor4", "/map", ros::Time(0), ros::Duration(1));
                listener.lookupTransform("/map", "/sensor4", ros::Time(0), transform);
                corner_x = transform.getOrigin().getX();
                corner_y = transform.getOrigin().getY();
                corner_x_cell = floor(corner_x/resolution);
                corner_y_cell = floor(corner_y/resolution);
            }
                catch(tf::TransformException ex)
            {
                    ROS_ERROR("%s",ex.what());
            }

        }


        //map_vector[x_sens_cell+width_map*y_sens_cell]=100;
        int x1, x2, y1, y2;
        if (corner_x_cell > x_sens_cell) {
            x1 = x_sens_cell;
            x2 = corner_x_cell;
        } else {
            x1 = corner_x_cell;
            x2 = x_sens_cell;
        }
        if (corner_y_cell > y_sens_cell) {
            y1 = y_sens_cell;
            y2 = corner_y_cell;
        } else {
            y1 = corner_y_cell;
            y2 = y_sens_cell;
//.........这里部分代码省略.........

void
FootstepNavigation::executeFootsteps()
{
  if (ivPlanner.getPathSize() <= 1)
    return;

  // lock this thread
  ivExecutingFootsteps = true;

  ROS_INFO("Start walking towards the goal.");

  humanoid_nav_msgs::StepTarget step;
  humanoid_nav_msgs::StepTargetService step_srv;

  tf::Transform from;
  std::string support_foot_id;

  // calculate and perform relative footsteps until goal is reached
  state_iter_t to_planned = ivPlanner.getPathBegin();
  if (to_planned == ivPlanner.getPathEnd())
  {
    ROS_ERROR("No plan available. Return.");
    return;
  }

  const State* from_planned = to_planned.base();
  to_planned++;
  while (to_planned != ivPlanner.getPathEnd())
  {
    try
    {
      boost::this_thread::interruption_point();
    }
    catch (const boost::thread_interrupted&)
    {
      // leave this thread
      return;
    }

    if (from_planned->getLeg() == RIGHT)
      support_foot_id = ivIdFootRight;
    else // support_foot = LLEG
      support_foot_id = ivIdFootLeft;

    // try to get real placement of the support foot
    if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
                         ros::Duration(0.5), &from))
    {
      // calculate relative step and check if it can be performed
      if (getFootstep(from, *from_planned, *to_planned, &step))
      {
        step_srv.request.step = step;
        ivFootstepSrv.call(step_srv);
      }
      // ..if it cannot be performed initialize replanning
      else
      {
        ROS_INFO("Footstep cannot be performed. Replanning necessary.");

        replan();
        // leave the thread
        return;
      }
    }
    else
    {
      // if the support foot could not be received wait and try again
      ros::Duration(0.5).sleep();
      continue;
    }

    from_planned = to_planned.base();
    to_planned++;
  }
  ROS_INFO("Succeeded walking to the goal.\n");

  // free the lock
  ivExecutingFootsteps = false;
}

bool GazeboRosControllerManager::LoadControllerManagerFromURDF()
{
  std::string urdf_string = GetURDF(this->robotParam);

  // initialize TiXmlDocument doc with a string
  TiXmlDocument doc;
  if (!doc.Parse(urdf_string.c_str()) && doc.Error())
  {
    ROS_ERROR("Could not load the gazebo controller manager plugin's"
              " configuration file: %s\n", urdf_string.c_str());
    return false;
  }
  else
  {
    // debug
    // doc.Print();
    // std::cout << *(doc.RootElement()) << std::endl;

    // Pulls out the list of actuators used in the robot configuration.
    struct GetActuators : public TiXmlVisitor
    {
      std::set<std::string> actuators;
      virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
      {
        if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        else if (elt.ValueStr() ==
          std::string("rightActuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        else if (elt.ValueStr() ==
          std::string("leftActuator") && elt.Attribute("name"))
          actuators.insert(elt.Attribute("name"));
        return true;
      }
    } get_actuators;
    doc.RootElement()->Accept(&get_actuators);

    // Places the found actuators into the hardware interface.
    std::set<std::string>::iterator it;
    for (it = get_actuators.actuators.begin();
         it != get_actuators.actuators.end(); ++it)
    {
      // std::cout << " adding actuator " << (*it) << std::endl;
      pr2_hardware_interface::Actuator* pr2_actuator =
        new pr2_hardware_interface::Actuator(*it);
      pr2_actuator->state_.is_enabled_ = true;
      this->hardware_interface_.addActuator(pr2_actuator);
    }

    // Setup mechanism control node
    this->controller_manager_->initXml(doc.RootElement());

    if (this->controller_manager_->state_ == NULL)
    {
      ROS_ERROR("Mechanism unable to parse robot_description URDF"
                " to fill out robot state in controller_manager.");
      return false;
    }

    // set fake calibration states for simulation
    for (unsigned int i = 0;
      i < this->controller_manager_->state_->joint_states_.size(); ++i)
      this->controller_manager_->state_->joint_states_[i].calibrated_ =
        calibration_status_;

    return true;
  }
}

  boost::shared_ptr<kinematics::KinematicsBase> allocKinematicsSolver(const robot_model::JointModelGroup *jmg)
  {
    boost::shared_ptr<kinematics::KinematicsBase> result;
    if (!jmg)
    {
      ROS_ERROR("Specified group is NULL. Cannot allocate kinematics solver.");
      return result;
    }

    ROS_DEBUG("Received request to allocate kinematics solver for group '%s'", jmg->getName().c_str());

    if (kinematics_loader_ && jmg)
    {
      std::map<std::string, std::vector<std::string> >::const_iterator it = possible_kinematics_solvers_.find(jmg->getName());
      if (it != possible_kinematics_solvers_.end())
      {
        // just to be sure, do not call the same pluginlib instance allocation function in parallel
        boost::mutex::scoped_lock slock(lock_);

        for (std::size_t i = 0 ; !result && i < it->second.size() ; ++i)
        {
          try
          {
            result.reset(kinematics_loader_->createUnmanagedInstance(it->second[i]));
            if (result)
            {
              const std::vector<const robot_model::LinkModel*> &links = jmg->getLinkModels();
              if (!links.empty())
              {
                const std::string &base = links.front()->getParentJointModel()->getParentLinkModel() ?
                  links.front()->getParentJointModel()->getParentLinkModel()->getName() : jmg->getParentModel().getModelFrame();

                // choose the tip of the IK solver
                const std::vector<std::string> tips = chooseTipFrames(jmg);

                // choose search resolution
                double search_res = search_res_.find(jmg->getName())->second[i]; // we know this exists, by construction

                if (!result->initialize(robot_description_, jmg->getName(),
                                        (base.empty() || base[0] != '/') ? base : base.substr(1) , tips, search_res))
                {
                  ROS_ERROR("Kinematics solver of type '%s' could not be initialized for group '%s'", it->second[i].c_str(), jmg->getName().c_str());
                  result.reset();
                }
                else
                {
                  result->setDefaultTimeout(jmg->getDefaultIKTimeout());
                  ROS_DEBUG("Successfully allocated and initialized a kinematics solver of type '%s' with search resolution %lf for group '%s' at address %p",
                            it->second[i].c_str(), search_res, jmg->getName().c_str(), result.get());
                }
              }
              else
                ROS_ERROR("No links specified for group '%s'", jmg->getName().c_str());
            }
          }
          catch (pluginlib::PluginlibException& e)
          {
            ROS_ERROR("The kinematics plugin (%s) failed to load. Error: %s", it->first.c_str(), e.what());
          }
        }
      }
      else
        ROS_DEBUG("No kinematics solver available for this group");
    }

    if (!result)
    {
      ROS_DEBUG("No usable kinematics solver was found for this group.");
      ROS_DEBUG("Did you load kinematics.yaml into your node's namespace?");
    }
    return result;
  }

int main(int argc, char **argv)
{
	int rate= 50;
	float inv_rate=1/rate;

	ros::init(argc, argv, "ARdrone_PID_to_Point");
	ros::NodeHandle n;
	ros::Rate loop_rate(rate);
	
	tf::TransformListener tf_listener;
	tf::TransformBroadcaster br;

	tf::StampedTransform desired_pos;
	tf::StampedTransform ardrone;
	tf::StampedTransform trackee;
	tf::StampedTransform desired_in_ardrone_coords;

	ros::Publisher pub_twist;
	geometry_msgs::Twist twist_msg;
	tf::Quaternion ardrone_yawed;
	
	memset(controls, 0, sizeof(double)*4);
	memset(old_data, 0, sizeof(double)*5);
	memset(new_data, 0, sizeof(double)*5);
	memset(integration, 0, sizeof(double)*5);
	memset(derivative, 0, sizeof(double)*5);
	memset(pid, 0, sizeof(double)*4);
	
	//PID params
	min_control[yaw] =-1.0;
	min_control[roll] =-1.0;
	min_control[pitch]=-1.0;
	min_control[thrust]=-1.0;

	max_control[yaw]=1.0;
	max_control[roll]=1.0;
	max_control[pitch]=1.0;
	max_control[thrust]=1.0;

	min_pid =-5.0;
	max_pid =5.0;

	while (ros::ok())
	{
		  try {
		//Get desired position transform
		tf_listener.waitForTransform("/optitrak", "/desired_position", ros::Time(0), ros::Duration(inv_rate));
		tf_listener.lookupTransform("/optitrak", "/desired_position",  ros::Time(0), desired_pos); 
		// Get the quad rotor transform
		tf_listener.waitForTransform("/optitrak", "/ardrone", ros::Time(0), ros::Duration(inv_rate));
		tf_listener.lookupTransform("/optitrak", "/ardrone",  ros::Time(0), ardrone);

		} catch (...) {
		  	ROS_ERROR("Failed on initial transform: Check VRPN server");}
	
		  double y1, p1, r1;
		  btMatrix3x3(ardrone.getRotation()).getRPY(r1, p1, y1);
		  
		  ardrone_yawed.setRPY(0.0,0.0,y1);
		  
		  /* //Dep code 
		  btQuaternion ardrone_yawed(y1, 0.0, 0.0);
		  */
		  
		  ardrone.setRotation(ardrone_yawed);

		  //set up twist publisher
		  pub_twist = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1); /* Message queue length is just 1 */
		
		  // Register the ardrone without roll and pitch with the transform system
		  br.sendTransform( tf::StampedTransform(ardrone, ros::Time::now(), "/optitrak", "ardrone_wo_rp") );
		  
		  try {
		  // Get the vector between quad without roll and pitch and the desired point
		  tf_listener.waitForTransform("/ardrone_wo_rp", "/desired", ros::Time(0), ros::Duration(inv_rate));
		  tf_listener.lookupTransform("/ardrone_wo_rp", "/desired", ros::Time(0), desired_in_ardrone_coords);
		  }
		  catch (...) {
		  	ROS_ERROR("Failed on w/o roll and pitch transform");}
		  	
		  // Extract the yaw, x, y, z components
		  btMatrix3x3(desired_in_ardrone_coords.getRotation()).getRPY(r1, p1, y1);
		  new_data[yaw]=y1;
		  new_data[pitch] = desired_in_ardrone_coords.getOrigin().getX();
		  new_data[roll] = desired_in_ardrone_coords.getOrigin().getY();
		  new_data[thrust] = desired_in_ardrone_coords.getOrigin().getZ();
 		  new_data[msg_time] = (double)ros::Time::now().toSec();
		  ROS_DEBUG("Error: [x: %f y:  %f z: %f]", new_data[pitch], new_data[roll], new_data[thrust]);

		  // Integrate/Derivate the data
		  double deltaT = (new_data[msg_time] - old_data[msg_time]);
		  integration[yaw] += new_data[yaw] * deltaT;
		  integration[pitch] += new_data[pitch] * deltaT;
		  integration[roll] += new_data[roll] * deltaT;
		  integration[thrust] += new_data[thrust] * deltaT;

		  ROS_DEBUG("Integration: [deltaT: %f x: %f y: %f z: %f]", deltaT, integration[pitch], integration[roll], integration[thrust]);

		  derivative[yaw] = (new_data[yaw] - old_data[yaw])/deltaT;
		  derivative[pitch] = (new_data[pitch] - old_data[pitch])/deltaT;
//.........这里部分代码省略.........

  void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
    }
    //cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
    dataPtr = (ARUint8 *) capture_->imageData;

    // detect the markers in the video frame 
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      ROS_FATAL ("arDetectMarker failed");
      ROS_BREAK ();             // FIXME: I don't think this should be fatal... -Bill
    }

    // check for known patterns
    k = -1;
    for (i = 0; i < marker_num; i++)
    {
      if (marker_info[i].id == patt_id_)
      {
        ROS_DEBUG ("Found pattern: %d ", patt_id_);

        // make sure you have the best pattern (highest confidence factor)
        if (k == -1)
          k = i;
        else if (marker_info[k].cf < marker_info[i].cf)
          k = i;
      }
    }

    if (k != -1)
    {
      // **** get the transformation between the marker and the real camera
      double arQuat[4], arPos[3];

      if (!useHistory_ || contF == 0)
        arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
      else
        arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);

      contF = 1;

      //arUtilMatInv (marker_trans_, cam_trans);
      arUtilMat2QuatPos (marker_trans_, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];
    
      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      quat[0] = -arQuat[0];
      quat[1] = -arQuat[1];
      quat[2] = -arQuat[2];
      quat[3] = arQuat[3];

      ROS_DEBUG (" QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0], pos[1], pos[2]);
      ROS_DEBUG ("     Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);

      // **** publish the marker

		  ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
		  ar_pose_marker_.header.stamp    = image_msg->header.stamp;
		  ar_pose_marker_.id              = marker_info->id;

		  ar_pose_marker_.pose.pose.position.x = pos[0];
		  ar_pose_marker_.pose.pose.position.y = pos[1];
		  ar_pose_marker_.pose.pose.position.z = pos[2];

		  ar_pose_marker_.pose.pose.orientation.x = quat[0];
		  ar_pose_marker_.pose.pose.orientation.y = quat[1];
		  ar_pose_marker_.pose.pose.orientation.z = quat[2];
		  ar_pose_marker_.pose.pose.orientation.w = quat[3];
		
		  ar_pose_marker_.confidence = marker_info->cf;

		  arMarkerPub_.publish(ar_pose_marker_);
		  ROS_DEBUG ("Published ar_single marker");
		
      // **** publish transform between camera and marker

      btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
      btVector3 origin(pos[0], pos[1], pos[2]);
//.........这里部分代码省略.........

void costmap_cb(const nav_msgs::GridCells::ConstPtr& msg) {
    ODOM_FRAME = msg->header.frame_id;
    
    geometry_msgs::PoseArray ff_msg;
    ff_msg.header.stamp = msg->header.stamp;
    ff_msg.header.frame_id = ODOM_FRAME;
    
    last_fv = msg->header.stamp;
    force_vector[0] = force_vector[1] = 0;
    std::vector<geometry_msgs::Point> points = msg->cells;
    
    std::vector<geometry_msgs::Pose> poses;
    
    int count = 0;
    float total_weight = 0.0;
    
    for (std::vector<std::string>::size_type i = 0; i < points.size(); i++) {
        geometry_msgs::PointStamped point, point_trans;
        point.header.stamp = msg->header.stamp;
        point.header.frame_id = ODOM_FRAME;
        point.point.x = points[i].x;
        point.point.y = points[i].y;
        tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, point.header.stamp, ros::Duration(0.1));
        try {
            tf_listener->transformPoint(BASE_FRAME, point, point_trans);
        }
        catch (tf::ExtrapolationException e) {
            ROS_ERROR("Unable to get tf transform: %s", e.what());
            return;
        }
        
        float x = point_trans.point.x;
        float y = point_trans.point.y;
        float yaw = atan(y / x);
        if (x > 0) yaw = modulus(yaw + PI, 2*PI);
        
        std::vector<float> point_vector = point_to_vector(x, y);
        
        if (point_vector.size() > 0) {
            count++;
            //float weight = 0.75*(MAX_RANGE - BASE_RADIUS - CLEARING_DIST)/(sqrt(pow(x, 2) + pow(y, 2)) - BASE_RADIUS - CLEARING_DIST);
            //if ( weight < 0 )             
            //    std::cout << "evil" << std::endl;
            //length += weight;
            //float weight = (PI - abs(yaw)) / PI;
            force_vector[0] += point_vector[0];
            force_vector[1] += point_vector[1];

            geometry_msgs::PoseStamped pose, pose_trans;
            pose.header.stamp = msg->header.stamp;
            pose.header.frame_id = BASE_FRAME;
            pose.pose.position.x = x;
            pose.pose.position.y = y;
            pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);

            tf_listener->waitForTransform(ODOM_FRAME, BASE_FRAME, pose.header.stamp, ros::Duration(0.1));
            try {
                tf_listener->transformPose(ODOM_FRAME, pose, pose_trans);
            }
            catch (tf::ExtrapolationException e) {
                ROS_ERROR("Unable to get tf transform: %s", e.what());
                return;
            }
            
            poses.push_back(pose_trans.pose);
        }
    }
    
    //force_vector[0] /= length + 1;
    //force_vector[1] /= length + 1;
    
    std::cout << force_vector[0] << "," << force_vector[1] << std::endl;
    
    // publish resulting force vector as Pose
    geometry_msgs::PoseStamped force, force_trans;
    force.header.stamp = msg->header.stamp;
    force.header.frame_id = BASE_FRAME;
    float force_yaw = atan(force_vector[1] / force_vector[0]);
    if (force_vector[0] > 0) force_yaw = modulus(force_yaw + PI, 2*PI);
    force.pose.orientation = tf::createQuaternionMsgFromYaw(force_yaw);    
    tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, msg->header.stamp, ros::Duration(0.1));
    try {
        tf_listener->transformPose(ODOM_FRAME, force, force_trans);
    }
    catch (tf::ExtrapolationException e) {
        ROS_ERROR("Unable to get tf transform: %s", e.what());
        return;
    }
    force_obst_pub.publish(force_trans);    
    
    // publish force field as PoseArray
    ff_msg.poses = poses;  
    force_field_pub.publish(ff_msg);
}

int main(int argc, char **argv)
{
  // Init the ROS node
  ros::init (argc, argv, "move_right_arm_joint_goal_test");

  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Action client for sending motion planing requests
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);

  // Wait for the action client to be connected to the server
  ROS_INFO("Connecting to server...");
  if (!move_arm.waitForServer(ros::Duration(5.0)))
  {
    ROS_ERROR("Timed-out waiting for the move_arm action server.");
    return EXIT_FAILURE;
  }
  ROS_INFO("Connected to server.");

  // Prepare motion plan request with joint-space goal
  arm_navigation_msgs::MoveArmGoal goal;
  std::vector<std::string> names(9);
  names[0] = "torso_1_joint";
  names[1] = "torso_2_joint";
  names[2] = "arm_right_1_joint";
  names[3] = "arm_right_2_joint";
  names[4] = "arm_right_3_joint";
  names[5] = "arm_right_4_joint";
  names[6] = "arm_right_5_joint";
  names[7] = "arm_right_6_joint";
  names[8] = "arm_right_7_joint";

  goal.motion_plan_request.group_name = "right_arm_torso";
  goal.motion_plan_request.num_planning_attempts = 1;
  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goal.motion_plan_request.planner_id= std::string("");
  goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
  }

  goal.motion_plan_request.goal_constraints.joint_constraints[0].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[1].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[2].position =  1.2;
  goal.motion_plan_request.goal_constraints.joint_constraints[3].position =  0.15;
  goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
  goal.motion_plan_request.goal_constraints.joint_constraints[5].position =  1.3;
  goal.motion_plan_request.goal_constraints.joint_constraints[6].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[7].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[8].position =  0.0;

  // Send motion plan request
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goal);
    finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving joint-space goal.");
    }
    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 PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{

  boost::mutex::scoped_lock(mutex_);
  sensor_msgs::PointCloud2 output;
  sensor_msgs::PointCloud2 convex_hull;
  sensor_msgs::PointCloud2 object_msg;
  sensor_msgs::PointCloud2 nonObject_msg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*cloud2Msg_input, *cloud);


  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  
  // *** Normal estimation
  // Create the normal estimation class and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(cloud);
  // Creating the kdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  ne.setSearchMethod(tree);

  // output dataset
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);

  // use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch(0.3);

  // compute the features
  ne.compute(*cloud_normals);
  // *** End of normal estimation
  // *** Plane Estimation From Normals Start
  // Create the segmentation object
  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
//  seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
  seg.setModelType(pcl::SACMODEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  //y가 z
//  const Eigen::Vector3f z_axis(0,-1.0,0);
//  seg.setAxis(z_axis);
//  seg.setEpsAngle(seg_setEpsAngle_);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations (seg_setMaxIterations_);
  seg.setDistanceThreshold(seg_setDistanceThreshold_);
  seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
//  seg.setProbability(seg_probability_);

  seg.setInputCloud((*cloud).makeShared());
  seg.setInputNormals(cloud_normals);
  seg.segment(*inliers, *coefficients);

  std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
  if (inliers->indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  // *** End of Plane Estimation
  // *** Plane Estimation Start
  // Create the segmentation object
/*  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  //seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PLANE);
//  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  //y가 z
  const Eigen::Vector3f z_axis(0,-1.0,0);
  seg.setAxis(z_axis);
  seg.setEpsAngle(seg_setEpsAngle_);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations (seg_setMaxIterations_);
  seg.setDistanceThreshold(seg_setDistanceThreshold_);

  seg.setInputCloud((*cloud).makeShared());
  seg.segment(*inliers, *coefficients);

  std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
  if (inliers->indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
  }
  // *** End of Plane Estimation
*/
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  // Extrat the inliers
  extract.setInputCloud(cloud);
  extract.setIndices(inliers);

  pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

  if ((int)inliers->indices.size() > minPoints_)
//.........这里部分代码省略.........

void Sub8StereoHandler::generate_point_cloud(
                                             const sensor_msgs::ImageConstPtr &msg_l,
                                             const sensor_msgs::ImageConstPtr &msg_r,
                                             pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pcl_ptr, ros::Publisher &pc_pub) {
    // NOTE: Calls to this function will block until the Synchronizer is able to
    // secure
    //       the corresponding messages.
    float px, py, pz;
    cv_bridge::CvImagePtr cv_ptr_l;
    cv_bridge::CvImagePtr cv_ptr_r;
    cv_bridge::CvImagePtr cv_ptr_bgr;
    
    try {
        cv_ptr_l = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::MONO8);
        cv_ptr_r = cv_bridge::toCvCopy(msg_r, sensor_msgs::image_encodings::MONO8);
        
        // Used to reproject color onto pointcloud (We'll just use the left image)
        cv_ptr_bgr = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    cv::gpu::Stream pc_stream = cv::gpu::Stream();
    this->d_left.upload(cv_ptr_l->image);
    this->d_right.upload(cv_ptr_r->image);
    
    switch (this->matching_method) {
        case 0:
            if (this->d_left.channels() > 1 || this->d_right.channels() > 1) {
                // TODO: Add handle for these cases
                ROS_WARN("Block-matcher does not support color images");
            }
            this->bm(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // BM
        case 1:
            this->bp(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // BP
        case 2:
            this->csbp(this->d_left, this->d_right, this->d_disp, pc_stream);
            break; // CSBP
    }
    
    // cv::Mat h_disp, h_3d_img, h_point;
    
    cv::gpu::reprojectImageTo3D(this->d_disp, this->gpu_pdisp, this->Q_, 4,
                                pc_stream);
    this->gpu_pdisp.download(this->pdisp);
    
    for (int i = 0; i < this->pdisp.rows; i++) {
        // For future reference OpenCV is ROW-MAJOR
        for (int j = 0; j < this->pdisp.cols; j++) {
            cv::Point3f ptxyz = this->pdisp.at<cv::Point3f>(i, j);
            
            if (!isinf(ptxyz.x) && !isinf(ptxyz.y) && !isinf(ptxyz.z)) {
                // Reproject color points onto distances
                cv::Vec3b color_info = cv_ptr_bgr->image.at<cv::Vec3b>(i, j);
                pcl::PointXYZRGB pcl_pt;
                
                pcl_pt.x = ptxyz.x;
                pcl_pt.y = ptxyz.y;
                pcl_pt.z = ptxyz.z;
                
                uint32_t rgb = (static_cast<uint32_t>(color_info[2]) << 16 |
                                static_cast<uint32_t>(color_info[1]) << 8 |
                                static_cast<uint32_t>(color_info[0]));
                pcl_pt.rgb = *reinterpret_cast<float *>(&rgb);
                pcl_ptr->points.push_back(pcl_pt);
            }
        }
    }
    
    pcl_ptr->width = (int)pcl_ptr->points.size();
    pcl_ptr->height = 1;
    
    pcl::toROSMsg(*pcl_ptr, this->stereo_pc);
    this->stereo_pc.header.frame_id = "/stereo_front";
    pcl_ptr->clear();
    pc_pub.publish(this->stereo_pc);
}

  void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
  {
    ARUint8 *dataPtr;
    ARMarkerInfo *marker_info;
    int marker_num;
    int i, k, j;

    /* Get the image from ROSTOPIC
     * NOTE: the dataPtr format is BGR because the ARToolKit library was
     * build with V4L, dataPtr format change according to the 
     * ARToolKit configure option (see config.h).*/
#if ROS_VERSION_MINIMUM(1, 9, 0)
    try
    {
      capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what()); 
    }
    dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData;
#else
    try
    {
      capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException & e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what()); 
    }
    dataPtr = (ARUint8 *) capture_->imageData;
#endif

    // detect the markers in the video frame
    if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
    {
      argCleanup ();
      ROS_BREAK ();
    }

    arPoseMarkers_.markers.clear ();
    // check for known patterns
    for (i = 0; i < objectnum; i++)
    {
      k = -1;
      for (j = 0; j < marker_num; j++)
      {
        if (object[i].id == marker_info[j].id)
        {
          if (k == -1)
            k = j;
          else                  // make sure you have the best pattern (highest confidence factor)
          if (marker_info[k].cf < marker_info[j].cf)
            k = j;
        }
      }
      if (k == -1)
      {
        object[i].visible = 0;
        continue;
      }

      // calculate the transform for each marker
      if (object[i].visible == 0)
      {
        arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      else
      {
        arGetTransMatCont (&marker_info[k], object[i].trans,
                           object[i].marker_center, object[i].marker_width, object[i].trans);
      }
      object[i].visible = 1;

      double arQuat[4], arPos[3];

      //arUtilMatInv (object[i].trans, cam_trans);
      arUtilMat2QuatPos (object[i].trans, arQuat, arPos);

      // **** convert to ROS frame

      double quat[4], pos[3];

      pos[0] = arPos[0] * AR_TO_ROS;
      pos[1] = arPos[1] * AR_TO_ROS;
      pos[2] = arPos[2] * AR_TO_ROS;

      if (isRightCamera_) {
        pos[2] += 0; // -0.001704;
        pos[0] +=