Esempio n. 1
0
	DataProcessor::DataProcessor(ros::NodeHandle & n): n_(n) 
	{
		processed_pointcloud_ = false;
		new_cloud_wanted_ = false;
		nocluster_count_ = 0;
		process_service_ = n_.advertiseService("process_pcd", &DataProcessor::processDataCallback, this);
		pub_active_region_ = n_.advertise<sensor_msgs::PointCloud2>("activeregion",1);
		marker_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
		regions_pub_ = n_.advertise<visualization_msgs::MarkerArray>("region_nums", 1);

		//ros::Subscriber sub = n.subscribe("/camera/rgb/points",1,got_point_cloud);
		//client_grasp_ = n_.serviceClient<duplo_v1::Grasp_Duplo>("grasp_duplo");
		client_manipulate_ = n_.serviceClient<duplo_v1::Manipulate_Duplo>("manipulate_duplo");
		//client_tumble_ = n_.serviceClient<duplo_v1::Tumble_Duplo>("tumble_duplo");
		reset_posn.x = 0.6;	reset_posn.y = -0.5; reset_posn.z = 1;

		while ( !ros::service::waitForService("get_new_pcd",ros::Duration(2.0)) && n.ok() ) 
			ROS_INFO("DUPLO: Waiting for object detection service to come up");
		if (!n.ok()) exit(0);

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp1(new pcl::PointCloud<pcl::PointXYZRGB>);
		object_cloud_ = temp1;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp2(new pcl::PointCloud<pcl::PointXYZRGB>);
		planar_cloud_ = temp2;
		
		ROS_INFO("Ready to process input point cloud data.");
		client_newpcd_ = n.serviceClient<duplo_v1::Get_New_PCD>("get_new_pcd");
	}
  TFMonitor(bool using_specific_chain, std::string framea  = "", std::string frameb = ""):
    framea_(framea), frameb_(frameb),
    using_specific_chain_(using_specific_chain)
  {
    
    if (using_specific_chain_)
    {
      cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
      while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
        cout << "." << flush;
      cout << endl;
     
      try{
        tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
      }
      catch(tf::TransformException& ex){
        ROS_WARN("Transform Exception %s", ex.what());
        return;
      } 

      /*      cout << "Chain currently is:" <<endl;
      for (unsigned int i = 0; i < chain_.size(); i++)
      {
        cout << chain_[i] <<", ";
      }
      cout <<endl;*/
    }
    subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
    subscriber_tf_message_ = node_.subscribe<tf::tfMessage>("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1));
    
  }
Esempio n. 3
0
void setParamLoop(ros::NodeHandle& nh) {
    // red
    /*MIN_HUE = 160; MAX_HUE = 10;
    MIN_SAT = 150; MAX_SAT = 255;
    MIN_VAL = 100; MAX_VAL = 255;*/

    // yellow sponge
    MIN_HUE = 163;
    MAX_HUE = 44;
    MIN_SAT = 107;
    MAX_SAT = 184;
    MIN_VAL = 133;
    MAX_VAL = 255;

    while (nh.ok()) {
        if (LocalConfig::trackbars) {
            // Needs this to update the opencv windows
            char key = cv::waitKey(20);
            if (key == 'q')
                exit(0);

            cv::namedWindow("hue trackbars");
            cv::createTrackbar("hue min", "hue trackbars", &MIN_HUE, 255);
            cv::createTrackbar("hue max", "hue trackbars", &MAX_HUE, 255);
            cv::createTrackbar("sat min", "hue trackbars", &MIN_SAT, 255);
            cv::createTrackbar("sat max", "hue trackbars", &MAX_SAT, 255);
            cv::createTrackbar("val min", "hue trackbars", &MIN_VAL, 255);
            cv::createTrackbar("val max", "hue trackbars", &MAX_VAL, 255);
        }

        setParams(nh);
        sleep(0.2);
    }
}
bool spin()
{
    ROS_INFO("summit_robot_control::spin()");
    ros::Rate r(desired_freq_);  // 50.0 

    while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
    {
      if (starting() == 0)
      {
	    while(ros::ok() && node_handle_.ok()) {
          UpdateControl();
          UpdateOdometry();
          PublishOdometry();
          diagnostic_.update();
          ros::spinOnce();
	      r.sleep();
          }
	      ROS_INFO("END OF ros::ok() !!!");
      } else {
       // No need for diagnostic here since a broadcast occurs in start
       // when there is an error.
       usleep(1000000);
       ros::spinOnce();
      }
   }

   return true;
}
Esempio n. 5
0
 void Spin(){
   while(nh.ok()){
     Receive();
     ros::spinOnce();
     if (step(TIME_STEP) == -1) break;
   }
 }
Esempio n. 6
0
 bool spin()
 {
     ROS_INFO("camera node is running.");
     h264Server.start();
     while (node.ok())
     {
         // Process any pending service callbacks
         ros::spinOnce();
         std::string newResolution;
         node.getParam("resolution", newResolution);
         int newWidth = std::stoi(newResolution.substr(0, newResolution.find('x')));
         int newHeight = std::stoi(newResolution.substr(newResolution.find('x') + 1));
         std::string newVideoDevice;
         node.getParam("video_device", newVideoDevice);
         bool newDetectorEnabled;
         node.getParam("detector_enabled", newDetectorEnabled);
         if (newVideoDevice != videoDevice || 
                 newDetectorEnabled != detectorEnabled ||
                 newWidth != cameraWidth ||
                 newHeight != cameraHeight) {
             initCameraAndEncoders();
         }
         if(!sendPreview()) {
             // Sleep and hope the camera recovers
             usleep(1000*1000);
         }
         // Run at 1kHz
         usleep(1000);
     }
     h264Server.stop();
     return true;
 }
    geometry_msgs::PoseStamped getCartBaseLinkPosition()
    {
        geometry_msgs::PoseStamped pose_base_link;
        pose_base_link.header.frame_id = base_link_;

        while (nh_.ok()) {
            try {
                tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);

                pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
                pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
                pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
                pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
                pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
                pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
                pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();

                return pose_base_link;
            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
                ros::Duration(0.1).sleep();
            }
        }
    }
Esempio n. 8
0
 void
 run ()
 {
   while (nh_.ok ())
   {
     ros::spinOnce ();
   }
 }
void LoopbackControllerManager::run()
{

  ros::Rate rate(1.0/dt_);
  while(rosnode_->ok())
  {
    update();
    rate.sleep();
  }
}
Esempio n. 10
0
  //Close the gripper
  void close(){
    
    ROS_INFO("Closing Gripper");
    ros::Duration(0.5).sleep();

    //wait for the listener to get the first message
    listener_.waitForTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                               ros::Time(0), ros::Duration(1.0));
    
    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

    //record the starting transform from the gripper to the base frame
    listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                              ros::Time(0), start_transform);

    bool done = false;
    pr2_controllers_msgs::Pr2GripperCommand gripper_cmd;
    gripper_cmd.position = 0.0;
    gripper_cmd.max_effort = 50.0;

    ros::Rate rate(10.0);

    double dist_moved_before;
    double dist_moved;
    while (!done && nh_.ok())
    {
      r_gripper_.publish(gripper_cmd);

      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
  
      //see how if the gripper is open or if it hit some object
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;

      dist_moved_before = dist_moved;
      dist_moved = relative_transform.getOrigin().length();

      //ROS_INFO("%f",dist_moved);
      if(dist_moved > 0.03 || dist_moved < dist_moved_before) done = true;
    
    }
  }
Esempio n. 11
0
void I2cImu::spin()
{
	ros::Rate r(1.0 / (imu_->IMUGetPollInterval() / 1000.0));
	while (nh_.ok())
	{
		update();

		ros::spinOnce();
		r.sleep();
	}
}
Esempio n. 12
0
  int main (int argc, char **argv) {
    struct usb_dev_handle *dev;
  
    dev = ctxInit();
    
    if (!dev) {
      perror("Initializing CNX-P2140");
      return -1;
    }

    ros::NodeHandle node_param("~");
    node_param.param<double>("diag_frequency", diag_frequency, 1.0);
    node_param.param<double>("input", input_nominal, 13.8);
    node_param.param<double>("primary", primary, 18.5);
    node_param.param<double>("secondary", secondary, 12.0);

    diag_pub = node.advertise<power_msgs::PowerReading>("ctx2140", 1);

    ros::Rate loop_rate(diag_frequency);
    while (node.ok()) {
      ctxValues diag;
      power_msgs::PowerReading reading;
      
      if (ctxReadValues(dev, &diag)) {
	printf("Error reading from ctx2140\n");
	return -1;
      }

      reading.volts_read.push_back(diag.battVoltage);
      reading.volts_read.push_back(diag.priVoltage);
      reading.volts_read.push_back(diag.secVoltage);

      reading.current.push_back(diag.battCurrent);
      reading.current.push_back(diag.priCurrent);
      reading.current.push_back(diag.secCurrent);

      reading.volts_full.push_back(input_nominal);
      reading.volts_full.push_back(primary);
      reading.volts_full.push_back(secondary);

      reading.temperature.push_back(diag.temperature);

      reading.header.stamp = ros::Time::now();
  
      diag_pub.publish(reading);

      loop_rate.sleep();
    }

    ctxClose(dev);
    
    return 0;
  }
void LoopbackControllerManager::ControllerManagerROSThread()
{
  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());

  ros::Rate rate(0.2/dt_);

  while (rosnode_->ok())
  {
    rate.sleep();
    ros::spinOnce();
  }
}
Esempio n. 14
0
 bool spin()
 {
   ros::Rate loop_rate(this->framerate_);
   while (node_.ok())
   {
     if (!take_and_send_image())
       ROS_WARN("USB camera did not respond in time.");
     ros::spinOnce();
     loop_rate.sleep();
   }
   return true;
 }
void waitForAction(const T &action, const ros::NodeHandle &node_handle,
                   const ros::Duration &wait_for_server, const std::string &name)
{
  ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());

  // in case ROS time is published, wait for the time data to arrive
  ros::Time start_time = ros::Time::now();
  while (start_time == ros::Time::now())
  {
    ros::WallDuration(0.01).sleep();
    ros::spinOnce();
  }

  // wait for the server (and spin as needed)
  if (wait_for_server == ros::Duration(0, 0))
  {
    while (node_handle.ok() && !action->isServerConnected())
    {
      ros::WallDuration(0.02).sleep();
      ros::spinOnce();
    }
  }
  else
  {
    ros::Time final_time = ros::Time::now() + wait_for_server;
    while (node_handle.ok() && !action->isServerConnected() && final_time > ros::Time::now())
    {
      ros::WallDuration(0.02).sleep();
      ros::spinOnce();
    }
  }

  if (!action->isServerConnected())
    throw std::runtime_error("Unable to connect to move_group action server within allotted time (2)");
  else
    ROS_DEBUG("Connected to '%s'", name.c_str());
}
Esempio n. 16
0
void MoveR::IK_loop()
{

  ros::Rate r(2);

  while(nh_.ok())
  {
    ros::spinOnce();
    IK4(Target);
    axis_pub.publish(arm_angle);
    r.sleep();
  }

  return;
}
Esempio n. 17
0
  //! Drive forward a specified distance based on odometry information
  bool driveForwardOdom(double distance)
  {
    //wait for the listener to get the first message
    listener_.waitForTransform("base_footprint", "odom_combined", 
                               ros::Time(0), ros::Duration(1.0));
    
    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

    //record the starting transform from the odometry to the base frame
    listener_.lookupTransform("base_footprint", "odom_combined", 
                              ros::Time(0), start_transform);
    
    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;
    //the command will be to go forward at 0.25 m/s
    base_cmd.linear.y = base_cmd.angular.z = 0;
    base_cmd.linear.x = 0.25;
    
    ros::Rate rate(10.0);
    bool done = false;
    while (!done && nh_.ok())
    {
      //send the drive command
      cmd_vel_pub_.publish(base_cmd);
      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform("base_footprint", "odom_combined", 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
      //see how far we've traveled
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;
      double dist_moved = relative_transform.getOrigin().length();

      if(dist_moved > distance) done = true;
    }
    if (done) return true;
    return false;
  }
Esempio n. 18
0
void HeaderManipulation::publishMsgLoop(const ros::NodeHandle &nh)
{
    ROS_DEBUG("HeaderManipulation publishMsgLoop");
    while (nh.ok())
    {
        {
            boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
            if (!stamped_msg_buffer_.empty())
            {
                ROS_DEBUG("publishing msg");
                publishMsg(stamped_msg_buffer_.front());
                stamped_msg_buffer_.pop();
                continue;
            }
        }
        publish_retry_rate_.sleep();
    }
}
  void start()
  {
    initialize();
    char input[30];

    while (rosNode.ok())
    {
      readLineInput(input);

      // check if the type was toggled (between 'door' & 'elevator')
      if (setControlType(input))
      {
        continue;
      }

      callServices(input);
    }
  }
Esempio n. 20
0
  SignalSimulator() : nh()
{
    ros::Publisher signal_pub = nh.advertise<heatmap::wifi_signal>("signal", 1000);
    ros::ServiceServer service = nh.advertiseService("get_wifi_signal", &SignalSimulator::srvGetSignal, this);

    ros::Rate loop_rate(10);

    ws.essid = "Sim WIFI";
    ws.link_quality_max = 70;

    while(nh.ok())
    {
      ws.link_quality = rand() % 71;
      signal_pub.publish(ws);

      ros::spinOnce();
      loop_rate.sleep();
    }
}
  bool spin ()
  {
    int nr_points = cloud.width * cloud.height;
    std::string fields_list = pcl::getFieldsList(cloud);
    ros::Rate r(ros::Duration(1,0)); //1s tact

    while(nh.ok ())
    {
      ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
                            << " points " << fields_list
                            << " on topic \"" << nh.resolveName(cloud_topic)
                            << "\" in frame \"" << cloud.header.frame_id << "\"");
      cloud.header.stamp = ros::Time::now();
      pub.publish(cloud);

      r.sleep();
    }
    return (true);
  }
double NodeClass::moveRelative(double x_rel, double y_rel, bool only_get_time) {
	double dist_tot_, time_tot_, time_to_acc_, v_tot_;
	bool finished = false;
	ros::Duration motion_time_;
	ros::Time motion_begin_ = ros::Time::now();
	
	dist_tot_ = sqrt(x_rel * x_rel + y_rel * y_rel);
	
	if( dist_tot_ <= 2 * 0.5 * ACC * pow((V_MAX/ACC),2) ) { // v/a = time to accelerate to top speed
		//max speed won't be reached:
		time_to_acc_ = sqrt(dist_tot_ / 2 / ACC * 2);
		time_tot_ = time_to_acc_ * 2;
	} else {
		//max speed will be reached:
		time_to_acc_ = V_MAX / ACC;
		time_tot_ = 2 * time_to_acc_ + (dist_tot_ - (0.5*ACC*pow(time_to_acc_,2) * 2)) / V_MAX;
	}
	
	if(only_get_time) return time_tot_;
		
	while(finished == false && n.ok()) {
		motion_time_ = ros::Time::now() - motion_begin_;
		if(motion_time_.toSec() < time_to_acc_) {
			//accelerating
			v_tot_ = ACC * motion_time_.toSec();
		} else if(motion_time_.toSec() < time_tot_ - time_to_acc_) {
			//driving at max speed
			v_tot_ = V_MAX;
		} else if(motion_time_.toSec() < time_tot_){
			//deccelerating
			v_tot_ = ACC * (time_tot_ - motion_time_.toSec());
		} else {
			v_tot_ = 0.0f;
			finished = true;
		}
		
		commandPltfSpeed(x_rel / dist_tot_ * v_tot_, y_rel / dist_tot_ * v_tot_, 0.0f); //x_rel / dist_tot_ = vx / v_tot  = sin a
	}
	
	commandPltfSpeed(0, 0, 0);

	return 0;
}
void Smooth_derivative::mainLoop() {
	// determines the number of loops per second
	ros::Rate loop_rate(20);

	// als long as all is O.K : run
	// terminate if the node get a kill signal
	while (m_nodeHandle.ok())
	{
		calculateCommand();
		emergencyStop();

		// send the command to the roomrider for execution
		m_commandPublisher.publish(m_roombaCommand);

		// spinOnce, just make the loop happen once
		ros::spinOnce();
		// and sleep, to be aligned with the 50ms loop rate
		loop_rate.sleep();
	}
}// end of mainLoop
Esempio n. 24
0
void setParamLoop(ros::NodeHandle& nh) {

	while (nh.ok()) {
		if (LocalConfig::trackbars) {
			// Needs this to update the opencv windows
			char key = cv::waitKey(20);
			if (key == 'q')
				exit(0);

			cv::namedWindow("YCrCb trackbars");
			cv::createTrackbar("Y min", "YCrCb trackbars", &MIN_Y, 256);
			cv::createTrackbar("Y max", "YCrCb trackbars", &MAX_Y, 256);
			cv::createTrackbar("Cr min", "YCrCb trackbars", &MIN_CR, 256);
			cv::createTrackbar("Cr max", "YCrCb trackbars", &MAX_CR, 256);
			cv::createTrackbar("Cb min", "YCrCb trackbars", &MIN_CB, 256);
			cv::createTrackbar("Cb max", "YCrCb trackbars", &MAX_CB, 256);
		}
		sleep(0.2);
	}
}
Esempio n. 25
0
void AsyncSpinnerImpl::threadFunc()
{
  disableAllSignalsInThisThread();

  CallbackQueue* queue = callback_queue_;
  bool use_call_available = thread_count_ == 1;
  WallDuration timeout(0.1);

  while (continue_ && nh_.ok())
  {
    if (use_call_available)
    {
      queue->callAvailable(timeout);
    }
    else
    {
      queue->callOne(timeout);
    }
  }
}
	// In order to keep the legs updated in the visual representation, re-publish the
	// latest values at a faster rate, spinOnce frequently to make sure we get published values.
	void spin()
	{
		ros::Rate r(100);

		while (nh_.ok()) {
			ros::spinOnce();
//			for (size_t i=0;i<NUM_LEGS;++i) {
//				if( i==0 || i==2 || i==3 || i==5 )
//				{
//					joint_state_.position[i] = 0.75*sin(10.0*ros::Time::now().toSec());
//				}
//				else{
//					joint_state_.position[i] = 0.75*sin(10.0*ros::Time::now().toSec()+3.1415);
//				}
//
//			}
			joint_state_.header.stamp = ros::Time::now();
			js_pub_.publish(joint_state_);
			r.sleep();
		}
	}
Esempio n. 27
0
  bool spin()
  {
    while (node_.ok())
    {
      if (take_and_send_image())
      {
        count_++;
        ros::Time now_time = ros::Time::now();
        if (now_time > next_time_) {
          ROS_DEBUG("%d frames/sec", count_);
          count_ = 0;
          next_time_ = next_time_ + ros::Duration(1,0);
        }
      } else {
        ROS_ERROR("couldn't take image.");
        usleep(1000000);
      }
//      self_test_.checkTest();
    }
    return true;
  }
Esempio n. 28
0
  void run(int argc, char** argv)
  {
    // Initialize ROS
    ros::Rate r(10);

    conn.sendLine("scanSet width=180");
    //conn.sendLine("scanPush cmd=\"scanGet\"");

    while (nh_.ok())
    {
      std::cout << conn.receiveData() << std::endl;
      std::string data;
      while( conn.popLine(data) )
      {
        parse( data );
      }
      ros::spinOnce();
      conn.sendLine("scanGet");
      r.sleep();
    }
  }
Esempio n. 29
0
void JoyStickWindow::loop(){
	ros::Rate r(5.0);
	while(n.ok())
	{	
		char ans = 0;
		
		drawScreen();
		
		switch(ans = getch()){
			case 'q': // forward
				delwin(screen);
				endwin();
				return;
		}
		
		ros::spinOnce();
		r.sleep();
	}
	
	delwin(screen);
	endwin();
}
Esempio n. 30
0
    bool getMeshFromDB(object_recognition_msgs::GetObjectInformation &obj_info)
    {
      //! Client for getting the mesh for a database object
      ros::ServiceClient get_model_mesh_srv_;
      std::string get_model_mesh_srv_name("get_object_info");
      ros::Time start_time = ros::Time::now();
      while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) )
      {
        ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
        if (!nh_.ok() || ros::Time::now() - start_time >= ros::Duration(5.0))
          return false;
      }

      get_model_mesh_srv_ = nh_.serviceClient<object_recognition_msgs::GetObjectInformation>
        (get_model_mesh_srv_name, false);

      if ( !get_model_mesh_srv_.call(obj_info) )
      {
        ROS_ERROR("Get model mesh service service call failed altogether");
        return false;
      }
      return true;
    }