void JointTrajectoryStreamer::streamingThread()
{
  int connectRetryCount = 1;

  ROS_INFO("Starting joint trajectory streamer thread");
  while (ros::ok())
  {
    ros::Duration(0.005).sleep();

    // automatically re-establish connection, if required
    if (connectRetryCount-- > 0)
    {
      ROS_INFO("Connecting to robot motion server");
      this->connection_->makeConnect();
      ros::Duration(0.250).sleep();  // wait for connection

      if (this->connection_->isConnected())
        connectRetryCount = 0;
      else if (connectRetryCount <= 0)
      {
        ROS_ERROR("Timeout connecting to robot controller.  Send new motion command to retry.");
        this->state_ = TransferStates::IDLE;
      }
      continue;
    }

    this->mutex_.lock();

    SimpleMessage msg, tmpMsg, reply;
        
    switch (this->state_)
    {
      case TransferStates::IDLE:
        ros::Duration(0.250).sleep();  //  slower loop while waiting for new trajectory
        break;

      case TransferStates::STREAMING:
        if (this->current_point_ >= (int)this->current_traj_.size())
        {
          ROS_INFO("Trajectory streaming complete, setting state to IDLE");
          this->state_ = TransferStates::IDLE;
          break;
        }

        if (!this->connection_->isConnected())
        {
          ROS_DEBUG("Robot disconnected.  Attempting reconnect...");
          connectRetryCount = 5;
          break;
        }

        tmpMsg = this->current_traj_[this->current_point_];
        msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
                 ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST
            
        ROS_DEBUG("Sending joint trajectory point");
        if (this->connection_->sendAndReceiveMsg(msg, reply, false))
        {
          ROS_INFO("Point[%d of %d] sent to controller",
                   this->current_point_, (int)this->current_traj_.size());
          this->current_point_++;
        }
        else
          ROS_WARN("Failed sent joint point, will try again");

        break;
      default:
        ROS_ERROR("Joint trajectory streamer: unknown state");
        this->state_ = TransferStates::IDLE;
        break;
    }

    this->mutex_.unlock();
  }

  ROS_WARN("Exiting trajectory streamer thread");
}

  virtual bool adaptAndPlan(const PlannerFn &planner,
                            const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest &req,
                            planning_interface::MotionPlanResponse &res,
                            std::vector<std::size_t> &added_path_index) const
  {
    ROS_DEBUG("Running '%s'", getDescription().c_str());

    // get the specified start state
    robot_state::RobotState start_state = planning_scene->getCurrentState();
    robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);

    // if the start state is otherwise valid but does not meet path constraints
    if (planning_scene->isStateValid(start_state, req.group_name) &&
        !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
    {
      ROS_INFO("Path constraints not satisfied for start state...");
      planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
      ROS_INFO("Planning to path constraints...");

      planning_interface::MotionPlanRequest req2 = req;
      req2.goal_constraints.resize(1);
      req2.goal_constraints[0] = req.path_constraints;
      req2.path_constraints = moveit_msgs::Constraints();
      planning_interface::MotionPlanResponse res2;
      // we call the planner for this additional request, but we do not want to include potential 
      // index information from that call
      std::vector<std::size_t> added_path_index_temp;
      added_path_index_temp.swap(added_path_index);
      bool solved1 = planner(planning_scene, req2, res2);
      added_path_index_temp.swap(added_path_index);
      
      if (solved1)
      {
        planning_interface::MotionPlanRequest req3 = req;
        ROS_INFO("Planned to path constraints. Resuming original planning request.");

        // extract the last state of the computed motion plan and set it as the new start state
        robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
        bool solved2 = planner(planning_scene, req3, res);
        res.planning_time_ += res2.planning_time_;

        if (solved2)
        {
          // since we add a prefix, we need to correct any existing index positions
          for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
            added_path_index[i] += res2.trajectory_->getWayPointCount();
          
          // we mark the fact we insert a prefix path (we specify the index position we just added)
          for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
            added_path_index.push_back(i);
          
          // we need to append the solution paths.
          res2.trajectory_->append(*res.trajectory_, 0.0);
          res2.trajectory_->swap(*res.trajectory_);
          return true;
        }
        else
          return false;
      }
      else
      {
        ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
        bool result = planner(planning_scene, req, res);
        res.planning_time_ += res2.planning_time_;
        return result;
      }
    }
    else
    {
      ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
      return planner(planning_scene, req, res);
    }
  }

 void 
   noFilterCallback (const sensor_msgs::PointCloud2ConstPtr &cloud)
 {
   pointCloudPublisher_.publish (cloud);
   ROS_DEBUG ("Self filter publishing unfiltered frame");
 }

void set_speed_callback(const drive::speed::ConstPtr& msg)
{
	driveChain.setMotorSpeeds((int)(msg->speedLeft * 1000000.0), (int)(msg->speedRight * 1000000.0));
  ROS_DEBUG("set_speed_callback(): setting speeds %.2f / %.2f according to message on topic", msg->speedLeft, msg->speedRight);
}

//.........这里部分代码省略.........
      ideal.push_back( makeRGBPoint(-w/2,w/2,0) );
      ideal.push_back( makeRGBPoint(w/2,w/2,0) );
      ideal.push_back( makeRGBPoint(w/2,-w/2,0) );
      ideal.push_back( makeRGBPoint(-w/2,-w/2,0) );

      /* get transformation */
      Eigen::Matrix4f t;
      pcl::estimateRigidTransformationSVD( marker, ideal, t );
      
      /* get final transformation */
      tf::Transform transform = tfFromEigen(t.inverse());
   
      // any(transform == nan)
      btMatrix3x3  m = transform.getBasis();
      btVector3    p = transform.getOrigin();
      bool invalid = false;
      for(int i=0; i < 3; i++)
        for(int j=0; j < 3; j++)
          invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0);

      for(int i=0; i < 3; i++)
          invalid = (invalid || isnan(p[i]));
       

      if(invalid)
        continue; 

      /* publish the marker */
      ar_pose::ARMarker ar_pose_marker;
      ar_pose_marker.header.frame_id = msg->header.frame_id;
      ar_pose_marker.header.stamp = msg->header.stamp;
      ar_pose_marker.id = object[i].id;

      ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX();
      ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY();
      ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ();

      ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX();
      ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY();
      ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ();
      ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW();

      ar_pose_marker.confidence = marker_info->cf;
      arPoseMarkers_.markers.push_back (ar_pose_marker);

      /* publish transform */
      if (publishTf_)
      {
	    broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name));
      }

      /* publish visual marker */

      if (publishVisualMarkers_)
      {
        btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
        btTransform m (btQuaternion::getIdentity (), markerOrigin);
        btTransform markerPose = transform * m; // marker pose in the camera frame

        tf::poseTFToMsg (markerPose, rvizMarker_.pose);

        rvizMarker_.header.frame_id = msg->header.frame_id;
        rvizMarker_.header.stamp = msg->header.stamp;
        rvizMarker_.id = object[i].id;

        rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
        rvizMarker_.ns = "basic_shapes";
        rvizMarker_.type = visualization_msgs::Marker::CUBE;
        rvizMarker_.action = visualization_msgs::Marker::ADD;
        switch (i)
        {
          case 0:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 1.0f;
            rvizMarker_.color.a = 1.0;
            break;
          case 1:
            rvizMarker_.color.r = 1.0f;
            rvizMarker_.color.g = 0.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
            break;
          default:
            rvizMarker_.color.r = 0.0f;
            rvizMarker_.color.g = 1.0f;
            rvizMarker_.color.b = 0.0f;
            rvizMarker_.color.a = 1.0;
        }
        rvizMarker_.lifetime = ros::Duration ();

        rvizMarkerPub_.publish (rvizMarker_);
        ROS_DEBUG ("Published visual marker");
      }
    }
    arMarkerPub_.publish (arPoseMarkers_);
    ROS_DEBUG ("Published ar_multi markers");
  }

void EncoderOdometryNode::publishEstimatedOdomMsg(
const ros::TimerEvent& timer_event) {
    // Check to make sure that we have some counts for both the left and right
    // encoders
    if (!left_encoder_num_ticks_curr || !right_encoder_num_ticks_curr) {
        // If we don't have either the left or right encoder ticks, just return
        return;
    }

    // Checks if we've made an estimate before
    if (!left_encoder_num_ticks_prev || !right_encoder_num_ticks_prev) {
        ROS_DEBUG("Got first ticks from encoders");
        // If we haven't, save the current state as the previous state
        // and just return. The first actual estimate will be made at the
        // next call to this function
        left_encoder_num_ticks_prev  = left_encoder_num_ticks_curr;
        right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;

        // Save the time so that next time we get ticks, we can
        // generate a state estimate
        last_estimate.header.stamp = ros::Time::now();

        return;
    }

    // All math taken from:
    // https://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/16311/www/s07/labs/NXTLabs/Lab%203.html

    double dt = ros::Time::now().toSec() - last_estimate.header.stamp.toSec();

    // left and right wheel velocities (in meters/s)
    int left_encoder_ticks =
    *left_encoder_num_ticks_curr - *left_encoder_num_ticks_prev;
    int right_encoder_ticks =
    *right_encoder_num_ticks_curr - *right_encoder_num_ticks_prev;
    double v_l =
    (left_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;
    double v_r =
    (right_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;

    // linear and angular velocity
    double v = (v_r + v_l) / 2;
    double w = (v_r - v_l) / wheelbase_length;

    // x and y velocity
    double x_vel = v * cos(w);
    double y_vel = v * sin(w);

    // position
    double prev_x   = last_estimate.pose.pose.position.x;
    double prev_y   = last_estimate.pose.pose.position.y;
    double prev_yaw = tf::getYaw(last_estimate.pose.pose.orientation);

    double k_00 = v * cos(prev_yaw);
    double k_01 = v * sin(prev_yaw);
    double k_02 = w;
    double k_10 = v * cos(prev_yaw + dt / 2 * k_02);
    double k_11 = v * sin(prev_yaw + dt / 2 * k_02);
    double k_12 = w;
    double k_20 = v * cos(prev_yaw + dt / 2 * k_12);
    double k_21 = v * sin(prev_yaw + dt / 2 * k_12);
    double k_22 = w;
    double k_30 = v * cos(prev_yaw + dt * k_22);
    double k_31 = v * sin(prev_yaw + dt * k_22);
    double k_32 = w;

    double x   = prev_x + (dt / 6) * (k_00 + 2 * (k_10 + k_20) + k_30);
    double y   = prev_y + (dt / 6) * (k_01 + 2 * (k_11 + k_21) + k_31);
    double yaw = prev_yaw + (dt / 6) * (k_02 + 2 * (k_12 + k_22) + k_32);

    // Update our estimate
    last_estimate.pose.pose.position.x = x;
    last_estimate.pose.pose.position.y = y;
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(yaw),
                          last_estimate.pose.pose.orientation);
    last_estimate.twist.twist.linear.x  = x_vel;
    last_estimate.twist.twist.linear.y  = y_vel;
    last_estimate.twist.twist.angular.z = w;
    last_estimate.header.stamp          = ros::Time::now();
    last_estimate.header.frame_id       = odom_frame_id;

    // Save the current number of ticks as the "previous" number for the
    // next time we estimate
    left_encoder_num_ticks_prev  = left_encoder_num_ticks_curr;
    right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;

    // Publish our estimate
    odom_estimate_publisher.publish(last_estimate);
}

long long
hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout)
{
  ROS_DEBUG("Entering calcLatency.");

  if (!portOpen())
    HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");

  offset_ = 0;

  uint64_t comp_time = 0;
  uint64_t laser_time = 0;
  long long diff_time = 0;
  long long drift_time = 0;
  long long tmp_offset1 = 0;
  long long tmp_offset2 = 0;

  int count = 0;
 
  sendCmd("TM0",timeout);
  count = 100;

  for (int i = 0; i < count;i++)
  {
    usleep(1000);
    sendCmd("TM1",timeout);
    comp_time = timeHelper();
    try 
    {
      laser_time = readTime();

      diff_time = comp_time - laser_time;

      tmp_offset1 += diff_time / count;
    } catch (hokuyo::RepeatedTimeException &e)
    {
      // We expect to get Repeated Time's when hammering on the time server
      continue;
    }
  }

  uint64_t start_time = timeHelper();
  usleep(5000000);
  sendCmd("TM1;a",timeout);
  sendCmd("TM1;b",timeout);
  comp_time = timeHelper();
  drift_time = comp_time - start_time;
  laser_time = readTime() + tmp_offset1;
  diff_time = comp_time - laser_time;
  double drift_rate = double(diff_time) / double(drift_time);

  sendCmd("TM2",timeout);
  
  if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0)
    HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation");

  hokuyo::LaserScan scan;

  count = 200;
  for (int i = 0; i < count;i++)
  {
    try
    {
      serviceScan(scan, 1000);
    } catch (hokuyo::CorruptedDataException &e) {
      continue;
    }

    comp_time = scan.system_time_stamp;
    drift_time = comp_time - start_time;
    laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate);
    diff_time = laser_time - comp_time;

    tmp_offset2 += diff_time / count;
  }

  offset_ = tmp_offset2;

  stopScanning();
  
  ROS_DEBUG("Leaving calcLatency.");

  return offset_;
}

// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {


    std::clock_t starttime=std::clock();
    if(reset_request_) {
        int numLevels = 3;
        int nodeDistance = 2;
        marker_id =0;
        time_of_last_transform_= ros::Time();
        last_batch_update_=std::clock();
        delete optimizer_;
        optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
        graph_.clear();
        matches_.clear();
        freshlyOptimized_= false;
        reset_request_ = false;
    }


    if (new_node.feature_locations_2d_.size() <= 5) {
        ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
        return false;
    }

    //set the node id only if the node is actually added to the graph
    //needs to be done here as the graph size can change inside this function
    new_node.id_ = graph_.size();

    //First Node, so only build its index, insert into storage and add a
    //vertex at the origin, of which the position is very certain
    if (graph_.size()==0) {
        new_node.buildFlannIndex(); // create index so that next nodes can use it

//        graph_.insert(make_pair(new_node.id_, new_node)); //store
        graph_[new_node.id_] = new_node;
        optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
        return true;
    }


    unsigned int num_edges_before = optimizer_->edges().size();


    std::vector<cv::DMatch> initial_matches;
    ROS_DEBUG("Graphsize: %d", (int) graph_.size());
    marker_id = 0; //overdraw old markers


    Eigen::Matrix4f ransac_trafo, final_trafo;

    bool edge_added_to_base;
    std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
    int id_of_id = vertices_to_comp.size() -1;
    for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
        initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
        //initial_matches = processNodePair(new_node, graph_rit->second);
        //What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
    }
    id_of_id++;//set back to last valid id

    if (optimizer_->edges().size() > num_edges_before) {

        new_node.buildFlannIndex();
        graph_[new_node.id_] = new_node;

        ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());

        optimizeGraph();
        //make the transform of the last node known
        ros::TimerEvent unused;
        broadcastTransform(unused);
        visualizeGraphEdges();
        visualizeGraphNodes();
        visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
    } else {
        ROS_WARN("##### could not find link for PointCloud!");
        matches_.clear();
    }
    ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC  <<"sec");
    return (optimizer_->edges().size() > num_edges_before);
}

bool ControllerManager::unloadController(const std::string &name)
{
  ROS_DEBUG("Will unload controller '%s'", name.c_str());

  // lock the controllers
  boost::recursive_mutex::scoped_lock guard(controllers_lock_);

  // get reference to controller list
  int free_controllers_list = (current_controllers_list_ + 1) % 2;
  while (ros::ok() && free_controllers_list == used_by_realtime_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  std::vector<ControllerSpec>
    &from = controllers_lists_[current_controllers_list_],
    &to = controllers_lists_[free_controllers_list];
  to.clear();

  // Transfers the running controllers over, skipping the one to be removed and the running ones.
  bool removed = false;
  for (size_t i = 0; i < from.size(); ++i)
  {
    if (from[i].info.name == name){
      if (from[i].c->isRunning()){
        to.clear();
        ROS_ERROR("Could not unload controller with name %s because it is still running",
                  name.c_str());
        return false;
      }
      removed = true;
    }
    else
      to.push_back(from[i]);
  }

  // Fails if we could not remove the controllers
  if (!removed)
  {
    to.clear();
    ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
              name.c_str());
    return false;
  }

  // Destroys the old controllers list when the realtime thread is finished with it.
  ROS_DEBUG("Realtime switches over to new controller list");
  int former_current_controllers_list_ = current_controllers_list_;
  current_controllers_list_ = free_controllers_list;
  while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
    if (!ros::ok())
      return false;
    usleep(200);
  }
  ROS_DEBUG("Destruct controller");
  from.clear();
  ROS_DEBUG("Destruct controller finished");

  ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
  return true;
}

bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
                                         const std::vector<std::string>& stop_controllers,
                                         int strictness)
{
  if (!stop_request_.empty() || !start_request_.empty())
    ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");

  if (strictness == 0){
    ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",
             controller_manager_msgs::SwitchController::Request::STRICT,
             controller_manager_msgs::SwitchController::Request::BEST_EFFORT);
    strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT;
  }

  ROS_DEBUG("switching controllers:");
  for (unsigned int i=0; i<start_controllers.size(); i++)
    ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
  for (unsigned int i=0; i<stop_controllers.size(); i++)
    ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());

  // lock controllers
  boost::recursive_mutex::scoped_lock guard(controllers_lock_);

  controller_interface::ControllerBase* ct;
  // list all controllers to stop
  for (unsigned int i=0; i<stop_controllers.size(); i++)
  {
    ct = getControllerByName(stop_controllers[i]);
    if (ct == NULL){
      if (strictness ==  controller_manager_msgs::SwitchController::Request::STRICT){
        ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
                  stop_controllers[i].c_str());
        stop_request_.clear();
        return false;
      }
      else{
        ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
                  stop_controllers[i].c_str());
      }
    }
    else{
      ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
                stop_controllers[i].c_str());
      stop_request_.push_back(ct);
    }
  }
  ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());

  // list all controllers to start
  for (unsigned int i=0; i<start_controllers.size(); i++)
  {
    ct = getControllerByName(start_controllers[i]);
    if (ct == NULL){
      if (strictness ==  controller_manager_msgs::SwitchController::Request::STRICT){
        ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
                  start_controllers[i].c_str());
        stop_request_.clear();
        start_request_.clear();
        return false;
      }
      else{
        ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
                  start_controllers[i].c_str());
      }
    }
    else{
      ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
                start_controllers[i].c_str());
      start_request_.push_back(ct);
    }
  }
  ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());

  // Do the resource management checking
  std::list<hardware_interface::ControllerInfo> info_list;
  std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
  for (size_t i = 0; i < controllers.size(); ++i)
  {
    bool in_stop_list  = false;

    for(size_t j = 0; j < stop_request_.size(); j++)
      in_stop_list = in_stop_list || (stop_request_[j] == controllers[i].c.get());

    bool in_start_list = false;
    for(size_t j = 0; j < start_request_.size(); j++)
      in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get());

    bool add_to_list = controllers[i].c->isRunning();
    if (in_stop_list)
      add_to_list = false;
    if (in_start_list)
      add_to_list = true;

    if (add_to_list)
      info_list.push_back(controllers[i].info);
  }

  bool in_conflict = robot_hw_->checkForConflict(info_list);
  if (in_conflict)
  {
//.........这里部分代码省略.........

/// \brief callback for setting models joints states
bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req,
                           pr2_gazebo_plugins::SetModelsJointsStates::Response &res)
{
  ROS_DEBUG("setModelsJointsStates");
  return true;
}

void GazeboRosControllerManager::ReadPr2Xml()
{

  std::string urdf_param_name;
  std::string urdf_string;
  // search and wait for robot_description on param server
  while(urdf_string.empty())
  {
    ROS_DEBUG("gazebo controller manager plugin is waiting for urdf: %s on the param server.", this->robotParam.c_str());
    if (this->rosnode_->searchParam(this->robotParam,urdf_param_name))
    {
      this->rosnode_->getParam(urdf_param_name,urdf_string);
      ROS_DEBUG("found upstream\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
    }
    else
    {
      this->rosnode_->getParam(this->robotParam,urdf_string);
      ROS_DEBUG("found in node namespace\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str());
    }
    usleep(100000);
  }
  ROS_DEBUG("gazebo controller manager got pr2.xml from param server, parsing it...");

  // 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());
  }
  else
  {
    //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->hw_.addActuator(pr2_actuator);
    }

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

    for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
      this->cm_->state_->joint_states_[i].calibrated_ = fake_calibration_;
  }
}

void DecosSensorRing::ReadSensorRingValues()
{
    int r = 1;

    if(fd == P_ERROR)
    {
        ROS_ERROR("Port not opened. Abort!");
        return;
    }

    int totalrx = readData(fd, rxbuf, 64);
    rxbuf[totalrx] = 0;

    //ROS_INFO("RX: <%s>", rxbuf);

    for (int i=0; i<totalrx; i++)
    {
        unsigned char z = rxbuf[i];

                                // one frame = [I|%03i|%03i|%03i|%03i|%03i|%03i|U|%04i]
        switch (rxstate)        // this is our state machine to read one full frame of values
        {
            case 0: 
                if (z=='[') rxstate++;
                break;
            
            case 1: 
                if (z=='I') 
                    rxstate++;
                else
                    rxstate = 0;
                break;
            
            case 2:
            case 4:
            case 6:
            case 8:
            case 10:
            case 12:
            case 14:
            case 16:
                if (z=='|') 
                {
                    rxstate++;
                    rxcount = 0;
                }
                else
                    rxstate = 0;
                break;
            
            case 3:
            case 5:
            case 7:
            case 9:
            case 11:
            case 13: 
                if (CollectValue(z, 3)==0)  // done with one value
                {
                    int idx = (int)(rxstate-3)/2;
                    if ((idx>=0) && (idx<6))
                        sensorSet.IR[idx] = atoi(collectedvalue);
                }            
                break;
                        
            case 15:
                if (z=='U') 
                    rxstate++;
                else
                    rxstate = 0;
                break;

            case 17: 
                if (CollectValue(z, 4)==0)  // got value?
                {
                    sensorSet.US = atoi(collectedvalue);
                    sensorSet.counter++;
                    sensorSet.valid = true;
                    rxstate = 0;
                }            
                break;

            case 18:    // in case a fail occured at step 17 in CollectValue
                rxstate = 0;
                break;                

        }
    }

    if (sensorSet.valid)
    {
        ROS_DEBUG("Set: %03i, %03i, %03i, %03i, %03i, %03i | %04i", sensorSet.IR[0], sensorSet.IR[1], sensorSet.IR[2], sensorSet.IR[3], sensorSet.IR[4], sensorSet.IR[5], sensorSet.US);

        sensorSet.valid = false;

        decos_sensorring::SensorValues sv;
        sv.IR1 = sensorSet.IR[0];
        sv.IR2 = sensorSet.IR[1];
        sv.IR3 = sensorSet.IR[2];
        sv.IR4 = sensorSet.IR[3];
        sv.IR5 = sensorSet.IR[4];
//.........这里部分代码省略.........

//returns a single point cloud colored objects ,
void CObjectCandidateExtraction::extractObjectCandidates(pcl::PointCloud<
		pcl::PointXYZRGB> &point_cloud,
		pcl::PointCloud<pcl::PointXYZRGBNormal> &planar_point_cloud,
		std::vector<structPlanarSurface> &hierarchyPlanes) {

	ROS_DEBUG("[extractObjectCandidates] extractObjectCandidates started ...");
	ros::Time start, start2, finish, finish2, start3, finish3;
	start = ros::Time::now();
	std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredObjects;
	std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredPlanes;
	pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_RGB;
	pcl::PointIndices inliers, inliersObjects;
	pcl::PointCloud<pcl::PointXYZRGBNormal> total_point_cloud,
			point_cloud_normal;
	//std::vector<structPlanarSurface> hierarchyPlanes;
	//	std::vector<bool> delete_total_point_cloud;


	ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares started ... ");
	//	total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares(
	//			point_cloud, 0.02f); //0.02f works good with 0.008 subsampling//0.01 veryfast but not good
	total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares(
			point_cloud, 0.01f); //0.02f works good //0.01 veryfast but not good
	ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares done ... ");

	hierarchyPlanes = horizontalSurfaceExtractor.extractMultiplePlanes(
			point_cloud_normal, planar_point_cloud, clusteredPlanes, 2);

	if (hierarchyPlanes.empty()) {
		return;
	}

	for (unsigned int indexMaxPointsClusteredPlane = 0; indexMaxPointsClusteredPlane
			< hierarchyPlanes.size(); indexMaxPointsClusteredPlane++) {
		//planar_point_cloud = hierarchyPlanes[indexMaxPointsClusteredPlane].pointCloud;
		//---------------------------------------------
		// get plane hull
		start2 = ros::Time::now();

		pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_hull;
		//	pcl::ConvexHull2D<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> chull;
		//	chull.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> > (planar_point_cloud));
		//	chull.reconstruct (cloud_hull);

		cloud_hull = hierarchyPlanes[indexMaxPointsClusteredPlane].convexHull; //pointCloud;

		//ROS_DEBUG("[%s/extractObjectCandidates] dZmax %f dZmin %f",this->nodeName.c_str(),dZmax,dZmin );
		pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_objects;
		cloud_objects.header = total_point_cloud.header;

		bool reject = false;
		unsigned int total_point_cloud_size = total_point_cloud.points.size();

//		int chunk = total_point_cloud_size / 4;
		omp_set_num_threads(4);
#pragma omp parallel shared (total_point_cloud,chunk) private(j)
		{
#pragma omp for schedule(dynamic,chunk) nowait
			for (unsigned int j = 0; j < total_point_cloud_size; ++j) {
				//here check also whether point below upper plane is
				if (toolBox.pointInsideConvexHull2d(cloud_hull,
						total_point_cloud.points[j])
						&& total_point_cloud.points[j].z
								> (toolBox.getNearestNeighborPlane(
										hierarchyPlanes[indexMaxPointsClusteredPlane],
										total_point_cloud.points[j]).z + this->threshold_point_above_lower_plane))
				//(dZmax		+ this->threshold_point_above_lower_plane))//(dZmax+this->threshold_point_above_lower_plane)) //dZmax //dZmax-(dZmax*0.005)) //(((dZmax+dZmin)/2)+dZmax)/2)
				{
					reject = false;
					for (unsigned int iterUpperPlanes = 0; iterUpperPlanes
							< hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces.size(); iterUpperPlanes++) {
						/*
						if (toolBox.pointInsideConvexHull2d(
								hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].convexHull,
								total_point_cloud.points[j])
								&& total_point_cloud.points[j].z
										> hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].plane_height
												+ THRESHOLD_POINT_ABOVE_UPPER_PLANE) //a little bit over surface(upperplane) in order to get a cube as a cube and not without uppersurface
						{
							reject = true;
							break;
						}*/
					}

					if (!reject) {
						cloud_objects.points.push_back(
								total_point_cloud.points[j]);
					}
				}
			}
		} //PRAGMA

		cloud_objects.width = cloud_objects.points.size();
		cloud_objects.height = 1;
		//---------------------------------------------

		ROS_DEBUG ("[extractObjectCandidates] Number of object point candidates: %d.", (int)cloud_objects.points.size());

		if ((unsigned int) cloud_objects.points.size() > 0) {
//.........这里部分代码省略.........

  /** Dynamic reconfigure callback
   *
   *  Called immediately when callback first defined. Called again
   *  when dynamic reconfigure starts or changes a parameter value.
   *
   *  @param newconfig new Config values
   *  @param level bit-wise OR of reconfiguration levels for all
   *               changed parameters (0xffffffff on initial call)
   **/
  void Camera1394Driver::reconfig(Config &newconfig, uint32_t level)
  {
    // Do not run concurrently with poll().  Tell it to stop running,
    // and wait on the lock until it does.
    reconfiguring_ = true;
    boost::mutex::scoped_lock lock(mutex_);
    ROS_DEBUG("dynamic reconfigure level 0x%x", level);

    // resolve frame ID using tf_prefix parameter
    if (newconfig.frame_id == "")
      newconfig.frame_id = "camera";
    std::string tf_prefix = tf::getPrefixParam(priv_nh_);
    ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
    newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);

    if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
      {
        // must close the device before updating these parameters
        closeCamera();                  // state_ --> CLOSED
      }

    if (state_ == Driver::CLOSED)
      {
        // open with new values
        openCamera(newconfig);
      }

    if (config_.camera_info_url != newconfig.camera_info_url)
      {
        if (!validateConfig(newconfig))
          {
            // new URL not valid, use the old one
            newconfig.camera_info_url = config_.camera_info_url;
          }
      }

    if (state_ != Driver::CLOSED)       // openCamera() succeeded?
      {
        // configure IIDC features
        if (level & Levels::RECONFIGURE_CLOSE)
          {
            // initialize all features for newly opened device
            if (false == dev_->features_->initialize(&newconfig))
              {
                ROS_ERROR_STREAM("[" << camera_name_
                                 << "] feature initialization failure");
                closeCamera();          // can't continue
              }
          }
        else
          {
            // update any features that changed
            // TODO replace this with a dev_->reconfigure(&newconfig);
            dev_->features_->reconfigure(&newconfig);
          }
      }

    config_ = newconfig;                // save new parameters
    reconfiguring_ = false;             // let poll() run again

    ROS_DEBUG_STREAM("[" << camera_name_
                     << "] reconfigured: frame_id " << newconfig.frame_id
                     << ", camera_info_url " << newconfig.camera_info_url);
  }

void Commander::keyLoop() {
	char c;

	// get the console in raw mode                                                              
	tcgetattr(kfd, &cooked);
	memcpy(&raw, &cooked, sizeof(struct termios));
	raw.c_lflag &=~ (ICANON | ECHO);
	// Setting a new line, then end of file                         
	raw.c_cc[VEOL] = 1;
	raw.c_cc[VEOF] = 2;
	tcsetattr(kfd, TCSANOW, &raw);

	puts("Reading from keyboard");
	puts("---------------------------");
	puts("Use arrow keys to move the ROBOT.");


	for(;;) {
		// get the next event from the keyboard  
		LunarNXT::Order msg;
  	
		if(read(kfd, &c, 1) < 0) {
			perror("read():");
			exit(-1);
		}

		ROS_DEBUG("value: 0x%02X\n", c);
  
		switch(c) {
			case KEYCODE_Q:
				ROS_INFO("Turn Left !");
				msg.sOrder = "turn_l";
			        break;
			case KEYCODE_D:
				ROS_INFO("Turn Right !");
				msg.sOrder = "turn_r";
				break;
			case KEYCODE_Z:
				ROS_INFO("Go !");
				msg.sOrder = "go";
			        break;
			case KEYCODE_S:
				ROS_INFO("Back !");
				msg.sOrder = "back";
				break;
			case KEYCODE_A:
				ROS_INFO("Stop !");
				msg.sOrder = "stop";
				break;
			case KEYCODE_L:
				ROS_INFO("LineLauncher");
				msg.sOrder = "launch";
				break;
			case KEYCODE_M:
				ROS_INFO("LineUnLauncher");
				msg.sOrder = "unlaunch";
				break;
		}

		this->commander_pub.publish(msg);
	}
	return;
}