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

Added a QR filter on the search and tracking algorithms to select a list of QRto found/follow

parent a1d65d0e
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
+ 3
− 2
View file @ b64aaff4
......@@ -87,7 +87,8 @@ class CSearchModule : public CModule<search_module::SearchModuleConfig>
double pan_angle;
double tilt_angle;
bool qr_detected;
std::string qr_id;
double qr_follow;
std::vector<std::string> qr_id;
//odometry
bool initial_odometry;
bool stop_rotating;
......@@ -126,7 +127,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 tilt_angle=0.8, const double head_speed=0.5, const double head_acceleration=0.0, const double yaw_step=0.1);
void start_search(std::vector<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/include/humanoid_modules/tracking_module.h
+ 3
− 2
View file @ b64aaff4
......@@ -102,7 +102,8 @@ class CTrackingModule : public CModule<tracking_module::TrackingModuleConfig>
double tilt_angle;
bool tracking;
bool qr_detected;
std::string qr_id;
std::vector<std::string> qr_id;
double qr_follow;
CROSWatchdog watchdog_qr_lost;
//PID
TPID z_distance;
......@@ -138,7 +139,7 @@ class CTrackingModule : public CModule<tracking_module::TrackingModuleConfig>
public:
CTrackingModule(const std::string &name);
/* control functions */
void start_tracking(std::string qr_id, double z_target, double x_target, double yaw_target, double z_tolerance, double x_tolerance, double yaw_tolerance);
void start_tracking(std::vector<std::string> qr_id, double z_target, double x_target, double yaw_target, double z_tolerance, double x_tolerance, double yaw_tolerance);
void stop_tracking(void);
bool is_finished(void);
void configure_pid(double yaw_kp, double z_kp, double x_kp/*, double ki, double kd*/);
......
humanoid_modules/src/search_module.cpp
+ 28
− 13
View file @ b64aaff4
......@@ -37,6 +37,7 @@ CSearchModule::CSearchModule(const std::string &name) : CModule(name),
this->current_tilt_angle=0.0;
this->qr_detected=false;
this->qr_follow=0;
this->yaw_step=0.1;
this->rotate_angle = PI; //default: turn 180º
......@@ -137,7 +138,7 @@ void CSearchModule::state_machine(void)
{
if(!this->joints_trajectory_module.is_finished())
{
ROS_INFO("CSearchModule: Searching...");
//ROS_INFO("CSearchModule: Searching...");
this->state=SEARCH_MODULE_SEARCH;
angle_feedback=this->joints_trajectory_module.get_current_angle();
}
......@@ -234,9 +235,10 @@ 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 tilt_angle, const double head_speed, const double head_acceleration, const double yaw_step)
void CSearchModule::start_search(std::vector<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.clear();
this->qr_id=qr_id;
/* rad of rotation */
this->rotate_angle = angle_rotation; //Angle of rotation
......@@ -335,18 +337,33 @@ void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array:
//this->alg_.lock();
this->qr_pose_access.enter();
unsigned int i,j;
if(msg->tags.size()>0)
{
if(this->qr_id==msg->tags[0].tag_id)
//QR filter
for(i=0; i<msg->tags.size(); i++)
{
ROS_INFO("QR detected");
//qr_detected=msg->tags.size();
qr_detected=true;
//angle_feedback=this->joints_trajectory_module.get_current_angle();
for(j=0; j<qr_id.size(); j++)
{
if(this->qr_id[j]==msg->tags[i].tag_id)
{
ROS_WARN("CSearchModule:: QR with ID %s is found", msg->tags[i].tag_id.c_str());
ROS_WARN("CSearchModule:: QR DISTANCE %f", msg->tags[i].position.z);
// ROS_WARN();
qr_follow=i; //save ID index of the first QR detected
qr_detected=true;
}
if(qr_detected)
break;
}
if(qr_detected)
break;
}
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);
//angle_feedback=this->joints_trajectory_module.get_current_angle();
this->pan_angle=this->current_pan_angle+atan2(msg->tags[qr_follow].position.x,msg->tags[qr_follow].position.z);
this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[qr_follow].position.y,msg->tags[qr_follow].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));
/*
......@@ -355,12 +372,10 @@ void CSearchModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array:
send_head_target(pan_angle, tilt_angle);
}
*/
this->watchdog_qr_lost.reset(ros::Duration(5));
}
this->watchdog_qr_lost.reset(ros::Duration(5));
}else{
//ROS_INFO("QR not detected");
qr_detected=false;
}
......
humanoid_modules/src/tracking_module.cpp
+ 51
− 23
View file @ b64aaff4
......@@ -43,7 +43,7 @@ CTrackingModule::CTrackingModule(const std::string &name) : CModule(name),
z_distance.distance_target=0.0;
z_distance.prev_error=0.0;
z_distance.tolerance=0.0;
x_distance.kp=0.2;
x_distance.kp=0.1;
x_distance.ki=0.0;
x_distance.kd=0.0;
x_distance.qr_position=0.0;
......@@ -58,7 +58,7 @@ CTrackingModule::CTrackingModule(const std::string &name) : CModule(name),
orientation.prev_error=0.0;
orientation.tolerance=0.0010;
qr_follow=0;
}
void CTrackingModule::state_machine(void)
......@@ -287,7 +287,7 @@ void CTrackingModule::state_machine(void)
// Update pid
pid(z_distance, 0.01);
pid(orientation,0.2);
pid(orientation,0.08);
pid(x_distance, 0.2);
//si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
if((fabs(this->orientation.error) < this->orientation.tolerance)) //No parar la rotación hasta que este cerca (25 cm de error)
......@@ -315,9 +315,12 @@ void CTrackingModule::state_machine(void)
else
this->state=TRACK_MODULE_TRACK;
ROS_WARN("Error YAW: %f, PID: %f", orientation.error, orientation.pid_out);
ROS_WARN("Error Z: %f, PID: %f", z_distance.error, z_distance.pid_out);
ROS_INFO("Error X: %f", x_distance.error);
ROS_INFO("Error YAW: %f, PID: %f", orientation.error, orientation.pid_out);
ROS_INFO("Error Z: %f, PID: %f", z_distance.error, z_distance.pid_out);
//ROS_INFO("Error X: %f", x_distance.error);
// ROS_WARN("QR Z DISTANCE: %f", z_distance.qr_position);
// ROS_WARN("QR ORIENTATION: %f", orientation.qr_position);
// ROS_WARN("QR X DISTANCE: %f", x_distance.qr_position);
//send cmd_vel
walk_module.set_steps_size(-z_distance.pid_out,0.0,orientation.pid_out);
......@@ -419,15 +422,17 @@ void CTrackingModule::state_machine(void)
*/
case ORIENTATE: ROS_INFO("CTrackingModule: state ORIENTATE");
pid(orientation,0.1);
pid(orientation,0.06);
ROS_INFO("error: %f, qr_position: %f", orientation.error, orientation.qr_position);
if((fabs(this->orientation.error) < this->orientation.tolerance)) //No parar la rotación hasta que este cerca (25 cm de error)
{
ROS_INFO("CTrackingModule: Stop rotating");
orientation.pid_out=0.0;
walk_module.stop();
this->status=TRACK_MODULE_SUCCESS;
this->state=TRACK_MODULE_END;
}
else
{
walk_module.set_steps_size(0.0,0.0,orientation.pid_out);
......@@ -437,7 +442,6 @@ void CTrackingModule::state_machine(void)
case TRACK_MODULE_END: ROS_INFO("CTrackingModule: state END");
//this->tracking=false;
this->status=TRACK_MODULE_SUCCESS;
head_tracking_module.stop_tracking();
if(head_tracking_module.is_finished() && walk_module.is_finished())
this->state=TRACK_MODULE_IDLE;
......@@ -461,18 +465,19 @@ void CTrackingModule::reconfigure_callback(tracking_module::TrackingModuleConfig
}
//Start tracking state machine
void CTrackingModule::start_tracking(std::string qr_id, double z_target, double x_target, double yaw_target, double z_tolerance, double x_tolerance, double yaw_tolerance)
void CTrackingModule::start_tracking(std::vector<std::string> qr_id, double z_target, double x_target, double yaw_target, double z_tolerance, double x_tolerance, double yaw_tolerance)
{
/* Activate state machine */
this->new_track=true;
/* qr id to track */
this->qr_id.clear();
this->qr_id=qr_id;
/* set desired distance */
z_distance.distance_target=z_target;
x_distance.distance_target=x_target;
//orientation.distance_target=yaw_target;
this->yaw_target=yaw_target;
orientation.distance_target=0.0;
orientation.distance_target=-0.1;
/* set desired tolerance */
z_distance.tolerance=z_tolerance;
x_distance.tolerance=x_tolerance;
......@@ -589,18 +594,40 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
{
this->qr_pose_access.enter();
unsigned int i,j;
//QR filter
if(msg->tags.size()>0)
{
watchdog_qr_lost.reset(ros::Duration(1));
if(this->qr_id==msg->tags[0].tag_id)
{
// qr_detected=true;
for(i=0; i<msg->tags.size(); i++)
{
for(j=0; j<qr_id.size(); j++)
{
if(this->qr_id[j]==msg->tags[i].tag_id)
{
ROS_WARN("CTrackingModule:: QR with ID %s is found", msg->tags[i].tag_id.c_str());
ROS_WARN("CTrackingModule:: QR DISTANCE %f", msg->tags[i].position.z);
this->qr_follow=i; //save ID index of the first QR detected
this->qr_detected=true;
}
if(qr_detected)
break;
}
if(qr_detected)
break;
}
//If some QR of the entered list is detected:
if(qr_detected)
{
watchdog_qr_lost.reset(ros::Duration(4));
/* update pan and tilt*/
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);
head_tracking_module.update_target(pan_angle, tilt_angle);
// update pan and tilt
this->pan_angle=this->current_pan_angle+atan2(msg->tags[qr_follow].position.x,msg->tags[qr_follow].position.z);
this->tilt_angle=this->current_tilt_angle+atan2(msg->tags[qr_follow].position.y,msg->tags[qr_follow].position.z);
head_tracking_module.update_target(pan_angle, tilt_angle);
/* ROS_WARN("QR X: %f", (msg->tags[0].position.x));
ROS_WARN("QR Y: %f", (msg->tags[0].position.y));
......@@ -615,6 +642,7 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
*/
// this->tf_transform(msg->tags[0].position.x, msg->tags[0].position.y, msg->tags[0].position.z, msg->tags[0].orientation);
//Obtain QR position with respect robot's body
std::string source_frame="darwin/darwin_cam_optical_link";
std::string target_frame="darwin/base_link";
......@@ -653,11 +681,11 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
ROS_INFO("YAW: %f cm", (orientation.qr_position)*100);
*/
}
qr_detected=true;
}
// qr_detected=false;
}
}else{
ROS_INFO("Qr NOT detected");
ROS_INFO("CTrackingModule:: QR NOT detected");
qr_detected=false;
}
......
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