diff --git a/qr_detector/CMakeLists.txt b/qr_detector/CMakeLists.txt
index c53e292d3336e8c7308a12ef555a85ed3f6365b7..61e8fea4b8b51cafd6609eccd6de3e96f439ccd1 100644
--- a/qr_detector/CMakeLists.txt
+++ b/qr_detector/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver geometry_msgs sensor_msgs cv_bridge tf)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs geometry_msgs sensor_msgs cv_bridge tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver geometry_msgs sensor_msgs cv_bridge tf
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs geometry_msgs sensor_msgs cv_bridge tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -97,6 +97,7 @@ target_link_libraries(${PROJECT_NAME} ${detectqrcode_LIBRARY})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
diff --git a/qr_detector/include/qr_detector_driver.h b/qr_detector/include/qr_detector_driver.h
index 4b92bb4bdf2f444dd6e4524800123ab1d7279e52..a239f41deeb154d83ea57c1d1440c16e5135e773 100644
--- a/qr_detector/include/qr_detector_driver.h
+++ b/qr_detector/include/qr_detector_driver.h
@@ -161,6 +161,7 @@ class QrDetectorDriver : public iri_base_driver::IriBaseDriver
~QrDetectorDriver(void);
void init(double fx, double fy, double cx, double cy);
+ void set_qr_size(double qr_x,double qr_y);
void findQR(cv::Mat& grey, std::vector<std::string> &tag_ids,std::vector< std::vector<double> >& poses);
std::string get_camera_frame(void);
};
diff --git a/qr_detector/include/qr_detector_driver_node.h b/qr_detector/include/qr_detector_driver_node.h
index 2b9dfad60cc6930ada94887fa3065cdb27298d3b..c4b1b5a8a42ba624d970c57e907172cad98e63e6 100644
--- a/qr_detector/include/qr_detector_driver_node.h
+++ b/qr_detector/include/qr_detector_driver_node.h
@@ -39,6 +39,7 @@
#include <tf/transform_broadcaster.h>
// [service client headers]
+#include <humanoid_common_msgs/set_qr_size.h>
// [action server client headers]
@@ -84,6 +85,12 @@ class QrDetectorDriverNode : public iri_base_driver::IriBaseNodeDriver<QrDetecto
tf::TransformBroadcaster tag_pose_br;
// [service attributes]
+ ros::ServiceServer set_qr_size_server_;
+ bool set_qr_sizeCallback(humanoid_common_msgs::set_qr_size::Request &req, humanoid_common_msgs::set_qr_size::Response &res);
+ pthread_mutex_t set_qr_size_mutex_;
+ void set_qr_size_mutex_enter(void);
+ void set_qr_size_mutex_exit(void);
+
// [client attributes]
diff --git a/qr_detector/package.xml b/qr_detector/package.xml
index 60ea206ce6335bfc1e3bf0d3ecf543ada777de55..a7a9e394d3ec4b8c29e9422206b22bc417f04145 100644
--- a/qr_detector/package.xml
+++ b/qr_detector/package.xml
@@ -41,11 +41,13 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_driver</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<run_depend>iri_base_driver</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>cv_bridge</run_depend>
diff --git a/qr_detector/src/qr_detector_driver.cpp b/qr_detector/src/qr_detector_driver.cpp
index 9b03f67ab7175f9d477c6ffd15c9bcb3f6747d98..93f8d5ee806faa20dcdd4a4f9d22d70202007a49 100644
--- a/qr_detector/src/qr_detector_driver.cpp
+++ b/qr_detector/src/qr_detector_driver.cpp
@@ -87,6 +87,11 @@ void QrDetectorDriver::init(double fx, double fy, double cx, double cy)
this->detector->init(fx,fy,cx,cy,this->config_.qr_x,this->config_.qr_y);
}
+void QrDetectorDriver::set_qr_size(double qr_x,double qr_y)
+{
+ this->detector->set_qr_size(qr_x,qr_y);
+}
+
void QrDetectorDriver::findQR(cv::Mat& grey,std::vector<std::string> &tag_ids, std::vector< std::vector<double> >& poses)
{
std::vector<TQRInfo> tags;
diff --git a/qr_detector/src/qr_detector_driver_node.cpp b/qr_detector/src/qr_detector_driver_node.cpp
index b5831053f937ffdf2727c7561cadb651934128c3..235e4e503fd29abcd85d831d22330c2e836a4631 100644
--- a/qr_detector/src/qr_detector_driver_node.cpp
+++ b/qr_detector/src/qr_detector_driver_node.cpp
@@ -19,6 +19,9 @@ QrDetectorDriverNode::QrDetectorDriverNode(ros::NodeHandle &nh) :
// [init services]
+ this->set_qr_size_server_ = this->public_node_handle_.advertiseService("set_qr_size", &QrDetectorDriverNode::set_qr_sizeCallback, this);
+ pthread_mutex_init(&this->set_qr_size_mutex_,NULL);
+
// [init clients]
@@ -33,16 +36,9 @@ void QrDetectorDriverNode::mainNodeThread(void)
this->driver_.lock();
// [fill msg Header if necessary]
- //<publisher_name>.header.frame_id = "<publisher_topic_name>";
// [fill msg structures]
-
- //detector.findSquares(gray, squares);
- //detector.drawSquares(frame, squares);
- // publish frame
-
-
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
@@ -99,15 +95,6 @@ void QrDetectorDriverNode::image_callback(const sensor_msgs::Image::ConstPtr& ms
{
ROS_INFO("Camera not configured. camera_info not received");
}
- //use appropiate mutex to shared variables if necessary
- //this->driver_.lock();
- //this->image_mutex_enter();
-
- //std::cout << msg->data << std::endl;
-
- //unlock previously blocked shared variables
- //this->driver_.unlock();
- //this->image_mutex_exit();
}
void QrDetectorDriverNode::image_mutex_enter(void)
@@ -123,8 +110,6 @@ void QrDetectorDriverNode::camera_info_callback(const sensor_msgs::CameraInfo::C
{
ROS_INFO("QrDetectorDriverNode::camera_info_callback: New Message Received");
- //use appropiate mutex to shared variables if necessary
- //this->driver_.lock();
this->camera_info_mutex_enter();
double fx,fy,cx,cy;
@@ -135,10 +120,6 @@ void QrDetectorDriverNode::camera_info_callback(const sensor_msgs::CameraInfo::C
this->driver_.init(fx,fy,cx,cy);
this->camera_info_subscriber_.shutdown();
cam_info_received = true;
- //std::cout << msg->data << std::endl;
-
- //unlock previously blocked shared variables
- //this->driver_.unlock();
this->camera_info_mutex_exit();
}
@@ -153,6 +134,31 @@ void QrDetectorDriverNode::camera_info_mutex_exit(void)
}
/* [service callbacks] */
+bool QrDetectorDriverNode::set_qr_sizeCallback(humanoid_common_msgs::set_qr_size::Request &req, humanoid_common_msgs::set_qr_size::Response &res)
+{
+ ROS_INFO("QrDetectorDriverNode::set_qr_sizeCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ this->driver_.lock();
+
+ this->driver_.set_qr_size(req.qr_x,req.qr_y);
+ res.success=true;
+
+ this->driver_.unlock();
+
+ return true;
+}
+
+void QrDetectorDriverNode::set_qr_size_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->set_qr_size_mutex_);
+}
+
+void QrDetectorDriverNode::set_qr_size_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->set_qr_size_mutex_);
+}
+
/* [action callbacks] */
@@ -185,6 +191,7 @@ void QrDetectorDriverNode::reconfigureNodeHook(int level)
QrDetectorDriverNode::~QrDetectorDriverNode(void)
{
// [free dynamic memory]
+ pthread_mutex_destroy(&this->set_qr_size_mutex_);
pthread_mutex_destroy(&this->image_mutex_);
pthread_mutex_destroy(&this->camera_info_mutex_);
}