diff --git a/humanoid_modules/CMakeLists.txt b/humanoid_modules/CMakeLists.txt
index 73b491106eff67fb7288539bdec6449ed5f688de..d50ab03bc9348bb6587ae20e325de1a6fc2baed1 100644
--- a/humanoid_modules/CMakeLists.txt
+++ b/humanoid_modules/CMakeLists.txt
@@ -86,7 +86,7 @@ find_package(iriutils REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
- cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg
+ cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg
)
###################################
@@ -100,7 +100,7 @@ generate_dynamic_reconfigure_options(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module
+ LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module
CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs
# DEPENDS system_lib
)
@@ -176,6 +176,21 @@ target_link_libraries(head_tracking_module ${iriutils_LIBRARY})
## either from message generation or dynamic reconfigure
add_dependencies(head_tracking_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_library(search_module
+ src/search_module.cpp
+)
+
+ target_link_libraries(search_module ${catkin_LIBRARIES})
+ target_link_libraries(search_module ${iriutils_LIBRARY})
+ #target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINTATION}/libjoints_module.so)
+ target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libjoints_module.so)
+ target_link_libraries(search_module ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/libwalk_module.so)
+#
+# ## Add cmake target dependencies of the library
+# ## as an example, code may need to be generated before libraries
+# ## either from message generation or dynamic reconfigure
+add_dependencies(search_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
diff --git a/humanoid_modules/cfg/SearchModule.cfg b/humanoid_modules/cfg/SearchModule.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..579aaec702cfb5d086f41792b1467110c1005634
--- /dev/null
+++ b/humanoid_modules/cfg/SearchModule.cfg
@@ -0,0 +1,49 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='search_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+set_modules = gen.add_group("Set Servo Modules service")
+joint_trajectory_action = gen.add_group("Joint trajectory")
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("rate_hz", double_t, 0, "Joints module rate in Hz", 1.0, 1.0, 100.0)
+gen.add("enable", bool_t, 0, "Enable execution", True)
+
+
+exit(gen.generate(PACKAGE, "SearchModule", "SearchModule"))
diff --git a/humanoid_modules/include/humanoid_modules/search_module.h b/humanoid_modules/include/humanoid_modules/search_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..2aea8148a868450de8dd1e4596a59a588ac978eb
--- /dev/null
+++ b/humanoid_modules/include/humanoid_modules/search_module.h
@@ -0,0 +1,139 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author Irene Garcia Camacho
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef SEARCH_MODULE_H_
+#define SEARCH_MODULE_H_
+
+#include <iri_ros_tools/watchdog.h>
+#include <tf/tf.h>
+
+//#include <iri_base_algorithm/iri_base_algorithm.h>
+#include <humanoid_modules/SearchModuleConfig.h>
+
+// [publisher subscriber headers]
+#include <nav_msgs/Odometry.h>
+#include <humanoid_common_msgs/tag_pose_array.h>
+#include <sensor_msgs/JointState.h>
+
+//MODULES
+#include <humanoid_modules/joints_module.h>
+#include <humanoid_modules/walk_module.h>
+
+//states
+typedef enum {SEARCH_MODULE_IDLE,
+ SEARCH_MODULE_START,
+ SEARCH_MODULE_SEARCH,
+ SEARCH_MODULE_TRACK,
+ SEARCH_MODULE_START_ROTATE,
+ SEARCH_MODULE_ROTATE} search_module_states_t;
+//status
+typedef enum {SEARCH_MODULE_SUCCESS,
+ SEARCH_MODULE_CANCELED,
+ SEARCH_MODULE_FAIL,
+ SEARCH_MODULE_RUNNING,
+ SEARCH_MODULE_ERROR_JOINTS,
+ SEARCH_MODULE_ERROR_WALK } search_module_status_t;
+
+class CSearchModule : public CModule<search_module::SearchModuleConfig>
+{
+ private:
+
+ // [subscriber attributes]
+ ros::Subscriber odom_subscriber_;
+ void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+ CMutex odom_access;
+
+ ros::Subscriber qr_pose_subscriber_;
+ void qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg);
+ CMutex qr_pose_access;
+
+ ros::Subscriber joint_states_subscriber_;
+ void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
+ CMutex joint_state_access;
+
+ search_module::SearchModuleConfig config;
+ search_module_status_t status;
+ search_module_states_t state;
+ bool new_search;
+ bool cancel_search;
+ bool search_success;
+ double current_pan_angle;
+ double current_tilt_angle;
+ double tilt_angle_search;
+ double pan_angle_search;
+ //qr
+ double pan_angle;
+ double tilt_angle;
+ bool qr_detected;
+ //odometry
+ bool initial_odometry;
+ bool stop_rotating;
+ double odom_rot;
+ // double init_odom_rot;
+ // double actual_odom_rot;
+ double yaw_step;
+ unsigned int total_rotations;
+ double odom_target;
+ double rotate_angle;
+ double rotate_deviation (double ini, double actual);
+ double get_target (double a, double b);
+ bool start_odometry;
+ double odom_error;
+
+ CROSWatchdog watchdog_qr_lost;
+
+ std::vector<double> speed;
+ std::vector<double> accel;
+ std::vector<double> angle;
+ std::vector<unsigned int> id;
+
+ ///JOINTS MODULE
+ CJointsModule joints_trajectory_module;
+ std::vector<double> angle_feedback;
+
+ //WALK MODULE
+ CWalkModule walk_module;
+
+protected:
+ void state_machine(void);
+ void reconfigure_callback(search_module::SearchModuleConfig &config, uint32_t level);
+ public:
+
+ CSearchModule(const std::string &name);
+ /*control functions */
+ void start_search(double angle_rotation, double error, const double head_speed=1.0, const double head_acceleration=0.3, const double yaw_step=0.1);
+ void stop_search(void);
+ void get_position(double &pan_pos, double &tilt_pos);
+ bool is_finished(void);
+ /* feedback functions */
+ search_module_status_t get_status(void);
+ joints_module_status_t get_joints_module_status(void);
+ walk_module_status_t get_walk_module_status(void);
+ ~CSearchModule();
+
+ protected:
+
+};
+
+#endif
diff --git a/humanoid_modules/src/search_module.cpp b/humanoid_modules/src/search_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..204351a963895b0d401d1bab459237d24a664127
--- /dev/null
+++ b/humanoid_modules/src/search_module.cpp
@@ -0,0 +1,412 @@
+
+/* SEARCH QR ALGORITHM
+ *
+ * Moves head 180º. Since -pi/2 to pi/2
+ * Rotates body 180º
+ *
+ PROBLEMAS: wathdog timer crea expcepcion y se queda bloqueado. contador
+ */
+
+
+#include "humanoid_modules/search_module.h"
+
+#define PI 3.14159
+
+CSearchModule::CSearchModule(const std::string &name) : CModule(name),
+ joints_trajectory_module("joint_trajectory"),
+ walk_module("walk")
+{
+
+ this->odom_subscriber_ = this->module_nh.subscribe("odom", 1, &CSearchModule::odom_callback, this);
+
+ this->qr_pose_subscriber_ = this->module_nh.subscribe("qr_pose", 1, &CSearchModule::qr_pose_callback, this);
+
+ this->joint_states_subscriber_ = this->module_nh.subscribe("joint_states", 1, &CSearchModule::joint_states_callback, this);
+
+
+ this->status=SEARCH_MODULE_SUCCESS;
+ this->state=SEARCH_MODULE_IDLE;
+ this->new_search=false;
+ this->cancel_search=false;
+ this->search_success=false;
+
+ this->current_pan_angle=0.0;
+ this->current_tilt_angle=0.0;
+
+ this->qr_detected=false;
+
+ this->yaw_step=0.1;
+ this->rotate_angle = PI; //default: turn 180º
+ this->pan_angle_search = rotate_angle/2;
+ this->total_rotations = (PI/pan_angle_search)-1;
+ this->tilt_angle_search=0.8;
+
+ this->angle.resize(2);
+ this->angle[0]=pan_angle_search;
+ this->angle[1]=tilt_angle_search;
+ this->speed.resize(2);
+ this->speed[0]=1;
+ this->speed[1]=1;
+ this->accel.resize(2);
+ this->accel[0]=0.3;
+ this->accel[0]=0.3;
+ this->id.resize(2);
+ this->id[0]=19;
+ this->id[1]=20;
+
+ this->initial_odometry=false;
+ this->stop_rotating=false;
+ this->start_odometry=false;
+ this->odom_error=0.01;
+}
+
+void CSearchModule::state_machine(void)
+{
+ unsigned int num_rotations;
+ static signed int sign;
+
+ if(this->cancel_search)
+ {
+ ROS_INFO("CSearchModule: Search canceled");
+ this->joints_trajectory_module.stop();
+ this->walk_module.stop();
+ this->state=SEARCH_MODULE_IDLE;
+ this->status=SEARCH_MODULE_CANCELED;
+ this->cancel_search=false;
+ }
+ else
+ {
+ switch(this->state)
+ {
+ case SEARCH_MODULE_IDLE: ROS_INFO("CSearchModule: state IDLE");
+ if(this->new_search)
+ {
+ ROS_INFO("CSearchModule: New Search!");
+ this->search_success=false;
+ this->new_search=false;
+ sign=-1;
+ num_rotations=0;
+ this->angle[0]=sign*pan_angle_search;
+ std::vector<double> fast_speed(2,2);
+ this->joints_trajectory_module.execute(id,angle,fast_speed,accel);
+ this->status=SEARCH_MODULE_RUNNING;
+ this->state=SEARCH_MODULE_START;
+ }
+ else
+ this->state=SEARCH_MODULE_IDLE;
+ break;
+
+ case SEARCH_MODULE_START: ROS_INFO("CSearchModule: state START");
+ if(!this->joints_trajectory_module.is_finished())
+ {
+ ROS_INFO("CSearchModule: Going to initial position");
+ state=SEARCH_MODULE_START;
+ }
+ else
+ {
+ if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_SUCCESS)
+ {
+ ROS_INFO("CSearchModule: Arrived to initial position");
+ this->angle[0]=pan_angle_search; //1.5
+ this->joints_trajectory_module.execute(this->id,this->angle,this->speed,this->accel); //start search (go to end position
+ this->state=SEARCH_MODULE_SEARCH;
+ }
+ else
+ {
+ ROS_INFO("CSearchModule: Failed in arriving to initial position");
+ this->status=SEARCH_MODULE_ERROR_JOINTS;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ }
+ break;
+
+ case SEARCH_MODULE_SEARCH: ROS_INFO("CSearchModule: state SEARCH");
+ if(qr_detected)
+ {
+ ROS_INFO("CSearchmodule: QR detected!");
+ ROS_INFO("CSearchmodule: Search completed successfully");
+ this->joints_trajectory_module.stop();
+ this->search_success=true;
+ this->status=SEARCH_MODULE_SUCCESS;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ else
+ {
+ if(!this->joints_trajectory_module.is_finished())
+ {
+ ROS_INFO("CSearchModule: Searching...");
+ this->state=SEARCH_MODULE_SEARCH;
+ angle_feedback=this->joints_trajectory_module.get_current_angle();
+ }
+ else
+ {
+ if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_SUCCESS)
+ {
+ ROS_INFO("CSearchModule: ");
+ angle_feedback=this->joints_trajectory_module.get_current_angle();
+ //ROS_INFO("Angulo pan: %f", angle_feedback[0]);
+ this->state=SEARCH_MODULE_START_ROTATE;
+ }
+ else
+ {
+ ROS_INFO("CSearchModule: Failed in moving head joints");
+ this->status=SEARCH_MODULE_ERROR_JOINTS;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ }
+ }
+ break;
+
+ //Maquina de estados tracking
+ case SEARCH_MODULE_TRACK: ROS_INFO("CSearchModule: state TRACK");
+ if(this->watchdog_qr_lost.is_active())
+ {
+ ROS_INFO("QR lost");
+ // this->tracking=false;
+ // this->head_follow_target_client_.cancelGoal();
+ // this->angle[0]=pan_angle_init;
+ // angle[0]=-1.5;
+ this->joints_trajectory_module.execute(this->id, this->angle, this->speed, this->accel);
+ state=SEARCH_MODULE_START;
+ }
+ else
+ {
+ this->state=SEARCH_MODULE_TRACK;
+ }
+ break;
+
+ case SEARCH_MODULE_START_ROTATE: ROS_INFO("CSearchModule: state START ROTATION");
+ if(num_rotations==total_rotations)
+ {
+ ROS_INFO("CSearchmodule: Search completed unsuccessfully");
+ this->status=SEARCH_MODULE_FAIL;
+ this->state=SEARCH_MODULE_IDLE;
+ }
+ else
+ {
+ num_rotations++;
+ // ROS_INFO("num rotations: %d", num_rotations);
+ // walk_module.add_whole_body();
+ initial_odometry=true;
+ walk_module.set_steps_size(0.0,0.0,this->yaw_step);
+ this->state=SEARCH_MODULE_ROTATE;
+ }
+ break;
+
+ case SEARCH_MODULE_ROTATE: ROS_INFO("CSearchModule: State ROTATE");
+ start_odometry=true;
+ if(stop_rotating)
+ {
+ ROS_INFO("CSearchModule: Stop rotating");
+ stop_rotating=false;
+ start_odometry=false;
+ this->walk_module.stop();
+ this->angle[0]=sign*pan_angle_search;
+ sign=sign*(-1);
+ this->joints_trajectory_module.execute(this->id, this->angle, this->speed, this->accel);
+ this->state=SEARCH_MODULE_SEARCH;
+ }
+ else
+ state=SEARCH_MODULE_ROTATE;
+ break;
+
+ }
+ }
+}
+
+void CSearchModule::reconfigure_callback(search_module::SearchModuleConfig &config, uint32_t level)
+{
+ ROS_INFO("CSearchModule : reconfigure callback");
+ this->lock();
+ this->config=config;
+ /* set the module rate */
+ this->set_rate(config.rate_hz);
+
+ this->unlock();
+}
+/* control functions */
+void CSearchModule::start_search(double angle_rotation, double error, const double head_speed, const double head_acceleration, const double yaw_step)
+{
+ this->rotate_angle = angle_rotation; //Angle of rotation
+ this->pan_angle_search = this->rotate_angle/2; //pan angle to search
+ this->tilt_angle_search = -this->pan_angle_search; //tilt angle to search
+ this->total_rotations = (PI/this->pan_angle_search)-1; //number of iterations
+ this->odom_error = error; //min error in rotation
+ ROS_INFO("Angle rotation: %f, Total rotations: %d, Pan angle: %f", rotate_angle, total_rotations, pan_angle_search);
+ for(int i=0;i<2;i++)
+ {
+ this->speed[i]=head_speed;
+ this->accel[i]=head_acceleration;
+ }
+ this->yaw_step=yaw_step; //Yaw amplitud to rotate
+ ROS_INFO("speed: %f, accel: %f", speed[0], accel[0]);
+
+
+ this->new_search=true;
+}
+
+void CSearchModule::stop_search(void)
+{
+ if(!this->state==SEARCH_MODULE_IDLE)
+ this->cancel_search=true;
+}
+
+bool CSearchModule::is_finished(void)
+{
+ if(this->state==SEARCH_MODULE_IDLE && this->new_search==false)
+ return true;
+ else
+ return false;
+}
+
+void CSearchModule::get_position(double &pan_pos, double &tilt_pos)
+{
+ if(this->search_success)
+ {
+ pan_pos=this->current_pan_angle;
+ tilt_pos=this->current_tilt_angle;
+ }
+}
+/* feedback functions */
+search_module_status_t CSearchModule::get_status(void)
+{
+ return this->status;
+}
+joints_module_status_t CSearchModule::get_joints_module_status(void)
+{
+ return this->joints_trajectory_module.get_status();
+}
+walk_module_status_t CSearchModule::get_walk_module_status(void)
+{
+ return this->walk_module.get_status();
+}
+
+/* callbacks */
+double CSearchModule::rotate_deviation(double ini, double actual)
+{
+ double inc = actual - ini;
+ if (inc<(-PI)) inc+=(2*PI);
+ else if (inc>PI) inc-=(2*PI);
+ return fabs(inc);
+}
+
+double CSearchModule::get_target(double a, double b)
+{
+ double res = a + b;
+ if (res < 0 ) res+= (2*PI);
+ else if (res>(2*PI))
+ res-=(2*PI);
+ return res;
+}
+
+void CSearchModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ double init_odom_rot, actual_odom_rot;
+
+ //this->alg_.lock();
+ this->odom_access.enter();
+
+ //save initial odometry
+ if(initial_odometry)
+ {
+ init_odom_rot=tf::getYaw(msg->pose.pose.orientation);
+ init_odom_rot = init_odom_rot + rotate_angle; //change range (0-2pi) //paso del rango (-pi)-(pi) al rango 0-(2pi)
+ odom_target=get_target(init_odom_rot, PI); //calculate target (segun el angulo que depende de total_rotations) //pi para 1 vuelta 180º
+ initial_odometry=false;
+ }
+
+ else if(start_odometry)
+ {
+ odom_rot=tf::getYaw(msg->pose.pose.orientation); //get actual odometry
+ actual_odom_rot = odom_rot + PI; //change range (0-2pi) //paso del rango (-pi)-(pi) al rango 0-(2pi)
+ if((rotate_deviation(actual_odom_rot,odom_target))<odom_error) //Calculate difference
+ stop_rotating=true;
+
+ // ROS_INFO("TARGET: %f", odom_target);
+ // ROS_INFO("ACTUAL: %f", actual_odom_rot);
+ // ROS_INFO("diferencia: %f", rotate_deviation(actual_odom_rot,odom_target));
+ }
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->odom_access.exit();
+}
+
+void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
+{
+ //ROS_INFO("CSearchModule::qr_pose_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->qr_pose_access.enter();
+
+ if(msg->tags.size()>0)
+ {
+ ROS_INFO("QR detected");
+ //qr_detected=msg->tags.size();
+ qr_detected=true;
+
+ //angle_feedback=this->joints_trajectory_module.get_current_angle();
+
+ this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[0].position.y,msg->tags[0].position.z);
+ ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
+ ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
+ /*
+ if(this->tracking)
+ {
+ send_head_target(pan_angle, tilt_angle);
+ }
+ */
+ this->watchdog_qr_lost.reset(ros::Duration(5));
+ }else{
+ //ROS_INFO("QR not detected");
+ qr_detected=false;
+
+ }
+
+
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->qr_pose_access.exit();
+
+}
+
+void CSearchModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ //ROS_INFO("CSearchModule::joint_states_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->joint_state_access.enter();
+
+
+ unsigned int i;
+
+
+ for (i = 0; i < msg->name.size() ; ++i){
+ if (msg->name[i]=="j_pan"){
+ this->current_pan_angle=msg->position[i];
+ }
+ else if (msg->name[i]=="j_tilt"){
+ this->current_tilt_angle=(msg->position[i]);
+ }
+ }
+
+ // ROS_INFO("current pan: %f", current_pan_angle);
+ // ROS_INFO("current tilt: %f", current_tilt_angle);
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->joint_state_access.exit();
+
+}
+
+CSearchModule::~CSearchModule(void)
+{
+ if(!this->is_finished())
+ {
+ this->stop_search();
+ while(!this->is_finished());
+ }
+}