From b64aaff44bd5ac8511974506461e5d95ac50ddf1 Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Thu, 28 Sep 2017 17:08:30 +0200
Subject: [PATCH] Added a QR filter on the search and tracking algorithms to
select a list of QRto found/follow
---
.../include/humanoid_modules/search_module.h | 5 +-
.../humanoid_modules/tracking_module.h | 5 +-
humanoid_modules/src/search_module.cpp | 41 ++++++----
humanoid_modules/src/tracking_module.cpp | 74 +++++++++++++------
4 files changed, 85 insertions(+), 40 deletions(-)
diff --git a/humanoid_modules/include/humanoid_modules/search_module.h b/humanoid_modules/include/humanoid_modules/search_module.h
index 83e7e43..298e50e 100644
--- a/humanoid_modules/include/humanoid_modules/search_module.h
+++ b/humanoid_modules/include/humanoid_modules/search_module.h
@@ -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);
diff --git a/humanoid_modules/include/humanoid_modules/tracking_module.h b/humanoid_modules/include/humanoid_modules/tracking_module.h
index a66088e..b5e453f 100644
--- a/humanoid_modules/include/humanoid_modules/tracking_module.h
+++ b/humanoid_modules/include/humanoid_modules/tracking_module.h
@@ -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*/);
diff --git a/humanoid_modules/src/search_module.cpp b/humanoid_modules/src/search_module.cpp
index 2f9d7b9..4857a6c 100644
--- a/humanoid_modules/src/search_module.cpp
+++ b/humanoid_modules/src/search_module.cpp
@@ -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;
-
}
diff --git a/humanoid_modules/src/tracking_module.cpp b/humanoid_modules/src/tracking_module.cpp
index 932b64d..afebcfc 100644
--- a/humanoid_modules/src/tracking_module.cpp
+++ b/humanoid_modules/src/tracking_module.cpp
@@ -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;
}
--
GitLab