void RobotWidget::subscribeToCamera(const std::string& topic) {
boost::function<void(const sensor_msgs::ImageConstPtr)> imageCallBack
= boost::bind(&RobotWidget::onCameraImageMessage, this, _1);
image_transport::ImageTransport image_transport_(node_handle_);
camera_subscriber_ = image_transport_.subscribe(topic, 10, imageCallBack, ros::VoidPtr(),
image_transport::TransportHints("compressed", ros::TransportHints().unreliable()));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ellipse_detection_node");
ros::NodeHandle nh, nh_priv("~");
tf_listener_ = new tf::TransformListener();
// Reading params
bool params_loaded = true;
params_loaded *= nh_priv.getParam("img_in",img_in_topic_);
params_loaded *= nh_priv.getParam("contour_min_pix_size",contour_min_pix_size_);
params_loaded *= nh_priv.getParam("ellipse_max_ratio",ellipse_max_ratio_);
params_loaded *= nh_priv.getParam("hole_radius",hole_radius_);
params_loaded *= nh_priv.getParam("holes_min_spacing",holes_min_spacing_);
params_loaded *= nh_priv.getParam("debug",debug_);
params_loaded *= nh_priv.getParam("base_frame",base_frame_);
params_loaded *= nh_priv.getParam("out_pose_topic",out_pose_topic_);
params_loaded *= nh_priv.getParam("out_error_topic",out_error_topic_);
params_loaded *= nh_priv.getParam("fit_ellipse_max_error",fit_ellipse_max_error_);
if(!params_loaded){
ROS_ERROR("Couldn't find all the required parameters. Closing...");
return -1;
}
// ROS suscribers and publishers
image_transport::ImageTransport image_transport_(nh);
image_transport::CameraSubscriber depth_sub_ = image_transport_.subscribeCamera(img_in_topic_, 1, callback);
// pose0_pub_ = nh.advertise<geometry_msgs::PoseStamped>(out_pose_topic_+"_0", 1);
// pose1_pub_ = nh.advertise<geometry_msgs::PoseStamped>(out_pose_topic_+"_1", 1);
error_pub_ = nh_priv.advertise<geometry_msgs::Twist>(out_error_topic_, 1);
// OpenCV windows
if(debug_){
cv::namedWindow("Original Image");
cv::namedWindow("1/MedianBlur");
cv::namedWindow("2/MonoImage");
cv::namedWindow("3/AdaptiveThresholding");
cv::namedWindow("4/Opening");
}
cv::namedWindow("Ellipse detection");
ros::spin();
ros::shutdown();
return 0;
}
