diff --git a/humanoid_modules/include/humanoid_modules/search_module.h b/humanoid_modules/include/humanoid_modules/search_module.h
index cea1d98e0d436e6b7e559f2903d20d5da008dcf5..83e7e4323c1cb17ee90f117fc2494679af1a41e6 100644
--- a/humanoid_modules/include/humanoid_modules/search_module.h
+++ b/humanoid_modules/include/humanoid_modules/search_module.h
@@ -39,6 +39,7 @@
//MODULES
#include <humanoid_modules/joints_module.h>
#include <humanoid_modules/walk_module.h>
+#include <humanoid_modules/action_module.h>
//states
typedef enum {SEARCH_MODULE_IDLE,
@@ -115,6 +116,8 @@ class CSearchModule : public CModule<search_module::SearchModuleConfig>
//WALK MODULE
CWalkModule walk_module;
+ //Actio module
+ CActionModule action_module;
protected:
void state_machine(void);
@@ -123,7 +126,7 @@ class CSearchModule : public CModule<search_module::SearchModuleConfig>
CSearchModule(const std::string &name);
/*control functions */
- 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 start_search(std::string qr_id, double angle_rotation, double error, const double tilt_angle=0.8, 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);
diff --git a/humanoid_modules/src/search_module.cpp b/humanoid_modules/src/search_module.cpp
index fa809f5af98f5c1496eb2fffc344ddb2b75519d0..2f9d7b9ebf11648acf846566475151a2fbde3006 100644
--- a/humanoid_modules/src/search_module.cpp
+++ b/humanoid_modules/src/search_module.cpp
@@ -14,7 +14,8 @@
CSearchModule::CSearchModule(const std::string &name) : CModule(name),
joints_trajectory_module(name+"/joint_trajectory"),
- walk_module(name+"/walk")
+ walk_module(name+"/walk"),
+ action_module(name+"action")
{
this->start_operation();
@@ -144,6 +145,8 @@ void CSearchModule::state_machine(void)
{
if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_SUCCESS)
{
+ //stand up
+ this->action_module.execute(16);
angle_feedback=this->joints_trajectory_module.get_current_angle();
//ROS_INFO("Angulo pan: %f", angle_feedback[0]);
this->state=SEARCH_MODULE_START_ROTATE;
@@ -177,21 +180,25 @@ void CSearchModule::state_machine(void)
break;
case SEARCH_MODULE_START_ROTATE: ROS_INFO("CSearchModule: state START ROTATION");
+ if(!this->action_module.is_finished())
+ this->state=SEARCH_MODULE_START_ROTATE;
+ else{
ROS_INFO("num rotations: %d, total_rotations: %d", num_rotations, total_rotations);
- 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;
+ 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;
@@ -227,14 +234,15 @@ void CSearchModule::reconfigure_callback(search_module::SearchModuleConfig &conf
this->unlock();
}
/* control functions */
-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)
+void CSearchModule::start_search(std::string qr_id, double angle_rotation, double error, const double tilt_angle, const double head_speed, const double head_acceleration, const double yaw_step)
{
/* 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->tilt_angle_search = -this->pan_angle_search; //tilt angle to search
+ this->angle[1]=tilt_angle;
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);
@@ -339,8 +347,8 @@ void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array:
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));
+ // 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)
{