unsigned long init() { color_camera_image_sub_.subscribe(*it_, "colorimage", 1); point_cloud_sub_.subscribe(node_handle_, "pointcloud", 1); sync_pointcloud_->connectInput(point_cloud_sub_, color_camera_image_sub_); sync_pointcloud_callback_connection_ = sync_pointcloud_->registerCallback(boost::bind(&CobKinectImageFlipNodelet::inputCallback, this, _1, _2)); return ipa_Utils::RET_OK; }
virtual void onInit() { /// Configure publisher and synchronizer. nh = getMTNodeHandle(); pub = nh.advertise<art_common::KinectMsg>("/kinect/compressed", 10); synchronizer.registerCallback(&KinectPub::callback_synchronizer, this); /// Load and configure the Kinect device. OpenNIDriver& driver = OpenNIDriver::getInstance(); driver.updateDeviceList(); device = driver.getDeviceByIndex(0); XnMapOutputMode output_mode; output_mode.nXRes = XN_VGA_X_RES; output_mode.nYRes = XN_VGA_Y_RES; output_mode.nFPS = 30; device->setDepthOutputMode(output_mode); device->setImageOutputMode(output_mode); device->setDepthRegistration(true); dynamic_cast<DeviceKinect*>(device.get())->setDebayeringMethod(ImageBayerGRBG::EdgeAwareWeighted); /// Register callbacks and start the streams. device->registerImageCallback(&KinectPub::cb_image, *this); device->registerDepthCallback(&KinectPub::cb_depth, *this); device->startImageStream(); device->startDepthStream(); }
RecogInfo() : okao_sub( nh, "/humans/okao_server", 100 ), okaoNot_sub( nh, "/humans/okao_server_not", 100 ), sync( MySyncPolicy( 100 ), okao_sub, okaoNot_sub ) { sync.registerCallback( boost::bind( &RecogInfo::callback, this, _1, _2 ) ); recog_pub_ = nh.advertise<humans_msgs::Humans>("/humans/recog_info", 1); //unknownの場合 humans_msgs::Person unknown; unknown.okao_id = 0; unknown.name = "Unknown"; unknown.laboratory = "Unknown"; unknown.grade = "Unknown"; prop_buf[ 0 ] = unknown; //決定できない場合 humans_msgs::Person undetermined; undetermined.okao_id = -1; undetermined.name = "Undetermined"; undetermined.laboratory = "Undetermined"; undetermined.grade = "Undetermined"; prop_buf[ -1 ] = undetermined; //ファイル名 time_t now = time(NULL); struct tm *pnow = localtime(&now); file_name <<"/home/uema/histdata/" << pnow->tm_year+1900<< "-"<< pnow->tm_mon<< "-" << pnow->tm_mday<< "_" <<pnow->tm_hour<<"-" << pnow->tm_min<< "-" << pnow->tm_sec<<".txt"; }
// Constructor PcPublisher() : image_transport_(n_), tof_sync_(SyncPolicy(3)), c_xyz_image_32F3_(0), c_grey_image_32F1_(0) { topicPub_pointCloud_ = n_.advertise<sensor_msgs::PointCloud>("point_cloud", 1); xyz_image_subscriber_.subscribe(image_transport_, "image_xyz", 1); grey_image_subscriber_.subscribe(image_transport_,"image_grey", 1); tof_sync_.connectInput(xyz_image_subscriber_, grey_image_subscriber_); tof_sync_.registerCallback(boost::bind(&PcPublisher::syncCallback, this, _1, _2)); // topicSub_xyzImage_ = n_.subscribe("xyz_image", 1, &PcPublisher::topicCallback_xyzImage, this); }
RecogInfo() : okao_sub( nh, "/humans/OkaoServer", 100 ), okaoNot_sub( nh, "/humans/OkaoServerNot", 100 ), sync( MySyncPolicy( 100 ), okao_sub, okaoNot_sub ) { sync.registerCallback( boost::bind( &RecogInfo::callback, this, _1, _2 ) ); recog_pub_ = nh.advertise<humans_msgs::Humans>("/humans/RecogInfo", 1); }
ObjectTracker(ros::NodeHandle& nh) { camera_sub.subscribe(nh, "image", 1); saliency_sub.subscribe(nh, "saliency_img", 1); fg_objects_sub.subscribe(nh, "probability_image", 1); // ApproximateTime takes a queue size as its constructor argument, hence SyncPolicy(10) sync = new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), camera_sub, fg_objects_sub, saliency_sub); sync->registerCallback(boost::bind(&ObjectTracker::process_images, this, _1, _2, _3)); }
StereoSynchronizer () : nh_(), it_(nh_), sync_(15) { std::string left_raw = nh_.resolveName("left_raw"); std::string right_raw = nh_.resolveName("right_raw"); image_sub_l_.subscribe(it_, left_raw + "/image_raw", 4); info_sub_l_ .subscribe(nh_, left_raw + "/camera_info", 4); image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4); info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4); left_ns_ = nh_.resolveName("left"); right_ns_ = nh_.resolveName("right"); ros::NodeHandle cam_l_nh(left_ns_); ros::NodeHandle cam_r_nh(right_ns_); it_l_ = new image_transport::ImageTransport(cam_l_nh); it_r_ = new image_transport::ImageTransport(cam_r_nh); pub_left_ = it_l_->advertiseCamera("image_raw", 1); pub_right_ = it_r_->advertiseCamera("image_raw", 1); sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_); sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4)); }
MatcherTestbed(ros::NodeHandle nh): matcher_(), image_transport_(nh), subscriber_left_( image_transport_, left_image_topic, 1 ), subscriber_right_( image_transport_, right_image_topic, 1 ), lcinfo_sub_(nh,left_camera_info_topic,1), rcinfo_sub_(nh,right_camera_info_topic,1), sync_( MySyncPolicy(10), subscriber_left_, subscriber_right_ ), first_callback_(false) { template_image_ = cv::Mat (cvLoadImage (template_filename.c_str (), CV_LOAD_IMAGE_COLOR)); subscriber_ = image_transport_.subscribe(subscribe_topic, 1, &MatcherTestbed::imageCallback, this); sync_.registerCallback( boost::bind( &MatcherTestbed::stereoImagesCallback, this, _1, _2) ); publisher_ = image_transport_.advertise (image_matches_topic, 1); }
void start(const Mode mode) { this->mode = mode; running = true; std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info"; std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info"; // std::cout << "test1..." << std::endl; image_transport::TransportHints hints("raw"); subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints); subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints); subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize); subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize); // std::cout << "test2..." << std::endl; syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth); syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4)); spinner.start(); std::cout << "waiting for kinect2_bridge..." << std::endl; std::chrono::milliseconds duration(1); while(!updateImage || !updateCloud) { if(!ros::ok()) { return; } std::this_thread::sleep_for(duration); } std::cout << "kinect2_bridge is running.." << std::endl; cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>()); cloud->height = color.rows; cloud->width = color.cols; cloud->is_dense = false; cloud->points.resize(cloud->height * cloud->width); createLookup(this->color.cols, this->color.rows); // // // Create the filtering object // // pcl::PassThrough<pcl::PointXYZRGBA> pass; // // pass.setInputCloud (cloud); // // pass.setFilterFieldName ("z"); // // pass.setFilterLimits (0.0, 1.0); // // //pass.setFilterLimitsNegative (true); // // pass.filter (*cloud); cloudViewer(); }
KinectDataDumper(ros::NodeHandle nh) : visual_sub_(nh, "/camera/rgb/image_raw", 10000), depth_sub_(nh, "/camera/depth/image_raw", 10000), sync_(MySyncPolicy(10000), visual_sub_, depth_sub_) { std::cout << "KinectDataDumper() - start!" << std::endl; nh_ = nh; sync_.registerCallback( boost::bind(&KinectDataDumper::callback, this, _1, _2)); std::string fileName = path + "timestamps.txt"; timestampStream.open(fileName.c_str()); std::cout << "KinectDataDumper() - end!" << std::endl; }
MatcherNode() : _it(_node) , _subImage( _it, "image", 1) , _subInfo(_node, "camera_info", 1) , sync( SyncPolicy(10), _subImage, _subInfo) { _pubBestRect = _node.advertise< jsk_recognition_msgs::Rect >("best_rect", 1); _pubBestPolygon = _node.advertise< geometry_msgs::PolygonStamped >("best_polygon", 1); _pubBestPoint = _node.advertise< geometry_msgs::PointStamped >("rough_point", 1); _pubBestBoundingBox = _node.advertise< jsk_recognition_msgs::BoundingBoxArray >("best_box", 1); _debug_pub = _it.advertise("debug_image", 1); // _subImage = _it.subscribe("image", 1, &MatcherNode::image_cb, this); sync.registerCallback( boost::bind (&MatcherNode::image_cb , this, _1, _2) ); ros::NodeHandle local_nh("~"); std::string template_filename; std::string default_template_file_name; try { #ifdef ROSPACK_EXPORT rospack::ROSPack rp; rospack::Package *p = rp.get_pkg("jsk_perception"); if (p!=NULL) default_template_file_name = p->path + std::string("/sample/opencv-logo2.png"); #else rospack::Rospack rp; std::vector<std::string> search_path; rp.getSearchPathFromEnv(search_path); rp.crawl(search_path, 1); std::string path; if (rp.find("jsk_perception",path)==true) default_template_file_name = path + std::string("/sample/opencv-logo2.png"); #endif } catch (std::runtime_error &e) {} local_nh.param("template_filename", template_filename, default_template_file_name); local_nh.param("standard_width", standard_width, 12); local_nh.param("standard_height", standard_height, 12); local_nh.param("best_window_estimation_method", best_window_estimation_method, 1); local_nh.param("coefficient_threshold", coefficient_thre, 0.5); local_nh.param("coefficient_threshold_for_best_window", coefficient_thre_for_best_window, 0.67); local_nh.param("sliding_factor", sliding_factor, 1.0); local_nh.param("show_result", show_result_, true); local_nh.param("pub_debug_image", pub_debug_image_, true); local_nh.param("object_width", template_width, 0.06); local_nh.param("object_height", template_height, 0.0739); cv::Mat template_image=cv::imread(template_filename); if(template_add(template_image))template_images.push_back(template_image); }
PointCloudCapturer(ros::NodeHandle &n) : nh_(n), synchronizer_( MySyncPolicy(1), cloud_sub_, camera_sub_) { nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/kinect_head/camera/rgb/points")); nh_.param("input_image_topic", input_image_topic_, std::string("/kinect_head/camera/rgb/image_color")); nh_.param("input_camera_info_topic", input_camera_info_topic_, std::string("/kinect_head/camera/rgb/camera_info")); // cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudCapturer::cloud_cb, this); camera_sub_.subscribe( nh_, input_image_topic_, 1000 ); cloud_sub_.subscribe( nh_, input_cloud_topic_, 1000 ); sync_connection_ = synchronizer_.registerCallback( &PointCloudCapturer::callback, this ); ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str()); point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true); //wait for head controller action server to come up while(!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok()) { ROS_INFO("Waiting for the point_head_action server to come up"); } cloud_and_image_received_ = false; nh_.param("move_offset_y_min", move_offset_y_min_, -1.5); nh_.param("move_offset_y_max", move_offset_y_max_, 1.5); nh_.param("step_y", step_y_, 0.3); nh_.param("move_offset_z_min", move_offset_z_min_, 0.3); nh_.param("move_offset_z_max", move_offset_z_max_, 1.5); nh_.param("step_z", step_z_, 0.3); nh_.param("move_offset_x", move_offset_x_, 1.0); nh_.param("bag_name", bag_name_, std::string("bosch_kitchen_tr.bag")); nh_.param("to_frame", to_frame_, std::string("base_link")); nh_.param("rate", rate_, 1.0); current_position_x_ = move_offset_x_; current_position_y_ = move_offset_y_min_; current_position_z_ = move_offset_z_min_; move_head("base_link", current_position_x_, current_position_y_, current_position_z_); EPS = 1e-5; //thread ROS spinner spin_thread_ = boost::thread (boost::bind (&ros::spin)); bag_.open(bag_name_, rosbag::bagmode::Write); }
/// Initialize sensor fusion node. /// Setup publisher of point cloud and corresponding color data, /// setup camera toolboxes and colored point cloud toolbox /// @return <code>true</code> on success, <code>false</code> otherwise bool init() { if (loadParameters() == false) return false; image_transport::SubscriberStatusCallback imgConnect = boost::bind(&AllCameraViewer::connectCallback, this); image_transport::SubscriberStatusCallback imgDisconnect = boost::bind(&AllCameraViewer::disconnectCallback, this); save_camera_images_service_ = node_handle_.advertiseService("save_camera_images", &AllCameraViewer::saveCameraImagesServiceCallback, this); // Synchronize inputs of incoming image data. // Topic subscriptions happen on demand in the connection callback. // TODO: Dynamically determine, which cameras are available if(use_right_color_camera_ && use_tof_camera_ && !use_left_color_camera_) { ROS_INFO("[all_camera_viewer] Setting up subscribers for right color and tof camera"); shared_sub_sync_.connectInput(right_color_camera_image_sub_, tof_camera_grey_image_sub_); shared_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::sharedModeSrvCallback, this, _1, _2)); } else if(use_right_color_camera_ && !use_tof_camera_ && use_left_color_camera_) { ROS_INFO("[all_camera_viewer] Setting up subscribers left and right color camera"); stereo_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_); stereo_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::stereoModeSrvCallback, this, _1, _2)); } else if(use_right_color_camera_ && use_tof_camera_ && use_left_color_camera_) { ROS_INFO("[all_camera_viewer] Setting up subscribers for left color, right color and tof camera"); all_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_, tof_camera_grey_image_sub_); all_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::allModeSrvCallback, this, _1, _2, _3)); } else { ROS_ERROR("[all_camera_viewer] Specified camera configuration not available"); return false; } connectCallback(); ROS_INFO("[all_camera_viewer] Initializing [OK]"); return true; }
void startRecord() { std::cout << "Controls:" << std::endl << " [ESC, q] - Exit" << std::endl << " [SPACE, s] - Save current frame" << std::endl << " [l] - decreas min and max value for IR value rage" << std::endl << " [h] - increas min and max value for IR value rage" << std::endl << " [1] - decreas min value for IR value rage" << std::endl << " [2] - increas min value for IR value rage" << std::endl << " [3] - decreas max value for IR value rage" << std::endl << " [4] - increas max value for IR value rage" << std::endl; image_transport::TransportHints hints("compressed"); image_transport::TransportHints hintsIr("compressed"); image_transport::TransportHints hintsDepth("compressedDepth"); subImageColor = new image_transport::SubscriberFilter(it, topicColor, 4, hints); subImageIr = new image_transport::SubscriberFilter(it, topicIr, 4, hintsIr); subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, 4, hintsDepth); sync = new message_filters::Synchronizer<ColorIrDepthSyncPolicy>(ColorIrDepthSyncPolicy(4), *subImageColor, *subImageIr, *subImageDepth); sync->registerCallback(boost::bind(&Recorder::callback, this, _1, _2, _3)); spinner.start(); }
dataPublisher(ros::NodeHandle nh) : nh_(nh), priv_nh_("~"),sub_markers_readings(nh,nh.resolveName("/all_markers"),1),sub_camera_readings(nh,"/camera/depth_registered/points",1),sub_starlink_readings(nh,"/world_star",1),sub_wristlink_readings(nh,"/world_wrist",1),sync(MySyncPolicy(10),sub_markers_readings,sub_camera_readings,sub_starlink_readings,sub_wristlink_readings) { sync.registerCallback( boost::bind( &dataPublisher::callback, this, _1, _2 ,_3,_4) ); data_info = nh_.advertise<AllData>(nh_.resolveName("/data_info"), 10); }
IMU_odom(ros::NodeHandle& _nh): nh(_nh), sub_scan(nh, "/scan", 10), sub_arduino(nh, "/arduino/yaw", 10), sync_subs(Policy_sync_subs(10), sub_scan, sub_arduino) { sync_subs.registerCallback(boost::bind(&IMU_odom::sync_subs_callback, this, _1, _2)); first_execution = true; }
EKF(ros::NodeHandle& _nh): nh(_nh), sub_xyheading(nh, "/odometry", 10), sub_z(nh, "/altitude", 10), sub_arduino(nh, "/arduino/IMU", 10), sync_subs(Policy_sync_subs(10), sub_xyheading, sub_z, sub_arduino) { //pub_ekf = nh.advertise<sensor_msgs::PointCloud>("/world", 10); sync_subs.registerCallback(boost::bind(&EKF::sync_subs_callback, this, _1, _2, _3)); }