//================================================================
    // Close the gripper at the specified rate to the distance
    // specified
    //================================================================
    void closeToPosition(ros::Rate rate, double move_gripper_distance, 
                         double gripper_position)
    {
      // Get location of gripper currently 
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;

      // Continue until position is achieved, ros cancel, or if
      // the pressure approaches something dangerous
      while (current_gripper_position > gripper_position 
            && pressure_max < 500 
            && current_gripper_position > 0.0 
            && ros::ok())
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }
Esempio n. 2
0
position_tracker::Position take_step(int action, ros::Rate loop_rate, ros::ServiceClient client)
{  
  printf("p take action\n");
  cout << "taking action " <<action << endl;
  /*Execute the Action */
  if(action==LEFT){
    left(client);
  }
  else if(action==FORWARD){
    forward(client);
  }
  else{
    right(client);
  }
  
  cout << "took an action" << endl;
  ros::spinOnce();
  loop_rate.sleep();
   
  //check to make sure this observation is ok
  
  position_tracker::Position observation= getObservation();
  return observation;
  /*
  cout << "going to convert"<< endl;
  convertObservation(observation);
  cout << "converted"<<endl;

  this_reward_observation.reward= calculateReward(observation);
  this_reward_observation.terminal=checkTerminal(observation);
  
    cout << "we took a step" << endl;
    return &this_reward_observation;
  */
}
    //================================================================
    // Close gripper to specified pressure
    //================================================================
    void closeToPressure(ros::Rate rate, int desired_pressure, 
                         double move_gripper_distance)
    {
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;
      int pressure_min = 0;

      while (pressure_min < desired_pressure 
             && pressure_max < 500
             && ros::ok()
             && current_gripper_position > 0.0)
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }
void ObjRecInterface::recognize_objects() 
{
  while(ros::ok() && !time_to_stop_) {
    // Don't hog the cpu
    ros::Duration(0.03).sleep();

    // Scope for syncrhonization
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
    {
      // Lock the buffer mutex
      boost::mutex::scoped_lock buffer_lock(buffer_mutex_);

      // Continue if the cloud is empty
      static ros::Rate warn_rate(1.0);
      if(clouds_.empty()) {
        ROS_WARN("Pointcloud buffer is empty!");
        warn_rate.sleep();
        continue;
      }

      ros::Time time_back, time_front;
      time_back.fromNSec(clouds_.back()->header.stamp * 1000);
      time_front.fromNSec(clouds_.back()->header.stamp * 1000);
      ROS_INFO_STREAM("Computing objects from "
          <<scene_points_->GetNumberOfPoints()<<" points "
          <<"between "<<(ros::Time::now() - time_back)
          <<" to "<<(ros::Time::now() - time_front)<<" seconds after they were acquired.");

      // Copy references to the stored clouds
      cloud_full->header = clouds_.front()->header;

      while(!clouds_.empty()) {
        *cloud_full += *(clouds_.front());
        clouds_.pop();
      }
    }

    objrec_msgs::RecognizedObjects objects_msg = do_recognition(cloud_full);

    // Publish the visualization markers
    this->publish_markers(objects_msg);

    // Publish the recognized objects
    objects_pub_.publish(objects_msg);

    // Publish the points used in the scan, for debugging
    /**
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr foreground_points_pcl(new pcl::PointCloud<pcl::PointXYZRGB>());
    foreground_points_pcl->header = cloud->header;
    for(unsigned int i=0; i<foreground_points->GetNumberOfPoints(); i++) {
      double point[3];
      foreground_points->GetPoint(i,point);
      foreground_points_pcl->push_back(pcl::PointXYZ(point[0]/1000.0,point[1]/1000.0,point[2]/1000.0));
    }
    foreground_points_pub_.publish(foreground_points_pcl);
    **/
  }
}
Esempio n. 5
0
void turn(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count = 0;
	while(ros::ok()){
		count++;
    	out.publish(msg);
		rate.sleep();
		if (count == 40)break; //after 45 updates break
	}	
}	
Esempio n. 6
0
void moveForward(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count =0; //number of updates
    while(ros::ok()){
		out.publish(msg);		
    	rate.sleep();
		count++;
		if (count == 10) break; //after a second break
    }
}
void Node::mapPublishThread(ros::Rate rate)
{
  while(ros::ok()) {
    {
#ifdef USE_HECTOR_TIMING
      hector_diagnostics::TimingSection section("map publish");
#endif
      publishMap();
    }
    rate.sleep();
  }
}
    // TODO needs some love
    void run() {
        while(ros::ok()) {
            current_time = ros::Time::now();
            double dt = (current_time - last_time).toSec();
            // TODO these predicst and measure variables are not something that I understand
            if (predict) {
                prediction(dt);
                last_time = current_time;
                predict = false;

                if (measure) {
                    update_estimated_measurement();
                    weighting();
                    measure = false;
                }

                Normalization();
                resample();
            }

            if (visualization_enabled) {
                if (publish_particles) {
                    particles_pub.publish(loc_viz.get_particle_marker(particle_state));
                }
                if (publish_robot) {
                    robot_pub.publish(
                        loc_viz.get_robot_marker(std::vector<ras::sensor_info>(), x, y, theta, visualization_use_distance));
                }
                if (publish_thickened_walls) {
                    wall_cell_pub.publish(loc_viz.get_thick_wall_grid_cells());
                }
                if (publish_walls) {
                    grid_cell_pub.publish(loc_viz.get_wall_grid_cells());
                }
                if (publish_path) {
                    point_path_pub.publish(loc_viz.add_to_path(x, y));
                }
            }

            //Publish Geometry msg of Predicted postion
            geometry_msgs::Pose2D msg;
            msg.x = x;
            msg.y = y;
            msg.theta = theta;
            position_pub.publish(msg);

            //Use if add a subscription(Add as good measure)
            ros::spinOnce();
            //Delays untill it is time to send another message
            rate->sleep();
            last_time = current_time;
        }
    }
Esempio n. 9
0
position_tracker::Position start_environment(ros::Rate loop_rate)
{
  /* see where the robot is.  And then send that position*/
  
  ros::spinOnce();
  loop_rate.sleep();
  
  position_tracker::Position observation= getObservation();
  
  cout << observation.x << " " << observation.y << " " << observation.theta << endl;  
  return observation;
  
}
    //================================================================ 
    // Closes the gripper until a pressure is achieved, 
    // then opens again at the same rate the gripper closed at
    //================================================================
    void squeeze(ros::Rate rate, double move_gripper_distance)
    {
      int previous_pressure_max = 0; 
      int pressure_max = 0;
      int no_motion_counter = 0;

      // Close 
      while (pressure_max < SqueezePressureContact && ros::ok()
             && no_motion_counter < 250 
	           && simple_gripper->getGripperLastPosition() > 0.0)
      {
        previous_pressure_max = pressure_max;
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        simple_gripper->closeByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
   
      // Store the gripper position when max pressure is achieved
      gripper_max_squeeze_position = simple_gripper->getGripperLastPosition();

      // Open - 10 and not 0 because the values will drift
      while (pressure_max > 10 && ros::ok() 
	     && simple_gripper->getGripperLastPosition() < 0.08)
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->openByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }
    //================================================================
    // Open gripper by the rate and position specified.
    // This is necessary to keep opening the gripper until 
    // the bioTacs do not report any pressure 
    //================================================================
    void openUntilNoContact(ros::Rate rate, double gripper_position)
    {
      int pressure_max = LightPressureContact + 50;

      while (pressure_max > LightPressureContact && ros::ok())
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->open2Position(gripper_position);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }
Esempio n. 12
0
int main(int argc, char** argv)
{


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

	ROS_INFO("Started Odroid-C1 Button Blink Node");

	wiringPiSetup ();
	pinMode(LED, OUTPUT);


	ros::NodeHandle n;

	ros::Subscriber sub = n.subscribe("led_blink",10,blink_callback);
  	ros::Publisher chatter_pub = n.advertise<std_msgs::Bool>("led_blink", 10);


	std_msgs::Bool on = true;
	std_msgs::Bool off = false;


 	while (ros::ok())
  	{

	        if (digitalRead(butPin)) // Button is released if this returns 1
		{
	
			chatter_pub.publish(on);

		}
		else
		{

			chatter_pub.publish(off);

		}

		ros::spinOnce();

    		loop_rate.sleep();

	}

	



}
 void run(){
     
     RNG rng(12345);
     while(ros::ok()) {
         current_time = ros::Time::now();
         //computer odemetry using integration
         double dt = (current_time - last_time).toSec();
         //Prediction model
         Prediction(dt);
         //Measurement model
         EstimateMeasurement_Update();
         RealMeasurement_Update();
         Innovation();
         ReSampling();
         Normalization();
         
         // Show all the particles on the map
         image = imread(PATH,CV_LOAD_IMAGE_COLOR);
         
         Scalar color = Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255));
         for(int i=0;i<Particles;++i){
             particle_position = Point(particle_state[1][i]*100+5,particle_state[0][i]*100+5);
             circle(image,particle_position,4,color,-1,8,0);
         }
         
         // Show the final predicted position on the map
         circle(postion_image,position,4,color,-1,8,0);
         
         //Show odometry localization on the map
         namedWindow( OPENCV_WINDOW, CV_WINDOW_NORMAL );// Create a window for display.
         imshow( OPENCV_WINDOW, image );                  // Show our map inside it.
         namedWindow( OPENCV_WINDOW1, CV_WINDOW_NORMAL );// Create a window for display.
         imshow( OPENCV_WINDOW1, postion_image);
         
         //Publish Geometry msg of Predicted postion
         msg.x = x;
         msg.y = y;
         msg.theta = theta;
         position_pub.publish(msg);
         
         waitKey(3);                                 // Wait for a keystroke in the window
         last_time = current_time;
         //Use if add a subscription(Add as good measure)
         ros::spinOnce();
         //Delays untill it is time to send another message
         rate->sleep();
     }
 }
Esempio n. 14
0
//Moves forward for 3 seconds, moves backwards 3 seconds
//Press Enter for emergency land
void controller::testMove(ros::Rate loop_rate, ros::NodeHandle node)
{
	if (inFlight == 0)
	{
		controller::sendTakeoff(node);
		inFlight = 1;
	}

	start_time = (double)ros::Time::now().toSec();
	cout << "Started Time: " << start_time << "\n" << endl;

	linebufferedController(false);
	echoController(false);

	while ((double)ros::Time::now().toSec() < start_time + test_flight_time)
	{

		if ((double)ros::Time::now().toSec() < start_time + test_flight_time / 2)
		{
			controller::setMovement(5, 0, 0);
			controller::sendMovement(node);

		}
		else
		{
			controller::setMovement(-5,0, 0);
			controller::sendMovement(node);
		}

		ros::spinOnce();
		loop_rate.sleep();
		if (iskeypressed(500) && cin.get() == '\n') break;
	}

	controller::resetTwist();
	controller::sendMovement(node);
	controller::sendLand(node);

	echoController();
	linebufferedController();
}
void
computeBackgroundCloud (int frames, float voxel_size, int sr_conf_threshold, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud)
{
  std::cout << "Background acquisition..." << std::flush;

  // Create background cloud:
  PointCloudT::Ptr cloud_to_process(new PointCloudT);
  background_cloud->header = cloud->header;
  for (unsigned int i = 0; i < frames; i++)
  {
    *cloud_to_process = *cloud;

    // Remove low confidence points:
    removeLowConfidencePoints(confidence_image, sr_conf_threshold, cloud_to_process);

    // Voxel grid filtering:
    PointCloudT::Ptr cloud_filtered(new PointCloudT);
    pcl::VoxelGrid<PointT> voxel_grid_filter_object;
    voxel_grid_filter_object.setInputCloud(cloud_to_process);
    voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
    voxel_grid_filter_object.filter (*cloud_filtered);

    *background_cloud += *cloud_filtered;
    ros::spinOnce();
    rate.sleep();
  }

  // Voxel grid filtering:
  PointCloudT::Ptr cloud_filtered(new PointCloudT);
  pcl::VoxelGrid<PointT> voxel_grid_filter_object;
  voxel_grid_filter_object.setInputCloud(background_cloud);
  voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
  voxel_grid_filter_object.filter (*cloud_filtered);

  background_cloud = cloud_filtered;

  // Background saving:
  pcl::io::savePCDFileASCII ("/tmp/background_" + frame_id.substr(1, frame_id.length()-1) + ".pcd", *background_cloud);

  std::cout << "done." << std::endl << std::endl;
}
    //================================================================
    // Higher level motion to close gripper until contact is found
    // Pass in the rate at which contact is closed at and the 
    // distance the gripper moves rate is in Hz, 
    // move_gripper_distance in meters
    // Velocity gripper moves is found by how much moved by what rate
    //================================================================
    void findContact(ros::Rate rate, double move_gripper_distance)
    {
      int pressure_min = 0; 
      int pressure_max = 0;
      bool contact_found = false;
      int no_motion_counter = 0;
      int previous_pressure_max = 0;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (pressure_min < LightPressureContact && ros::ok()
             && pressure_max < 600  && no_motion_counter < 250)
      {
        // Check pressure min and max
        simple_gripper->closeByAmount(move_gripper_distance);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        
        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        // Set distance for object width 
        if (!contact_found && pressure_min > 10){
          gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
          contact_found = true;
        }

        previous_pressure_max = pressure_max;

        //ROS_INFO("Pressure Min is: [%d]", pressure_min);
       // ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }
    //================================================================
    // Function to move the arm a little in a direction it needs to
    // move
    //================================================================
    void centerArmIncremental(ros::Rate rate, double move_gripper_distance)
    {

      // Find position of arm
      arm_controller->getArmTransform();
      double x = arm_controller->getTransform('x');
      double y = arm_controller->getTransform('y');
      double z = arm_controller->getTransform('z');

      int pressure_min = 0; 
      int pressure_max = 0;
      bool fingerSet = false;
      int num_run = 0;
      bool not_centered = true;
      int pressure_left = 0;
      int pressure_right = 0;
      int move_counter = 0;
      int lastFingerContact = 5;
      double distance_move_arm = 0.005;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (num_run < 8 && not_centered 
             && pressure_max < 600 && ros::ok())
      { 

        simple_gripper->closeByAmount(move_gripper_distance);
       
        pressure_left = biotac_obs->pressure_normalized_[Left];
        pressure_right = biotac_obs->pressure_normalized_[Right];

        // Check pressure min and max
        pressure_min = min(pressure_left, pressure_right);
        pressure_max = max(pressure_left, pressure_right);
      
        // First touches object
        if (pressure_max > 5 && !fingerSet)
        {
          fingerSet = true;

          if (pressure_min > 5)
          {
            not_centered = false;
            fingerSet = false;
          }

          firstContact.position = simple_gripper->getGripperLastPosition();
          if (pressure_left > pressure_right)
          {
            firstContact.finger = Left;
          }
          else
          {
            firstContact.finger = Right;
          }
          
          // Checks if finger was not set yet
          if (lastFingerContact == 5)
          {
            lastFingerContact = firstContact.finger;
          }
          
          // Stop if finger contact changed
          if (lastFingerContact != firstContact.finger)
          {
           not_centered = false;
          }

        }

        if (fingerSet)
        {

          if (!not_centered)
          {
            distance_move_arm = 0.0025;
          }

          // Find position of arm
          arm_controller->getArmTransform();
          x = arm_controller->getTransform('x');
          y = arm_controller->getTransform('y');
          z = arm_controller->getTransform('z');
          
          // Open grippers 
          simple_gripper->open2Position(GripperMaxOpenPosition);
          
          // Move left case 
          if (firstContact.finger == Left)
          {
            arm_controller->move_arm_to(x,y+distance_move_arm,z,1);    
          } 
          else 
          {
            arm_controller->move_arm_to(x,y-distance_move_arm,z,1);
          }
          fingerSet = false;
          num_run++;
          move_counter = 0;
        } 

        //ROS_INFO("Pressure Min is: [%d]", pressure_min);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        //ROS_INFO("Left finger is: [%d]", pressure_left);
        //ROS_INFO("Right finger is: [%d]", pressure_right);
        move_counter++;
        ros::spinOnce();
        rate.sleep();
      }
    }
    void runPlanner()
    {



        while(ros::ok())
        {
            rate->sleep();
            //If we are receiving data
            switch(currentState)
            {
		    case STOPPED:
		        //ROS_ERROR("STOPPED!");
		        //Next state
		        if(distanceToPerson > planner_activation_distance && hold == false)
		        {
		            nextState = PLANNER;
		            delete rate;
		            rate = new ros::Rate(10);
		        }

		        break;
		    case PLANNER:
		        ROS_ERROR("PLANNER");

		        if(distanceToPerson >= minimum_planner_distance && hold == false)
		        {
		            nextState = PLANNER;
		            //Get transforms
		            tf::StampedTransform transform;
		            try
		            {
		                listener.waitForTransform(world_frame, robot_frame, ros::Time(0), ros::Duration(10.0) );
		                listener.lookupTransform(world_frame, robot_frame,ros::Time(0), transform);
		            }
		            catch(tf::TransformException ex)
		            {
		                ROS_ERROR("%s",ex.what());
		                ros::Duration(1.0).sleep();
		            }

		            Eigen::Affine3d eigen_transform;
		            tf::transformTFToEigen(transform, eigen_transform);


		            // convert matrix from Eigen to openCV
		            cv::Mat baseLinkToMap;
		            cv::eigen2cv(eigen_transform.matrix(), baseLinkToMap);

		            cv::Point3d personPoint;
		            personPoint.x = personInRobotBaseFrame.point.x;
		            personPoint.y = personInRobotBaseFrame.point.y;
		            personPoint.z = personInRobotBaseFrame.point.z;
		            segwayController::moveBase(personPoint, baseLinkToMap, ac, distance_to_target);

		        }

		        else if(distanceToPerson < minimum_planner_distance || hold == true)
		        {
		            nextState = STOPPED;
		            ac->cancelAllGoals();
		            delete rate;
		            rate = new ros::Rate(10);
		        }

		        break;
		    default:
		        nextState = STOPPED;
		        ac->cancelAllGoals();
	    }
            ros::spinOnce();
            currentState = nextState;

        }
    }
void TargetPointSender::sleep(){
	rate_.sleep();
	ros::spinOnce();
}
Esempio n. 20
0
void spinOnce(ros::Rate& loopRate) {
  loopRate.sleep(); //Maintain the loop rate
  ros::spinOnce();   //Check for new messages
}
Esempio n. 21
0
void MTMHaptics::set_effort_mode(){
    robot_state_cmd.data = "DVRK_EFFORT_CARTESIAN";
    robot_state_pub.publish(robot_state_cmd);
    ROS_INFO("Setting Robot state as: %s",robot_state_cmd.data.c_str());
    rate_->sleep();
}
Esempio n. 22
0
void ObjRecInterface::recognize_objects_thread()
{
  ros::Rate max_rate(100.0);

  // Working structures
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
  boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZRGB> > voxel_grid(new pcl::VoxelGrid<pcl::PointXYZRGB>());

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::PointIndices::Ptr outliers (new pcl::PointIndices);

  // Create point clouds for foreground and background points
  vtkSmartPointer<vtkPoints> foreground_points;
  foreground_points.TakeReference(vtkPoints::New(VTK_DOUBLE));

  std::list<boost::shared_ptr<PointSetShape> > detected_models;

  while(ros::ok() && !time_to_stop_)
  {
    // Don't hog the cpu
    max_rate.sleep();

    cloud_full->clear();
    cloud->clear();

    ROS_DEBUG_STREAM("ObjRec: Aggregating point clouds... ");
    {
      // Scope for syncrhonization

      // Continue if the cloud is empty
      static ros::Rate warn_rate(1.0);
      if(clouds_.empty()) {
        ROS_WARN("Pointcloud buffer is empty!");
        warn_rate.sleep();
        continue;
      }

      // Lock the buffer mutex
      boost::mutex::scoped_lock buffer_lock(buffer_mutex_);

      ROS_DEBUG_STREAM("ObjRec: Computing objects from "
          <<clouds_.size()<<" point clounds "
          <<"between "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.back()->header).stamp)
          <<" to "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.front()->header).stamp)<<" seconds after they were acquired.");

      // Copy references to the stored clouds
      cloud_full->header = clouds_.front()->header;

      for(std::list<boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > >::const_iterator it = clouds_.begin();
          it != clouds_.end();
          ++it)
      {
        *cloud_full += *(*it);
      }
    }

    // Recongize objects
    bool objects_recognized = this->recognize_objects(
        cloud_full,
        cloud,
        voxel_grid,
        coefficients, inliers, outliers,
        foreground_points,
        detected_models,
        true,
        true);

    // No objects recognized
    if(!objects_recognized) {
      continue;
    }

    // Construct recognized objects message
    objrec_msgs::RecognizedObjects objects_msg;
    objects_msg.header.stamp = pcl_conversions::fromPCL(cloud->header).stamp;
    objects_msg.header.frame_id = "/world";//cloud->header;

    for(std::list<boost::shared_ptr<PointSetShape> >::iterator it = detected_models.begin();
        it != detected_models.end();
        ++it)
    {
      boost::shared_ptr<PointSetShape> detected_model = *it;

      // Construct and populate a message
      objrec_msgs::PointSetShape pss_msg;
      pss_msg.label = detected_model->getUserData()->getLabel();
      pss_msg.confidence = detected_model->getConfidence();
      array_to_pose(detected_model->getRigidTransform(), pss_msg.pose);

      // Transform into the world frame TODO: make this frame a parameter
      geometry_msgs::PoseStamped pose_stamped_in, pose_stamped_out;
      pose_stamped_in.header = pcl_conversions::fromPCL(cloud->header);
      pose_stamped_in.pose = pss_msg.pose;

      try {
        listener_.transformPose("/world",pose_stamped_in,pose_stamped_out);
        pss_msg.pose = pose_stamped_out.pose;
      }
      catch (tf::TransformException ex){
        ROS_WARN("Not transforming recognized objects into world frame: %s",ex.what());
      }

      objects_msg.objects.push_back(pss_msg);
    }

    // Publish the visualization markers
    this->publish_markers(objects_msg);

    // Publish the recognized objects
    objects_pub_.publish(objects_msg);

    // Publish the points used in the scan, for debugging
    foreground_points_pub_.publish(cloud);
  }
}
void receive(ros::Rate & loop_rate) {
  for (unsigned i = 0u; i < 10u; i++){
    ros::spinOnce();
    loop_rate.sleep();
  }
}
    void runLocalController()
    {



        while(ros::ok())
        {
            rate->sleep();
            //If we are receiving data
            switch(currentState)
            {
            case STOPPED:
                //ROS_ERROR("STOPPED!");

                //Next state
                if(distanceToPerson > planner_activation_distance && hold == false)
                {

                    nextState = PLANNER;
                    //delete rate;
                    //rate = new ros::Rate(50);
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }
                else if(distanceToPerson < planner_activation_distance && distanceToPerson > minimum_distance && hold == false)
                {
                    nextState = LOCALCONTROLLER;
                    //delete rate;
                    //rate = new ros::Rate(50);
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }
                /*else
                {
                    nextState = STOPPED;
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);

                }*/

                break;
            case PLANNER:
                //ROS_ERROR("PLANNER");

                if(distanceToPerson >= minimum_planner_distance && hold == false)
                {
                    nextState = PLANNER;
                    //Get transforms

                    cv::Point3d personPoint;
                    personPoint.x = personInRobotBaseFrame.point.x;
                    personPoint.y = personInRobotBaseFrame.point.y;
                    personPoint.z = personInRobotBaseFrame.point.z;

                    segwayController::proportionalController(personPoint, cmdPub, kv, kalfa);

                }
                else if( distanceToPerson < minimum_planner_distance && distanceToPerson >= minimum_distance && hold == false)
                {
                    nextState = LOCALCONTROLLER;
                    //delete rate;
                    //rate = new ros::Rate(50);

                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }

                else if(distanceToPerson < minimum_distance || hold == true)
                {
                    nextState = STOPPED;
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                    //delete rate;
                    //rate = new ros::Rate(50);
                }

                break;
            case LOCALCONTROLLER:
                //ROS_ERROR("LOCALCONTROLLER");

                if(distanceToPerson >= minimum_distance && distanceToPerson < planner_activation_distance && hold == false)
                {
                    nextState = LOCALCONTROLLER;


                    cv::Point3d personPoint;
                    personPoint.x = personInRobotBaseFrame.point.x;
                    personPoint.y = personInRobotBaseFrame.point.y;
                    personPoint.z = personInRobotBaseFrame.point.z;

                    //Same controller, but no v

                    segwayController::proportionalController(personPoint, cmdPub, 0, kalfa);


                }
                else if(distanceToPerson < minimum_distance || hold == true)
                {
                    nextState = STOPPED;
                    geometry_msgs::Twist goal;
                    goal.angular.x = 0;
                    goal.angular.y = 0;
                    goal.angular.z = 0;
                    goal.linear.x = 0;
                    goal.linear.y = 0;
                    goal.linear.z = 0;
                    cmdPub.publish(goal);
                }
                else if(distanceToPerson >= planner_activation_distance)
                {


                    nextState = PLANNER;
                    //delete rate;
                    //rate = new ros::Rate(50);

                }

                break;
            }
            ros::spinOnce();
            currentState = nextState;


        }
    }
    //================================================================
    // Higher level motion to close gripper until contact is found
    // Pass in the rate at which contact is closed at and the 
    // distance the gripper moves rate is in Hz, 
    // move_gripper_distance in meters
    // Velocity gripper moves is found by how much moved by what rate
    //================================================================
    void findContactOld(ros::Rate rate, double move_gripper_distance)
    {
      int pressure_min = 0; 
      int pressure_max = 0;
      bool contact_found = false;
      bool fingerSet = true;
      int no_motion_counter = 0;
      int previous_pressure_max = 0;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (pressure_min < LightPressureContact && ros::ok()
             && pressure_max < 600  && no_motion_counter < 250)
      {
        previous_pressure_max = pressure_max;
        
        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        // First touches object
        if (pressure_max > 5 && fingerSet)
        {
          firstContact.position = simple_gripper->getGripperLastPosition();
          if (biotac_obs->pressure_normalized_[Left] > biotac_obs->pressure_normalized_[Right])
          {
            firstContact.finger = Left;
          }
          else
          {
            firstContact.finger = Right;
          }
          fingerSet = false;
        }

        // Second finger touches object
        if (pressure_min > 10)
        {
          secondContact.position = simple_gripper->getGripperLastPosition();
          if (firstContact.position == Left)
          {
            secondContact.finger = Right;
          }
          else
          {
            secondContact.finger = Left;
          }
        }

        // Set distance for object width 
        if (!contact_found && pressure_min > 10){
          gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
          contact_found = true;
        }
      
        // Store last pressure felt by each finger 
        tap_pressure_left = biotac_obs->pressure_normalized_[Left];
        tap_pressure_right = biotac_obs->pressure_normalized_[Right];

        // Check pressure min and max
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        simple_gripper->closeByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Min is: [%d]", pressure_min);
       // ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }