void unsubscribe()
{
if (use_indices_) {
sub_input_.unsubscribe();
sub_indices_.unsubscribe();
}
else {
sub_.shutdown();
}
}
bool
PointCloudProjector::enable(platform_motion_msgs::Enable::Request& req, platform_motion_msgs::Enable::Response& resp)
{
ros::NodeHandle nh;
if(req.state && !enabled_)
{
pointcloud_sub.subscribe(nh, "pointcloud", 1);
patch_sub.subscribe(nh, "patches", 1);
}
else if(enabled_)
{
pointcloud_sub.unsubscribe();
patch_sub.unsubscribe();
}
enabled_ = req.state;
resp.state = enabled_;
return true;
}
void disconnectCb()
{
num_subs--;
if(num_subs <= 0)
{
color_image_sub_.unsubscribe();
pc_sub_.unsubscribe();
}
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
message_filters::Subscriber<GroundPlane> &sub_gp,
image_transport::SubscriberFilter &sub_col,
image_transport::SubscriberFilter &sub_dep,
image_transport::ImageTransport &it) {
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
sub_cam.unsubscribe();
sub_gp.unsubscribe();
sub_col.unsubscribe();
sub_dep.unsubscribe();
} else {
ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
}
}
/// Unsubscribe from camera topics if possible.
void disconnectCallback()
{
if (sub_counter_ > 0)
{
ROS_INFO("[fiducials] Unsubscribing from camera topics");
color_camera_image_sub_.unsubscribe();
color_camera_info_sub_.unsubscribe();
sub_counter_--;
ROS_INFO("[fiducials] %i subscribers on camera topics [OK]", sub_counter_);
}
}
/// Unsubscribe from camera topics if possible.
void disconnectCallback()
{
sub_counter_--;
if (sub_counter_ == 0)
{
ROS_DEBUG("[all_camera_viewer] Unsubscribing from camera topics");
if (use_right_color_camera_)
{
right_color_camera_image_sub_.unsubscribe();
right_camera_info_sub_.unsubscribe();
}
if (use_left_color_camera_)
{
left_color_camera_image_sub_.unsubscribe();
left_camera_info_sub_.unsubscribe();
}
if (use_tof_camera_)
{
tof_camera_grey_image_sub_.unsubscribe();
}
}
}
// Handles (un)subscribing when clients (un)subscribe
void PointCloud2Nodelet::connectCb()
{
boost::lock_guard<boost::mutex> lock(connect_mutex_);
if (pub_points2_.getNumSubscribers() == 0)
{
sub_l_image_ .unsubscribe();
sub_l_info_ .unsubscribe();
sub_r_info_ .unsubscribe();
sub_disparity_.unsubscribe();
}
else if (!sub_l_image_.getSubscriber())
{
ros::NodeHandle &nh = getNodeHandle();
// Queue size 1 should be OK; the one that matters is the synchronizer queue size.
sub_l_image_ .subscribe(*it_, "left/image_rect_color", 1);
sub_l_info_ .subscribe(nh, "left/camera_info", 1);
sub_r_info_ .subscribe(nh, "right/camera_info", 1);
sub_disparity_.subscribe(nh, "disparity", 1);
}
}
void image_callback(const sensor_msgs::LaserScan::ConstPtr& laser,
const sensor_msgs::CompressedImage::ConstPtr& image) {
if (!this->bot.valid()) {
return;
}
try {
// Convert from ROS image msg to OpenCV matrix images
cv_bridge::CvImagePtr cv_ptr;
// cv_ptr = cv_bridge::toCvCopy((sensor_msgs::Imageu) image, sensor_msgs::image_encodings::BGR8);
// cv::Mat src = cv_ptr->image;
cv::Mat src = cv::imdecode(image->data, 1);
// Take hsv ranges from launch
// OpenCV filters to find colours
cv::Mat hsv, pink_threshold, yellow_threshold, blue_threshold, green_threshold;
cv::cvtColor(src, hsv, CV_BGR2HSV);
cv::inRange(hsv, cv::Scalar(pink_ranges[0],pink_ranges[2],pink_ranges[3]), cv::Scalar(pink_ranges[1],255,255), pink_threshold);
cv::inRange(hsv, cv::Scalar(yellow_ranges[0],yellow_ranges[2],yellow_ranges[3]), cv::Scalar(yellow_ranges[1],255,255), yellow_threshold);
cv::inRange(hsv, cv::Scalar(blue_ranges[0],blue_ranges[2],blue_ranges[3]), cv::Scalar(blue_ranges[1],255,255), blue_threshold);
cv::inRange(hsv, cv::Scalar(green_ranges[0],green_ranges[2],green_ranges[3]), cv::Scalar(green_ranges[1],255,255), green_threshold);
blue_threshold = cv::Scalar::all(255) - blue_threshold;
pink_threshold = cv::Scalar::all(255) - pink_threshold;
yellow_threshold = cv::Scalar::all(255) - yellow_threshold;
green_threshold = cv::Scalar::all(255) - green_threshold;
// blob detection
cv::SimpleBlobDetector::Params params;
params.filterByArea = 1;
params.minArea = 200;
params.maxArea = 100000;
params.filterByConvexity = 1;
params.minConvexity = 0.5;
params.filterByInertia = true;
params.minInertiaRatio = 0.65;
// find keypoints
std::vector<cv::KeyPoint> pink_keypoints, yellow_keypoints, blue_keypoints, green_keypoints;
cv::SimpleBlobDetector detector(params);
detector.detect(pink_threshold, pink_keypoints);
detector.detect(yellow_threshold, yellow_keypoints);
detector.detect(blue_threshold, blue_keypoints);
detector.detect(green_threshold, green_keypoints);
cv::Mat blobs;
cv::drawKeypoints (src, pink_keypoints, blobs,
cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, blue_keypoints, blobs,
cv::Scalar(255,0,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, green_keypoints, blobs,
cv::Scalar(0,255,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::drawKeypoints (blobs, yellow_keypoints, blobs,
cv::Scalar(125,125,125), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
// acceptable horizontal distance between the 2 colours on a pillar
for (auto pink_pt = pink_keypoints.begin(); pink_pt != pink_keypoints.end(); pink_pt++) {
ROS_INFO_STREAM("Pink x= " << pink_pt->pt.x);
double xCo = pink_pt->pt.x;
xCo = 320 - xCo;
// TODO: you might want atan2
double theta = atan2(xCo*tan(29 * M_PI / 180), 320.0);
//ROS_INFO_STREAM("theta: " << (theta * 180 / M_PI));
double lTheta = theta - laser->angle_min;
int distance_index = lTheta / laser->angle_increment;
/*
double r2 = laser->ranges[distance_index];
double r1 = 0.1;
*/
// double distance = sqrt( (r1 + r2*cos(theta))*(r1 + r2*cos(theta)) + (r2*sin(theta))*(r2*sin(theta)) );
double distance = laser->ranges[distance_index];
ROS_INFO_STREAM("BEACON IS " << distance << " FROM BOT");
if( std::isfinite(distance) && std::isfinite(theta) && distance < 2.0 ) {
distance = distance - 0.3;
//ROS_INFO_STREAM("theta " << (theta * 180 / M_PI) << " distance " << distance);
search_for_match(blue_keypoints.begin(), blue_keypoints.end(), pink_pt, "blue", make_pair(distance, theta));
search_for_match(yellow_keypoints.begin(), yellow_keypoints.end(), pink_pt, "yellow", make_pair(distance, theta));
search_for_match(green_keypoints.begin(), green_keypoints.end(), pink_pt, "green", make_pair(distance, theta));
}
}
// gui display
imshow("blobs", blobs);
bool found_all = true;
int count = 0;
for (auto it = beacons.begin(); it != beacons.end(); ++it) {
if (!it->found()) {
found_all = false;
} else {
ROS_INFO_STREAM("Found T: " << it->top << " B: " << it->bottom << " at " << it->x << " " << it->y);
count++;
}
}
ROS_INFO_STREAM("Found " << count << " of " << beacons.size() << " beacons.");
if (found_all && this->bot.valid()) {
ROS_INFO("Found all beacons");
// Send beacon message
ass1::FoundBeacons msg;
msg.n = beacons.size();
msg.positions.clear();
for (auto it = beacons.begin(); it != beacons.end(); ++it) {
geometry_msgs::Point point;
point.x = it->x;
point.y = it->y;
point.z = 0;
msg.positions.push_back(point);
}
beacons_pub.publish(msg);
//shutdown subscriptions
img_sub.unsubscribe();
laser_sub.unsubscribe();
odom_sub.shutdown();
}
cv::waitKey(30);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("Could not convert from to 'bgr8'.");
}
}
