Exemplo n.º 1
0
  SRNode(const ros::NodeHandle& nh): nh_(nh), it_d_(nh), it_i_(nh), it_c_(nh)
  {
    signal(SIGSEGV, &sigsegv_handler);

    info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
    image_pub_d_ = it_d_.advertise("distance/image_raw", 1);
    image_pub_i_ = it_i_.advertise("intensity/image_raw", 1);
    image_pub_c_ = it_c_.advertise("confidence/image_raw", 1);
    image_pub_d16_ = it_c_.advertise("distance/image_raw16", 1);
    cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("pointcloud_raw", 1);
    cloud_pub2_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud2_raw", 1);

    // set reconfigurable parameter defaults (all to automatic)
    // None right now

    try
      {
        dev_ = new sr::SR(use_filter_);
      }
    catch (sr::Exception& e)
      {
        ROS_ERROR_STREAM("Exception thrown while constructing camera driver: "
                         << e.what ());
        nh_.shutdown();
        return;
      }

    getInitParams();
  } 
Exemplo n.º 2
0
		ArSysSingleBoard()
			: cam_info_received(false),
			foundBoard(false),
			nh("~"),
			it(nh)
		{
			image_sub = it.subscribe("/image", 1, &ArSysSingleBoard::image_callback, this);
			cam_info_sub = nh.subscribe("/camera_info", 1, &ArSysSingleBoard::cam_info_callback, this);

			image_pub = it.advertise("result", 1);
			debug_pub = it.advertise("debug", 1);
			//Added/////////////////////////////////////////////////////////////////
			rvec_pub = it.advertise("rvec",1);
			tvec_pub = it.advertise("tvec",1);
			boardSize_pub = nh.advertise<std_msgs::Float32>("boardSize", 100);
			///////////////////////////////////////////////////////////////////
			pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
			transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
			position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
			
	
			nh.param<double>("marker_size", marker_size, 0.05);
			nh.param<std::string>("board_config", board_config, "boardConfiguration.yml");
			nh.param<std::string>("board_frame", board_frame, "");
			nh.param<bool>("image_is_rectified", useRectifiedImages, true);
			nh.param<bool>("draw_markers", draw_markers, false);
			nh.param<bool>("draw_markers_cube", draw_markers_cube, false);
			nh.param<bool>("draw_markers_axis", draw_markers_axis, false);
            		nh.param<bool>("publish_tf", publish_tf, false);

			the_board_config.readFromFile(board_config.c_str());

			ROS_INFO("Modified ArSys node started with marker size of %f m and board configuration: %s",
					 marker_size, board_config.c_str());
		}
    ArdroneTracker()
        : it_(nh_), useColorTracker_( true ),
          morphSize_( 15, 15 ),
          targetSize_( 10 ),
          hValue_( 30 ),
          hRange_( 50 ),
          matchThreshold_( 1.0 ),
          missesInARow_( 0 ),
          dampingFactor_( 0.9 ),
          surf_ ( 2500 )

    {
        imageSub_ = it_.subscribe("/ardrone/front/image_raw", 1, &ArdroneTracker::imageCb, this);
        thresholdPub_ = it_.advertise("/ardrone_tracker/image_threshold", 1 );
        histPub_ = it_.advertise( "/ardrone_tracker/image_histogram", 1 );
        morphPub_ = it_.advertise( "/ardrone_tracker/image_morphed", 1 );
        centersPub_ = it_.advertise("/ardrone_tracker/image_centers", 1 );
        pointPub_ = nh_.advertise<geometry_msgs::Point>("/ardrone_tracker/found_point", 1);

        morphSizeSub_ = nh_.subscribe( "/ardrone_tracker/control/morph_size", 1, &ArdroneTracker::morphSizeCb, this );
        targetSizeSub_ = nh_.subscribe( "/ardrone_tracker/control/target_size", 1, &ArdroneTracker::targetSizeCb, this );
        hValueSub_ = nh_.subscribe( "/ardrone_tracker/control/h_value", 1, &ArdroneTracker::hValueCb, this );
        hRangeSub_ = nh_.subscribe( "/ardrone_tracker/control/h_range", 1, &ArdroneTracker::hRangeCb, this );
        matchThreshSub_ = nh_.subscribe( "/ardrone_tracker/control/match_threshold", 1, &ArdroneTracker::matchThreshCb, this );
        roundelSource_ = cv::imread( "/home/sameer/Dropbox/Linux XFer/Roundel.png", 1 );

        matchContour_ = getMatchContour();
        calcMatchKeyPoints();
    }
Exemplo n.º 4
0
  ArucoSimple()
    : cam_info_received(false),
      nh("~"),
      it(nh)
  {
    image_sub = it.subscribe("/image", 1, &ArucoSimple::image_callback, this);
    cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoSimple::cam_info_callback, this);

    image_pub = it.advertise("result", 1);
    debug_pub = it.advertise("debug", 1);
    pose_pub = nh.advertise<geometry_msgs::Pose>("pose", 100);

    nh.param<double>("marker_size", marker_size, 0.05);
    nh.param<int>("marker_id", marker_id, 300);
    nh.param<std::string>("parent_name", parent_name, "");
    nh.param<std::string>("child_name", child_name, "");
    nh.param<bool>("image_is_rectified", useRectifiedImages, true);

    ROS_ASSERT(parent_name != "" && child_name != "");

    ROS_INFO("Aruco node started with marker size of %f m and marker id to track: %d",
             marker_size, marker_id);
    ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.",
             parent_name.c_str(), child_name.c_str());
  }
Exemplo n.º 5
0
  ArucoMarkerPublisher()
    : cam_info_received_(false),
      nh_("~"),
      it_(nh_)
  {
    image_sub_ = it_.subscribe("/image", 1, &ArucoMarkerPublisher::image_callback, this);
    cam_info_sub_ = nh_.subscribe("/camera_info", 1, &ArucoMarkerPublisher::cam_info_callback, this);

    image_pub_ = it_.advertise("result", 1);
    debug_pub_ = it_.advertise("debug", 1);
    marker_pub_ = nh_.advertise<aruco_msgs::MarkerArray>("markers", 100);

    nh_.param<double>("marker_size", marker_size_, 0.05);
    nh_.param<std::string>("reference_frame", reference_frame_, "");
    nh_.param<std::string>("camera_frame", camera_frame_, "");
    nh_.param<bool>("image_is_rectified", useRectifiedImages_, true);
    //nh.param<int>("marker_id", marker_id, 300);
    //nh_.param<std::string>("marker_frame", marker_frame_, "");
    //ROS_ASSERT(camera_frame_ != "" && marker_frame_ != "");

    ROS_ASSERT(! camera_frame_.empty());
    if ( reference_frame_.empty() )
      reference_frame_ = camera_frame_;

    //ROS_INFO("Aruco node started with marker size of %f m and marker id to track: %d",
             //marker_size, marker_id);
    //ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.",
             //reference_frame_.c_str(), marker_frame_.c_str());

    marker_msg_ = aruco_msgs::MarkerArray::Ptr( new aruco_msgs::MarkerArray() );
    marker_msg_->header.frame_id = reference_frame_;
    marker_msg_->header.seq = 0;
  }
Exemplo n.º 6
0
 LaneDetection()			// Constructor which initializes subscriptions and publications
   : it(node)
 {
   hough_sub = it.subscribe("/canny_edge_detector/edge_image", 1, &LaneDetection::process, this);
   hough_pub = it.advertise("/lane_detector/hough_image", 1);
   final_sub = it.subscribe("/canny_edge_detector/original_image", 1, &LaneDetection::Publish, this);
   final_pub = it.advertise("/lane_detector/lane_detected_image", 1);  
   cv::namedWindow(DETECTED_LINES, WINDOW_NORMAL);
   cv::namedWindow(DETECTED_LANES_ON_ORIGINAL_IMAGE, WINDOW_NORMAL);
 }
Exemplo n.º 7
0
 webcamControl(ros::NodeHandle &nh)
 : nh_(nh), it_(nh)
 {
    ros::Subscriber webcam_cmd = nh.subscribe("webcam_cmd", 50, &webcamControl::cameraCommandCallback, this); 
    filename_pub_ = nh_.advertise<std_msgs::String>("webcam_file", 50); // advertise camera messages
    home_image_pub_ = it_.advertise("home_target_image", 1); // advertise home target images
    image_pub_ = it_.advertise("webcam_image", 1);
    numWebcams_ = 0;
    cap_home_ = false;
    for (int i=0; i < MAX_NUM_WEBCAMS; i++) numImages_[i] = 0;
    openAllWebcams();
    //releaseAllWebcams();
 }
Exemplo n.º 8
0
    BoardScheme(std::string channel) : it_(nh_)
    {
        image_pub_ = it_.advertise(channel.c_str(), 1);
        sub = nh_.subscribe("baxter_tictactoe/new_board", 1, &BoardScheme::publish_draw_board, this);

        height=600;
        width =1024;

        // Let's find the minimum between height and width
        int minimum = height>width?width:height;

        board_bottom_left.x = (width -minimum)/2;
        board_bottom_left.y = (height-minimum)/2;

        n_cols = 3;
        n_rows = 3;

        cols_cell_img = minimum/n_cols;
        rows_cell_img = minimum/n_rows;
        ROS_DEBUG_STREAM("cols_cell_img=" << cols_cell_img << " rows_cell_img=" << rows_cell_img);

        white = cv::Scalar(255,255,255);
        blue  = cv::Scalar(180, 40, 40);  // REMEMBER that this is in BGR color code!!
        red   = cv::Scalar( 40, 40,150);  // REMEMBER that this is in BGR color code!!

    }
Exemplo n.º 9
0
  ImageConverter(char* ros_image_stream, char* ros_output_stream)
    : it_(nh_)
  {
    image_pub_ = it_.advertise(ros_output_stream, 1);
    image_sub_ = it_.subscribe(ros_image_stream, 1, &ImageConverter::imageCb, this);

  }
Exemplo n.º 10
0
  Target_XYWH()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/camera/image_raw", 1, 
      &Target_XYWH::imageCb, this);
    image_pub_ = it_.advertise("/target_xywh/output_video", 1);
    target_xywh_pub_ = nh_.advertise<std_msgs::Float32MultiArray>("/target_xywh", 1000);

    cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("/andbot/cmd_vel_camera", 1);


//    std_msgs::Float32MultiArray array;
    brray.data.clear();
    for (int i=0; i<4; i++)
        brray.data.push_back(0.0);
    
    
//    cv::namedWindow(OPENCV_WINDOW);

    //Load the cascades
    if (!face_cascade.load(face_cascade_name))  { 
        printf("--(!)Error loading face cascade\n"); 
    };

    
    gettimeofday(&tp, NULL);
    start_time = tp.tv_sec * 1000000 + tp.tv_usec;

  }
    TextReader(const char* correlation, const char* dictionary) :
        it_(nh_)//, cloud(new pcl::PointCloud<pcl::PointXYZRGB>), viewer(new pcl::visualization::PCLVisualizer("3D Viewer"))
    {
        /*
         * advertise on topic "text_detect" the result
         * subscribe on topic "image_color" for the camera picture with Callback-function "imageCb"
         * subscribe on topic "/base_odometry/state" with Callback-function "robotStateCb" for detecting camera movement
         */
        image_pub_ = it_.advertise("text_detect", 1);
        image_sub_ = it_.subscribe("image_color", 1, &TextReader::imageCb, this);
        robot_state_sub_ = nh_.subscribe("/base_odometry/state", 1, &TextReader::robotStateCb, this);

        //    depth_sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &TextReader::depthCb, this);

        detector = DetectText();
        detector.readLetterCorrelation(correlation);
        detector.readWordList(dictionary);

        pthread_mutex_init(&pr2_velocity_lock_, NULL);
        pthread_mutex_init(&pr2_image_lock_, NULL);
        okToDetect_ = false;
        initialized_ = false;

        x_ = 0;
        y_ = 0;
    }
Exemplo n.º 12
0
  SceneSegmentation()
  : nh_("~"), it_(nh_)
  {
    nh_.param<float>("sigma", sigma_, 0.5f);
    nh_.param<float>("k", k_, 500.f);
    nh_.param<int>("min", min_, 20);

    nh_.param<bool>("half", half_, true);
    bool draw_rect;
    nh_.param<bool>("draw_rect", draw_rect, true);

    ROS_INFO("Waiting for message on camera info topic.");
    camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh_);

    // segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, half_));
    if(half_)
      segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width/2, camera_info_->height/2, draw_rect));
    else
      segmentor_ = Segmentor::Ptr(new Segmentor(camera_info_->width, camera_info_->height, draw_rect));

    srv_segment_ = nh_.advertiseService("segment_image", &SceneSegmentation::segment_image_cb, this);
    ROS_INFO("segment_image is ready.");

    sub_image_ = it_.subscribe("image_in", 1, &SceneSegmentation::image_cb, this);
    pub_image_ = it_.advertise("image_out", 1);
    pub_rects_ = nh_.advertise<duckietown_msgs::Rects>("rects_out", 1);
    pub_segments_ = nh_.advertise<duckietown_msgs::SceneSegments>("segments_out", 1);
  }
Exemplo n.º 13
0
  ImageConverter()
    : it_(nh_)
  {
    image_pub_ = it_.advertise("output_image", 1);
    image_sub_ = it_.subscribe("input_image", 1, &ImageConverter::imageCb, this);

    //for blob detection
    params.minDistBetweenBlobs = 10.0;  // minimum 10 pixels between blobs
    params.filterByArea = true;         // filter my blobs by area of blob
    params.minArea = 100;              // min 100 pixels squared
    params.blobColor = 255;
//    params.maxArea = 5000.0;             // max 500 pixels squared
//	params.minThreshold = 0;
//	params.maxThreshold = 5;
//	params.thresholdStep = 5;
//
//	params.minArea = 10;
//	params.minConvexity = 0.3;
//	params.minInertiaRatio = 0.01;
//
//	params.maxArea = 8000;
//	params.maxConvexity = 10;
//
//	params.filterByColor = false;
//	params.filterByCircularity = false;

	blobDetector = new SimpleBlobDetector( params );
	blobDetector->create("SimpleBlob");

    cv::namedWindow(WINDOW);
  }
Exemplo n.º 14
0
	ImageConverter() : it_(nh_)
	{
		image_sub_ = it_.subscribe("/camera/front/image_raw", 1, &ImageConverter::imageCb, this);
		image_pub_ = it_.advertise("/image_converter/output_video", 1);

		cv::namedWindow(OPENCV_WINDOW);
	}
Exemplo n.º 15
0
  ImageConverter()
    : it_(nh_)
  {

    image_pub_ = it_.advertise("out", 1);
    image_sub_ = it_.subscribe("/camera/image_raw", 1, &ImageConverter::imageCb, this);

    cv::namedWindow(WINDOW);
    int temp=10;
    cv::createTrackbar( " TresholdEllipse ", WINDOW, &temp, 255, NULL);
//    cv::createTrackbar( " R0: ", WINDOW, &temp, 255, NULL);
//    temp=170;
//    cv::createTrackbar( " G0: ", WINDOW, &temp, 255, NULL);
//    temp=170;
//	cv::createTrackbar( " B0: ", WINDOW, &temp, 255, NULL);
//	temp=55;
//	cv::createTrackbar( " R1: ", WINDOW, &temp, 255, NULL);
//	temp=255;
//	cv::createTrackbar( " G1: ", WINDOW, &temp, 255, NULL);
//	temp=255;
//	cv::createTrackbar( " B1: ", WINDOW, &temp, 255, NULL);
	temp=150;
	cv::createTrackbar(" Threshold:",WINDOW,&temp,400,NULL);
	temp=4;
	cv::createTrackbar(" Dilate:",WINDOW,&temp,10,NULL);
	CameraCalibMat[0][2]=640;
    CameraCalibMat[1][2]=480;

    cv::setMouseCallback(WINDOW,ImageConverter::onMouse,&src_HSV);
    cv::namedWindow(WINDOW2);
  }
	ImageConverter():it_(nh_)
	{
		// Subscrive to input video feed and publish output video feed
		image_sub_ = it_.subscribe("/camera/image_raw", 1, &ImageConverter::imageCb, this);
		image_pub_ = it_.advertise("/legoDetection/output_video", 1);
		cv::namedWindow(OPENCV_WINDOW);
	}
Exemplo n.º 17
0
    ImageConverter() : it_(nh_)
    {
        image_pub_ = it_.advertise("out", 1);
        image_sub_ = it_.subscribe("usb_cam/image_raw", 1, &ImageConverter::imageCb, this);

        cv::namedWindow(WINDOW);
    }
Exemplo n.º 18
0
 ImageConverter()
   : it_(nh_)
 {
   int scale;
   if (nh_.getParam("/processingScale", scale))
   {
     processingScale = scale;
   }
   else
   {
     processingScale = 1;
   }
   double mTm;
   if (nh_.getParam("/majorToMinor", mTm))
   {
     majorToMinor = mTm;
   }
   else
   {
     majorToMinor = 2;
   }
   std::cout << " Scale: " << processingScale << std::endl;
   std::cout << " mTm: " << majorToMinor << std::endl;
   image_pub_ = it_.advertise("out", 1);
   image_sub_ = it_.subscribe("in", 1, &ImageConverter::imageCb, this);
   cv::namedWindow(WINDOW);
   circle_pub = nh_.advertise<std_msgs::Float64MultiArray>("circle_data", 10);
   circle_data.data.resize(12);
 }
Exemplo n.º 19
0
 Thresholder(bool show, bool maker, int* input_thresholds, string name, string input) : it_(nh_)
 {
     image_pub_ = it_.advertise("vision/thresholder_"+name, 1);
     image_sub_ = it_.subscribe(input , 1, &Thresholder::process, this);
     SHOW_IMAGES = show;
     SHOW_MAKER = maker;
     
     for(int i = 0; i < 13; i++) thresholds[i] = input_thresholds[i];
     if(SHOW_MAKER){
         namedWindow("Thresholder");
         createTrackbar("H+","Thresholder",&thresholds[0], H_MAX);
         createTrackbar("H-","Thresholder",&thresholds[1], H_MAX);
         createTrackbar("H~","Thresholder",&thresholds[2], BOOL);
         createTrackbar("S+","Thresholder",&thresholds[3], SVBGR_MAX);
         createTrackbar("S-","Thresholder",&thresholds[4], SVBGR_MAX);
         createTrackbar("V+","Thresholder",&thresholds[5], SVBGR_MAX);
         createTrackbar("V-","Thresholder",&thresholds[6], SVBGR_MAX);
         createTrackbar("B+","Thresholder",&thresholds[7], SVBGR_MAX);
         createTrackbar("B-","Thresholder",&thresholds[8], SVBGR_MAX);
         createTrackbar("G+","Thresholder",&thresholds[9], SVBGR_MAX);
         createTrackbar("G-","Thresholder",&thresholds[10], SVBGR_MAX);
         createTrackbar("R+","Thresholder",&thresholds[11], SVBGR_MAX);
         createTrackbar("R-","Thresholder",&thresholds[12], SVBGR_MAX);
     }
     if (SHOW_IMAGES) {
         namedWindow("Input Video");
         namedWindow("Output Video");
     }
 }
Exemplo n.º 20
0
 // constructor
 online_IPM(ros::NodeHandle nh, int ow, int oh, double f_u, double f_v, double c_u, double c_v, double deg, double cam_h, std::string camera_name)
     : nh_(nh), priv_nh_("~"),ipMapper(ow,oh,f_u,f_v,c_u,c_v,deg,cam_h),it(nh_)
 {
     read_images_= nh_.subscribe(nh_.resolveName(camera_name), 1,&online_IPM::publish_remapper,this);
     pub_mapped_images_= it.advertise("/camera/ground_image_ipmapped", 1);
     //ipMapper.initialize(200,PROJECTED_IMAGE_HEIGTH, fu, fv, cx, cy, pitch);
 }
 ImageConverter()
   : it_(nh_)
 {
     pub= n.advertise<turtlesim::Velocity>("/drocanny/vanishing_points", 500);//
     image_sub_ = it_.subscribe("/ardrone/image_raw", 1, &ImageConverter::imageCb, this);
     image_pub_= it_.advertise("/arcv/Image",1);    
 }
Exemplo n.º 22
0
 ImageConverter()
   : it_(nh_)
 {
   // Subscrive to input video feed and publish output video feed
   image_sub_ = it_.subscribe(CAMERA_TOPIC, 1, 
     &ImageConverter::imageCb, this);
   image_pub_ = it_.advertise("/image_converter/output_video", 1);
 }
Exemplo n.º 23
0
    findcircle()
    : it_(nh_)
    {
        image_sub_ = it_.subscribe("/camera/image_raw", 1, &findcircle::imageCb, this);
        image_pub_= it_.advertise("/camera/image_processed",1);

        namedWindow(WINDOW);
    }
Exemplo n.º 24
0
	ImageCropper(): it_(nh_)
	{
		// Subscribe to input video feed and publish output video feed
		image_sub_ = it_.subscribe(camera_topic, 1, 
			&ImageCropper::imageCb, this);
		image_pub_ = it_.advertise("/front_kinect/rgb/image_cropped", 1);
		/* Have to crop the depth data too to use this for find object3d*/
	}
Exemplo n.º 25
0
  ImageEdge()
    : it_(nh_)
  {
    image_pub_ = it_.advertise("image_edge", 1);
    image_sub_ = it_.subscribe("gscam/image_raw", 1, &ImageEdge::imageCb, this);

    cv::namedWindow("Image");
  }
Exemplo n.º 26
0
  ImageConverter()
    : it_(nh_)
  {
    image_pub_ = it_.advertise("out", 1);
    image_sub_ = it_.subscribe("/wide_stereo/left/image_color", 1, &ImageConverter::imageCb, this);

    cv::namedWindow(WINDOW);
  }
Exemplo n.º 27
0
  faceDetector() : it_(nh_)
  {
	image_sub_ = it_.subscribe("/usb_cam/image_raw",1,
	  &faceDetector::callback, this);
	image_pub_ = it_.advertise("/face_detection_CPU", 1);

	cv::namedWindow(OPENCV_WINDOW, WINDOW_NORMAL);
  }
  ImageGrabber(char* ros_image_stream)
    : it_(nh_)
  {
    image_pub_ = it_.advertise("correll_ros2opencv", 1);
    image_sub_ = it_.subscribe(ros_image_stream, 1, &ImageGrabber::imageCb, this);

    cv::namedWindow(WINDOW);
  }
Exemplo n.º 29
0
    ImageConverter()
            : it(nh)
    {
        image_sub = it.subscribe("/camera/image_raw", 1,
                                 &ImageConverter::imageCb, this);
        image_pub = it.advertise("/camera/horizon_line", 1);

    }
Exemplo n.º 30
0
 simplecanny()
    : it_(nh_)
  {
 
     image_sub_ = it_.subscribe("/camera/image_raw", 1, &simplecanny::imageCb, this);
     image_pub_= it_.advertise("/camera/image_processed",1);
 
 
  }