diff --git a/humanoid_modules/include/humanoid_modules/search_module.h b/humanoid_modules/include/humanoid_modules/search_module.h
index 2aea8148a868450de8dd1e4596a59a588ac978eb..cea1d98e0d436e6b7e559f2903d20d5da008dcf5 100644
--- a/humanoid_modules/include/humanoid_modules/search_module.h
+++ b/humanoid_modules/include/humanoid_modules/search_module.h
@@ -22,8 +22,8 @@
// 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_
+#ifndef _SEARCH_MODULE_H_
+#define _SEARCH_MODULE_H_
#include <iri_ros_tools/watchdog.h>
#include <tf/tf.h>
@@ -86,6 +86,7 @@ class CSearchModule : public CModule<search_module::SearchModuleConfig>
double pan_angle;
double tilt_angle;
bool qr_detected;
+ std::string qr_id;
//odometry
bool initial_odometry;
bool stop_rotating;
@@ -115,14 +116,14 @@ class CSearchModule : public CModule<search_module::SearchModuleConfig>
//WALK MODULE
CWalkModule walk_module;
-protected:
- void state_machine(void);
- void reconfigure_callback(search_module::SearchModuleConfig &config, uint32_t level);
+ 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 start_search(std::string qr_id, double angle_rotation, double error, const double head_speed=0.5, const double head_acceleration=0.0, const double yaw_step=0.1);
void stop_search(void);
void get_position(double &pan_pos, double &tilt_pos);
bool is_finished(void);
@@ -132,8 +133,6 @@ protected:
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
index 204351a963895b0d401d1bab459237d24a664127..fa809f5af98f5c1496eb2fffc344ddb2b75519d0 100644
--- a/humanoid_modules/src/search_module.cpp
+++ b/humanoid_modules/src/search_module.cpp
@@ -4,7 +4,7 @@
* Moves head 180º. Since -pi/2 to pi/2
* Rotates body 180º
*
- PROBLEMAS: wathdog timer crea expcepcion y se queda bloqueado. contador
+ Se podria centrar la camara al rotar y que tambien realizase un search mientras gira, asi cuando lo encontrase pararia de frente al qr
*/
@@ -13,10 +13,12 @@
#define PI 3.14159
CSearchModule::CSearchModule(const std::string &name) : CModule(name),
- joints_trajectory_module("joint_trajectory"),
- walk_module("walk")
+ joints_trajectory_module(name+"/joint_trajectory"),
+ walk_module(name+"/walk")
{
+ this->start_operation();
+
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);
@@ -37,7 +39,7 @@ CSearchModule::CSearchModule(const std::string &name) : CModule(name),
this->yaw_step=0.1;
this->rotate_angle = PI; //default: turn 180º
- this->pan_angle_search = rotate_angle/2;
+ this->pan_angle_search = (rotate_angle/2)-0.1;
this->total_rotations = (PI/pan_angle_search)-1;
this->tilt_angle_search=0.8;
@@ -62,8 +64,8 @@ CSearchModule::CSearchModule(const std::string &name) : CModule(name),
void CSearchModule::state_machine(void)
{
- unsigned int num_rotations;
- static signed int sign;
+ static unsigned int num_rotations=0;
+ static signed int sign=-1;
if(this->cancel_search)
{
@@ -142,7 +144,6 @@ void CSearchModule::state_machine(void)
{
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;
@@ -176,6 +177,7 @@ void CSearchModule::state_machine(void)
break;
case SEARCH_MODULE_START_ROTATE: ROS_INFO("CSearchModule: state START ROTATION");
+ ROS_INFO("num rotations: %d, total_rotations: %d", num_rotations, total_rotations);
if(num_rotations==total_rotations)
{
ROS_INFO("CSearchmodule: Search completed unsuccessfully");
@@ -185,7 +187,7 @@ void CSearchModule::state_machine(void)
else
{
num_rotations++;
- // ROS_INFO("num rotations: %d", 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);
@@ -225,24 +227,26 @@ void CSearchModule::reconfigure_callback(search_module::SearchModuleConfig &conf
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)
+void CSearchModule::start_search(std::string qr_id, 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;
+ /* QR ID to search */
+ this->qr_id=qr_id;
+ /* rad of rotation */
+ this->rotate_angle = angle_rotation; //Angle of rotation
+ this->pan_angle_search = (this->rotate_angle/2)-0.1; //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)
@@ -282,22 +286,6 @@ walk_module_status_t CSearchModule::get_walk_module_status(void)
}
/* 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)
{
@@ -341,6 +329,8 @@ void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array:
if(msg->tags.size()>0)
{
+ if(this->qr_id==msg->tags[0].tag_id)
+ {
ROS_INFO("QR detected");
//qr_detected=msg->tags.size();
qr_detected=true;
@@ -358,6 +348,7 @@ void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array:
}
*/
this->watchdog_qr_lost.reset(ros::Duration(5));
+ }
}else{
//ROS_INFO("QR not detected");
qr_detected=false;
@@ -402,6 +393,24 @@ void CSearchModule::joint_states_callback(const sensor_msgs::JointState::ConstPt
}
+/* functions */
+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;
+}
+
CSearchModule::~CSearchModule(void)
{
if(!this->is_finished())