Merge pull request #1 from UbiquityRobotics/kinetic-devel

Pulling in changes from main repo

Author: jack-digilabs

aruco_detect/src/aruco_detect.cpp

@@ -45,6 +45,7 @@
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/image_encodings.h>
 #include <dynamic_reconfigure/server.h>
+#include <std_srvs/SetBool.h>
 
 #include "fiducial_msgs/Fiducial.h"
 #include "fiducial_msgs/FiducialArray.h"
@@ -72,8 +73,11 @@ class FiducialsNode {
     image_transport::ImageTransport it;
     image_transport::Subscriber img_sub;
 
+    ros::ServiceServer service_enable_detections;
+
     // if set, we publish the images that contain fiducials
     bool publish_images;
+    bool enable_detections;
 
     double fiducial_len;
 
@@ -86,7 +90,6 @@ class FiducialsNode {
     std::vector<int> ignoreIds;
     std::map<int, double> fiducialLens;
 
-
     image_transport::Publisher image_pub;
 
     cv::Ptr<aruco::DetectorParameters> detectorParams;
@@ -105,6 +108,9 @@ class FiducialsNode {
     void camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg);
     void configCallback(aruco_detect::DetectorParamsConfig &config, uint32_t level);
 
+    bool enableDetectionsCallback(std_srvs::SetBool::Request &req,
+                                std_srvs::SetBool::Response &res);
+
     dynamic_reconfigure::Server<aruco_detect::DetectorParamsConfig> configServer;
     dynamic_reconfigure::Server<aruco_detect::DetectorParamsConfig>::CallbackType callbackType;
 
@@ -293,6 +299,10 @@ void FiducialsNode::camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg
 }
 
 void FiducialsNode::imageCallback(const sensor_msgs::ImageConstPtr & msg) {
+    if (enable_detections == false) {
+        return; //return without doing anything
+    }
+
     ROS_INFO("Got image %d", msg->header.seq);
     frameNum++;
 
@@ -319,6 +329,10 @@ void FiducialsNode::imageCallback(const sensor_msgs::ImageConstPtr & msg) {
         ROS_INFO("Detected %d markers", (int)ids.size());
 
         for (size_t i=0; i<ids.size(); i++) {
+	    if (std::count(ignoreIds.begin(), ignoreIds.end(), ids[i]) != 0) {
+	        ROS_INFO("Ignoring id %d", ids[i]);
+	        continue;
+	    }
             fiducial_msgs::Fiducial fid;
             fid.fiducial_id = ids[i];
 
@@ -411,6 +425,23 @@ void FiducialsNode::imageCallback(const sensor_msgs::ImageConstPtr & msg) {
     }
 }
 
+bool FiducialsNode::enableDetectionsCallback(std_srvs::SetBool::Request &req,
+                                std_srvs::SetBool::Response &res)
+{
+    enable_detections = req.data;
+    if (enable_detections){
+        res.message = "Enabled aruco detections.";
+        ROS_INFO("Enabled aruco detections.");
+    }
+    else {
+        res.message = "Disabled aruco detections.";
+        ROS_INFO("Disabled aruco detections.");
+    }
+    
+    res.success = true;
+    return true;
+}
+
 FiducialsNode::FiducialsNode(ros::NodeHandle & nh) : it(nh)
 {
     frameNum = 0;
@@ -422,6 +453,7 @@ FiducialsNode::FiducialsNode(ros::NodeHandle & nh) : it(nh)
     distortionCoeffs = cv::Mat::zeros(1, 5, CV_64F);
 
     haveCamInfo = false;
+    enable_detections = true;
 
     int dicno;
 
@@ -506,18 +538,20 @@ FiducialsNode::FiducialsNode(ros::NodeHandle & nh) : it(nh)
 
     image_pub = it.advertise("/fiducial_images", 1);
 
-
     vertices_pub = new ros::Publisher(nh.advertise<fiducial_msgs::FiducialArray>("/fiducial_vertices", 1));
 
     pose_pub = new ros::Publisher(nh.advertise<fiducial_msgs::FiducialTransformArray>("/fiducial_transforms", 1));
 
     dictionary = aruco::getPredefinedDictionary(dicno);
 
     img_sub = it.subscribe("/camera", 1,
-                           &FiducialsNode::imageCallback, this);
+                        &FiducialsNode::imageCallback, this);
 
     caminfo_sub = nh.subscribe("/camera_info", 1,
-			       &FiducialsNode::camInfoCallback, this);
+                    &FiducialsNode::camInfoCallback, this);
+
+    service_enable_detections = nh.advertiseService("enable_detections",
+                        &FiducialsNode::enableDetectionsCallback, this);
 
     callbackType = boost::bind(&FiducialsNode::configCallback, this, _1, _2);
     configServer.setCallback(callbackType);