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));
}
