Merge branch 'smart_charger' of...

Merge branch 'smart_charger' of https://gitlab.iri.upc.edu/humanoides/humanoid_common into smart_charger

humanoid_common_msgs/CMakeLists.txt

@@ -61,6 +61,7 @@ add_service_files(
   get_pid.srv
   set_smart_charger_config.srv
   get_smart_charger_config.srv
+  set_qr_size.srv
 )
 
 ## Generate actions in the 'action' folder

humanoid_common_msgs/srv/set_qr_size.srv

+float32 qr_x
+float32 qr_y
+---
+bool success

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)
 # ******************************************************************** 

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);
 };

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]
 

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>

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;

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_);
 }