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();
}
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();
}
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());
}
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;
}
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);
}
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();
}
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!!
}
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);
}
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;
}
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);
}
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);
}
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);
}
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);
}
ImageConverter() : it_(nh_)
{
image_pub_ = it_.advertise("out", 1);
image_sub_ = it_.subscribe("usb_cam/image_raw", 1, &ImageConverter::imageCb, this);
cv::namedWindow(WINDOW);
}
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);
}
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");
}
}
// 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);
}
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);
}
findcircle()
: it_(nh_)
{
image_sub_ = it_.subscribe("/camera/image_raw", 1, &findcircle::imageCb, this);
image_pub_= it_.advertise("/camera/image_processed",1);
namedWindow(WINDOW);
}
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*/
}
ImageEdge()
: it_(nh_)
{
image_pub_ = it_.advertise("image_edge", 1);
image_sub_ = it_.subscribe("gscam/image_raw", 1, &ImageEdge::imageCb, this);
cv::namedWindow("Image");
}
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);
}
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);
}
ImageConverter()
: it(nh)
{
image_sub = it.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub = it.advertise("/camera/horizon_line", 1);
}
simplecanny()
: it_(nh_)
{
image_sub_ = it_.subscribe("/camera/image_raw", 1, &simplecanny::imageCb, this);
image_pub_= it_.advertise("/camera/image_processed",1);
}