Skip to content
Snippets Groups Projects
Commit d325541f authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

Added the action module to the search module to stand up the robot before rotating.

parent 51242799
No related branches found
No related tags found
1 merge request!4Filtered localization
Hide whitespace changes
Inline Side-by-side
humanoid_modules/include/humanoid_modules/search_module.h
+ 4
− 1
View file @ d325541f
......@@ -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);
......
humanoid_modules/src/search_module.cpp
+ 27
− 19
View file @ d325541f
......@@ -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)
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment