From 9046b47cc426a762449e002e8219b2af0852babe Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Fri, 7 Jul 2017 16:33:55 +0200
Subject: [PATCH] Minor changes in the tracking module.
---
humanoid_modules/src/tracking_module.cpp | 305 ++++-------------------
1 file changed, 52 insertions(+), 253 deletions(-)
diff --git a/humanoid_modules/src/tracking_module.cpp b/humanoid_modules/src/tracking_module.cpp
index 6999037..932b64d 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.1;
+ x_distance.kp=0.2;
x_distance.ki=0.0;
x_distance.kd=0.0;
x_distance.qr_position=0.0;
@@ -282,49 +282,46 @@ void CTrackingModule::state_machine(void)
walk_module.set_steps_size(z_distance.pid_out,x_distance.pid_out,orientation.pid_out);
break;
*/
- /*
- case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
+
+ case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
// Update pid
pid(z_distance, 0.01);
pid(orientation,0.2);
- pid(x_distance, 0.005);
- // pid_yaw(orientation_x);
+ 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)
{
ROS_INFO("CTrackingModule: Stop rotating");
orientation.pid_out=0.0;
}
- if(this->z_distance.error < this->z_distance.tolerance)
+ if((fabs(this->z_distance.error) < this->z_distance.tolerance))
{
ROS_INFO("CTrackingModule: Stop walking Z");
z_distance.pid_out=0.0;
}
- if((fabs(this->x_distance.error)>this->x_distance.tolerance) && z_distance.error>0.3) //si la distancia x es mayor a la tolerancia
+ if((fabs(this->x_distance.error) > this->x_distance.tolerance) && (fabs(z_distance.error)>0.3)) //si la distancia x es mayor a la tolerancia
{
ROS_INFO("CTrackingModule: Rotate for X");
- pid_yaw(orientation); //mover yaw dependiendo de distancia x
- // x_distance.pid_out=0.0;
+ orientation.pid_out=-x_distance.pid_out;
+ //pid_yaw(orientation); //mover yaw dependiendo de distancia x
}
- if(orientation.pid_out==0.0 && z_distance.pid_out==0.0*/ /*&& x_distance.pid_out==0.0*/ /* )
+ if(orientation.pid_out==0.0 && z_distance.pid_out==0.0)
{
ROS_INFO("CTrackingModule: Stop aproaching");
- walk_module.stop();
- ROS_WARN("X: %f cm", (x_distance.qr_position)*100); //Distancia X QR
- ROS_WARN("Y: %f cm", (z_distance.qr_position)*100); //Distancia Z QR
- ROS_WARN("YAW: %f", (orientation.qr_position)*100);
- this->state=TRACK_MODULE_END;
- this->status=TRACK_MODULE_SUCCESS;
+ orientation.distance_target=this->yaw_target;
+ this->state=ORIENTATE;
}
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, PID: %f", x_distance.error, x_distance.pid_out);
+ ROS_INFO("Error X: %f", x_distance.error);
+
+ //send cmd_vel
+ walk_module.set_steps_size(-z_distance.pid_out,0.0,orientation.pid_out);
- walk_module.set_steps_size(z_distance.pid_out,0.0,orientation.pid_out);
// check if walk module failed
if(walk_module.is_finished())
@@ -343,20 +340,21 @@ void CTrackingModule::state_machine(void)
// check if qr was lost
if(this->watchdog_qr_lost.is_active())
{
- ROS_INFO("CTrackingModule: QR lost!");
+ ROS_ERROR("CTrackingModule: QR lost!");
walk_module.stop(); //stop walking
this->state=TRACK_MODULE_END;
this->status=TRACK_MODULE_LOST;
}
- break; */
-
- case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
+ break;
+
+ /*
+ case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
// Update pid
pid(z_distance, 0.01);
pid(orientation,0.2);
- pid(x_distance, 0.005);
+ pid(x_distance, 0.01);
// pid_yaw(orientation_x);
//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)
@@ -364,18 +362,17 @@ void CTrackingModule::state_machine(void)
ROS_INFO("CTrackingModule: Stop rotating");
orientation.pid_out=0.0;
}
- if(this->z_distance.error < this->z_distance.tolerance)
+ if((fabs(this->z_distance.error) < this->z_distance.tolerance))
{
ROS_INFO("CTrackingModule: Stop walking Z");
z_distance.pid_out=0.0;
}
- if((fabs(this->x_distance.error)>this->x_distance.tolerance) && z_distance.error>0.3) //si la distancia x es mayor a la tolerancia
+ if((fabs(this->x_distance.error) < this->x_distance.tolerance)) //si la distancia x es mayor a la tolerancia
{
- ROS_INFO("CTrackingModule: Rotate for X");
- pid_yaw(orientation); //mover yaw dependiendo de distancia x
- // x_distance.pid_out=0.0;
+ ROS_INFO("CTrackingModule: Stop walking X");
+ x_distance.pid_out=0.0;
}
- if(z_distance.pid_out==0.0)
+ if(orientation.pid_out==0.0 && z_distance.pid_out==0.0 && x_distance.pid_out==0.0)
{
ROS_INFO("CTrackingModule: Stop aproaching");
@@ -393,7 +390,7 @@ void CTrackingModule::state_machine(void)
ROS_WARN("Error Z: %f, PID: %f", z_distance.error, z_distance.pid_out);
ROS_INFO("Error X: %f, PID: %f", x_distance.error, x_distance.pid_out);
- walk_module.set_steps_size(z_distance.pid_out,0.0,orientation.pid_out);
+ walk_module.set_steps_size(-z_distance.pid_out,-x_distance.pid_out,orientation.pid_out);
// check if walk module failed
if(walk_module.is_finished())
@@ -419,7 +416,8 @@ void CTrackingModule::state_machine(void)
}
break;
-
+ */
+
case ORIENTATE: ROS_INFO("CTrackingModule: state ORIENTATE");
pid(orientation,0.1);
ROS_INFO("error: %f, qr_position: %f", orientation.error, orientation.qr_position);
@@ -567,10 +565,6 @@ void CTrackingModule::update_pid(void)
/* callbacks */
void CTrackingModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
- //ROS_INFO("CTrackingModule::joint_states_callback: New Message Received");
-
- //use appropiate mutex to shared variables if necessary
- //this->alg_.lock();
this->joint_states_access.enter();
unsigned int i;
@@ -586,65 +580,17 @@ void CTrackingModule::joint_states_callback(const sensor_msgs::JointState::Const
this->current_tilt_angle=(msg->position[i]);
}
}
-
- //unlock previously blocked shared variables
- //this->alg_.unlock();
+
this->joint_states_access.exit();
}
-/*
-void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
-{
- //ROS_INFO("CTrackingModule::qr_pose_callback: New Message Received");
-
- //use appropiate mutex to shared variables if necessary
- //this->alg_.lock();
- this->qr_pose_mutex_enter();
-
- if(msg->tags.size()>0)
- {
- //qr_detected=msg->tags.size();
- qr_detected=true;
- z_distance.qr_position=(msg->tags[0].position.z)*100;
- x_distance.qr_position=(msg->tags[0].position.x)*100;
- //qr_position_y=(msg->tags[0].position.y)*100;
- //ROS_INFO("qr posicion Z: %f", z_distance.qr_position);
- // ROS_INFO("qr posicion X: %f", x_distance.qr_position);
-
- 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));
-
- //if(this->tracking)
- //{
- ROS_INFO("Send new head target!");
- //send_head_target(pan_angle, tilt_angle);
-
- head_tracking_module.update_target(pan_angle, tilt_angle);
- //}
- watchdog_qr_lost.reset(ros::Duration(1));
- //counter=0;
- }else{
- ROS_INFO("Qr NOT detected");
- qr_detected=false;
- //counter++;
- }
-
-
- //unlock previously blocked shared variables
- //this->alg_.unlock();
- this->qr_pose_mutex_exit();
-}
-*/
void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
{
this->qr_pose_access.enter();
-
+
if(msg->tags.size()>0)
{
- /* QR detected */
watchdog_qr_lost.reset(ros::Duration(1));
if(this->qr_id==msg->tags[0].tag_id)
{
@@ -654,19 +600,23 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
/* 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);
- // ROS_INFO("Send new head target!");
head_tracking_module.update_target(pan_angle, tilt_angle);
- /* ROS_INFO("QR X: %f cm", (msg->tags[0].position.x)*100);
- ROS_INFO("QR Y: %f cm", (msg->tags[0].position.y)*100);
- ROS_INFO("QR Z: %f cm", (msg->tags[0].position.z)*100);
- ROS_INFO("QR YAW: %f", tf::getYaw(msg->tags[0].orientation));
- */
-
+/* ROS_WARN("QR X: %f", (msg->tags[0].position.x));
+ ROS_WARN("QR Y: %f", (msg->tags[0].position.y));
+ ROS_WARN("QR Z: %f", (msg->tags[0].position.z)); */
+ /* ROS_INFO("QR YAW: %f", tf::getYaw(msg->tags[0].orientation));
+ double x = msg->tags[0].position.z;
+ double y = -msg->tags[0].position.x;
+ double z = -msg->tags[0].position.y;
+ ROS_WARN("transf X: %f", x);
+ ROS_WARN("transf Y: %f", y);
+ ROS_WARN("transf Z: %f", z);
+ */
// this->tf_transform(msg->tags[0].position.x, msg->tags[0].position.y, msg->tags[0].position.z, msg->tags[0].orientation);
+ std::string source_frame="darwin/darwin_cam_optical_link";
std::string target_frame="darwin/base_link";
- std::string source_frame="darwin/head";
ros::Time target_time = ros::Time::now();
geometry_msgs::PoseStamped in;
@@ -685,20 +635,23 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
in.pose.position.z = msg->tags[0].position.z;
in.pose.orientation = msg->tags[0].orientation;
tf_listener.transformPose(target_frame, in, out);
-
+
x_distance.qr_position=out.pose.position.x;
- z_distance.qr_position=out.pose.position.y;
+ z_distance.qr_position=out.pose.position.z;
double z=out.pose.position.z;
tf::quaternionMsgToTF(out.pose.orientation,tf_quat);
tf_rotation=tf::Matrix3x3(tf_quat);
tf_rotation.getRPY(roll,pitch,yaw);
- orientation.qr_position=pitch;
+ orientation.qr_position=roll;
+/* ROS_WARN("ROLL: %f, PITCH: %f, YAW: %f", roll,pitch,yaw);
+ ROS_WARN("out x: %f, out y: %f, out z: %f",out.pose.position.x,out.pose.position.y,out.pose.position.z );
+
ROS_INFO("X: %f cm", (x_distance.qr_position)*100); //Distancia X QR
ROS_INFO("Y: %f cm", (z_distance.qr_position)*100); //Distancia Z QR
ROS_INFO("YAW: %f cm", (orientation.qr_position)*100);
-
+*/
}
qr_detected=true;
}
@@ -713,78 +666,8 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
}
-/* [service callbacks] */
-
-/* [action callbacks] */
-/*
-void CTrackingModule::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
-{
- alg_.lock();
- if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
- ROS_INFO("CTrackingModule::head_follow_targetDone: Goal Achieved!");
- else
- ROS_INFO("CTrackingModule::head_follow_targetDone: %s", state.toString().c_str());
-
- //copy & work with requested result
- alg_.unlock();
-}
-
-void CTrackingModule::head_follow_targetActive()
-{
- alg_.lock();
- //ROS_INFO("CTrackingModule::head_follow_targetActive: Goal just went active!");
- alg_.unlock();
-}
-
-void CTrackingModule::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
-{
- alg_.lock();
- //ROS_INFO("CTrackingModule::head_follow_targetFeedback: Got Feedback!");
- bool feedback_is_ok = true;
- //analyze feedback
- //my_var = feedback->var;
-
- //if feedback is not what expected, cancel requested goal
- if( !feedback_is_ok )
- {
- head_follow_target_client_.cancelGoal();
- //ROS_INFO("CTrackingModule::head_follow_targetFeedback: Cancelling Action!");
- }
- alg_.unlock();
-}
-*/
-
-/* [action requests] */
-/*
-bool CTrackingModule::head_follow_targetMakeActionRequest()
-{
- // IMPORTANT: Please note that all mutex used in the client callback functions
- // must be unlocked before calling any of the client class functions from an
- // other thread (MainNodeThread).
- // this->alg_.unlock();
- if(head_follow_target_client_.isServerConnected())
- {
- //ROS_DEBUG("CTrackingModule::head_follow_targetMakeActionRequest: Server is Available!");
- //send a goal to the action server
- //head_follow_target_goal_.data = my_desired_goal;
- head_follow_target_client_.sendGoal(head_follow_target_goal_,
- boost::bind(&CTrackingModule::head_follow_targetDone, this, _1, _2),
- boost::bind(&CTrackingModule::head_follow_targetActive, this),
- boost::bind(&CTrackingModule::head_follow_targetFeedback, this, _1));
- // this->alg_.lock();
- // ROS_DEBUG("CTrackingModule::MakeActionRequest: Goal Sent.");
- return true;
- }
- else
- {
- // this->alg_.lock();
- // ROS_DEBUG("CTrackingModule::head_follow_targetMakeActionRequest: HRI server is not connected");
- return false;
- }
-}
-*/
/*
void CTrackingModule::node_config_update(Config &config, uint32_t level)
{
@@ -850,7 +733,7 @@ void CTrackingModule::pid(TPID &axis, double max) //distancia z
// ROS_INFO("time period: %f", time_period);
//Calculate PID
- axis.error = axis.qr_position-axis.distance_target;//axis.error=z_distance_target-qr_position_z;
+ axis.error = axis.distance_target-axis.qr_position;//axis.error=z_distance_target-qr_position_z;
// axis.derivative = (axis.error-axis.prev_error)/time_period;
//axis.integral = (axis.error*time_period); //+=
//res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative;
@@ -882,7 +765,7 @@ void CTrackingModule::pid_yaw(TPID &axis) //distancia z
time_period = (new_time-this->start_time).toSec();
//Calculate PID
- axis.error = x_distance.qr_position-x_distance.distance_target;//axis.error=z_distance_target-qr_position_z;
+ axis.error = x_distance.distance_target-x_distance.qr_position;
// axis.derivative = (axis.error-axis.prev_error)/time_period;
//axis.integral = (axis.error*time_period); //+=
//res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative;
@@ -1014,90 +897,6 @@ void CTrackingModule::pid2(TPID , double &) //distancia z
this->start_time=ros::Time::now();
}
*/
-//Head tracking
-/*
-void CTrackingModule::set_head_tracking_goal(double pan_target, double tilt_target)
-{
- this->head_follow_target_goal_.target_pan=pan_target;
- this->head_follow_target_goal_.target_tilt=tilt_target;
- // this->head_follow_target_goal_.pan_range.resize(2);
-
- this->head_follow_target_goal_.pan_range[0]=1.5;
- this->head_follow_target_goal_.pan_range[1]=-1.5;
- //this->head_follow_target_goal_.tilt_range.resize(2);
- this->head_follow_target_goal_.tilt_range[0]=1;
- this->head_follow_target_goal_.tilt_range[1]=-1;
-}
-
-
-void CTrackingModule::send_head_target(double pan, double tilt)
-{
- //this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
- this->head_target_JointTrajectoryPoint_msg_.positions[0]=pan;
- this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt;
- this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
-}
-*/
-//Walk
-/*
-bool CTrackingModule::set_walk_params(void)
-{
- set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = config_.Y_SWAP_AMPLITUDE;
- set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = config_.Z_SWAP_AMPLITUDE;
- set_walk_params_srv_.request.params.ARM_SWING_GAIN = config_.ARM_SWING_GAIN;
- set_walk_params_srv_.request.params.PELVIS_OFFSET = config_.PELVIS_OFFSET;
- set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = config_.HIP_PITCH_OFFSET;
- set_walk_params_srv_.request.params.X_OFFSET = config_.X_OFFSET;
- set_walk_params_srv_.request.params.Y_OFFSET = config_.Y_OFFSET;
- set_walk_params_srv_.request.params.Z_OFFSET = config_.Z_OFFSET;
- set_walk_params_srv_.request.params.A_OFFSET = config_.A_OFFSET;
- set_walk_params_srv_.request.params.P_OFFSET = config_.P_OFFSET;
- set_walk_params_srv_.request.params.R_OFFSET = config_.R_OFFSET;
- set_walk_params_srv_.request.params.PERIOD_TIME = config_.PERIOD_TIME;
- set_walk_params_srv_.request.params.DSP_RATIO = config_.DSP_RATIO;
- set_walk_params_srv_.request.params.STEP_FB_RATIO = config_.STEP_FB_RATIO;
- set_walk_params_srv_.request.params.FOOT_HEIGHT = config_.FOOT_HEIGHT;
- set_walk_params_srv_.request.params.MAX_VEL = config_.MAX_VEL;
- set_walk_params_srv_.request.params.MAX_ROT_VEL = config_.MAX_ROT_VEL;
- ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
- if(set_walk_params_client_.call(set_walk_params_srv_))
- {
- if(set_walk_params_srv_.response.ret)
- ROS_INFO("SmQrSearchAlgNode:: Parameters changed successfully");
- else
- ROS_INFO("SmQrSearchAlgNode:: Impossible to change parameters");
- }
- else
- ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
-
-}
-*/
- /*
-void CTrackingModule::send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude)
-{
- this->cmd_vel_Twist_msg_.linear.x=x_amplitude; //longitudinal. de -0.04 a 0.04 en metros
- this->cmd_vel_Twist_msg_.linear.y=y_amplitude; //Transversal
- this->cmd_vel_Twist_msg_.linear.z=0;
- this->cmd_vel_Twist_msg_.angular.x=0;
- this->cmd_vel_Twist_msg_.angular.y=0;
- this->cmd_vel_Twist_msg_.angular.z=a_amplitude; //de -0.5 a 0.5 en radianes
-
- this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
-}
-*/
-/*
-void CTrackingModule::stop_walking(void)
-{
- this->cmd_vel_Twist_msg_.linear.x=0;
- this->cmd_vel_Twist_msg_.linear.y=0;
- this->cmd_vel_Twist_msg_.linear.z=0;
- this->cmd_vel_Twist_msg_.angular.x=0;
- this->cmd_vel_Twist_msg_.angular.y=0;
- this->cmd_vel_Twist_msg_.angular.z=0;
-
- this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
-}
-*/
/*
void pid(TPID algo)
--
GitLab