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

Minor changes in the tracking module.

parent d325541f
No related branches found
No related tags found
1 merge request!4Filtered localization
Hide whitespace changes
Inline Side-by-side
humanoid_modules/src/tracking_module.cpp
+ 52
− 253
View file @ 9046b47c
...@@ -43,7 +43,7 @@ CTrackingModule::CTrackingModule(const std::string &name) : CModule(name), ...@@ -43,7 +43,7 @@ CTrackingModule::CTrackingModule(const std::string &name) : CModule(name),
z_distance.distance_target=0.0; z_distance.distance_target=0.0;
z_distance.prev_error=0.0; z_distance.prev_error=0.0;
z_distance.tolerance=0.0; z_distance.tolerance=0.0;
x_distance.kp=0.1; x_distance.kp=0.2;
x_distance.ki=0.0; x_distance.ki=0.0;
x_distance.kd=0.0; x_distance.kd=0.0;
x_distance.qr_position=0.0; x_distance.qr_position=0.0;
...@@ -282,49 +282,46 @@ void CTrackingModule::state_machine(void) ...@@ -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); walk_module.set_steps_size(z_distance.pid_out,x_distance.pid_out,orientation.pid_out);
break; break;
*/ */
/*
case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK"); case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
// Update pid // Update pid
pid(z_distance, 0.01); pid(z_distance, 0.01);
pid(orientation,0.2); pid(orientation,0.2);
pid(x_distance, 0.005); pid(x_distance, 0.2);
// pid_yaw(orientation_x);
//si la distancia z (error) es menor a la minima distancia admitida (tolerancia) //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) 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"); ROS_INFO("CTrackingModule: Stop rotating");
orientation.pid_out=0.0; 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"); ROS_INFO("CTrackingModule: Stop walking Z");
z_distance.pid_out=0.0; 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"); ROS_INFO("CTrackingModule: Rotate for X");
pid_yaw(orientation); //mover yaw dependiendo de distancia x orientation.pid_out=-x_distance.pid_out;
// x_distance.pid_out=0.0; //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"); ROS_INFO("CTrackingModule: Stop aproaching");
walk_module.stop(); orientation.distance_target=this->yaw_target;
ROS_WARN("X: %f cm", (x_distance.qr_position)*100); //Distancia X QR this->state=ORIENTATE;
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;
} }
else else
this->state=TRACK_MODULE_TRACK; this->state=TRACK_MODULE_TRACK;
ROS_WARN("Error YAW: %f, PID: %f", orientation.error, orientation.pid_out); 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_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 // check if walk module failed
if(walk_module.is_finished()) if(walk_module.is_finished())
...@@ -343,20 +340,21 @@ void CTrackingModule::state_machine(void) ...@@ -343,20 +340,21 @@ void CTrackingModule::state_machine(void)
// check if qr was lost // check if qr was lost
if(this->watchdog_qr_lost.is_active()) if(this->watchdog_qr_lost.is_active())
{ {
ROS_INFO("CTrackingModule: QR lost!"); ROS_ERROR("CTrackingModule: QR lost!");
walk_module.stop(); //stop walking walk_module.stop(); //stop walking
this->state=TRACK_MODULE_END; this->state=TRACK_MODULE_END;
this->status=TRACK_MODULE_LOST; this->status=TRACK_MODULE_LOST;
} }
break; */ break;
case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK"); /*
case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
// Update pid // Update pid
pid(z_distance, 0.01); pid(z_distance, 0.01);
pid(orientation,0.2); pid(orientation,0.2);
pid(x_distance, 0.005); pid(x_distance, 0.01);
// pid_yaw(orientation_x); // pid_yaw(orientation_x);
//si la distancia z (error) es menor a la minima distancia admitida (tolerancia) //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) 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) ...@@ -364,18 +362,17 @@ void CTrackingModule::state_machine(void)
ROS_INFO("CTrackingModule: Stop rotating"); ROS_INFO("CTrackingModule: Stop rotating");
orientation.pid_out=0.0; 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"); ROS_INFO("CTrackingModule: Stop walking Z");
z_distance.pid_out=0.0; 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"); ROS_INFO("CTrackingModule: Stop walking X");
pid_yaw(orientation); //mover yaw dependiendo de distancia x x_distance.pid_out=0.0;
// 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"); ROS_INFO("CTrackingModule: Stop aproaching");
...@@ -393,7 +390,7 @@ void CTrackingModule::state_machine(void) ...@@ -393,7 +390,7 @@ void CTrackingModule::state_machine(void)
ROS_WARN("Error Z: %f, PID: %f", z_distance.error, z_distance.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, 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 // check if walk module failed
if(walk_module.is_finished()) if(walk_module.is_finished())
...@@ -419,7 +416,8 @@ void CTrackingModule::state_machine(void) ...@@ -419,7 +416,8 @@ void CTrackingModule::state_machine(void)
} }
break; break;
*/
case ORIENTATE: ROS_INFO("CTrackingModule: state ORIENTATE"); case ORIENTATE: ROS_INFO("CTrackingModule: state ORIENTATE");
pid(orientation,0.1); pid(orientation,0.1);
ROS_INFO("error: %f, qr_position: %f", orientation.error, orientation.qr_position); ROS_INFO("error: %f, qr_position: %f", orientation.error, orientation.qr_position);
...@@ -567,10 +565,6 @@ void CTrackingModule::update_pid(void) ...@@ -567,10 +565,6 @@ void CTrackingModule::update_pid(void)
/* callbacks */ /* callbacks */
void CTrackingModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg) 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(); this->joint_states_access.enter();
unsigned int i; unsigned int i;
...@@ -586,65 +580,17 @@ void CTrackingModule::joint_states_callback(const sensor_msgs::JointState::Const ...@@ -586,65 +580,17 @@ void CTrackingModule::joint_states_callback(const sensor_msgs::JointState::Const
this->current_tilt_angle=(msg->position[i]); this->current_tilt_angle=(msg->position[i]);
} }
} }
//unlock previously blocked shared variables
//this->alg_.unlock();
this->joint_states_access.exit(); 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) void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
{ {
this->qr_pose_access.enter(); this->qr_pose_access.enter();
if(msg->tags.size()>0) if(msg->tags.size()>0)
{ {
/* QR detected */
watchdog_qr_lost.reset(ros::Duration(1)); watchdog_qr_lost.reset(ros::Duration(1));
if(this->qr_id==msg->tags[0].tag_id) 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 ...@@ -654,19 +600,23 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
/* update pan and tilt*/ /* update pan and tilt*/
this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z); 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); 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); head_tracking_module.update_target(pan_angle, tilt_angle);
/* ROS_INFO("QR X: %f cm", (msg->tags[0].position.x)*100); /* ROS_WARN("QR X: %f", (msg->tags[0].position.x));
ROS_INFO("QR Y: %f cm", (msg->tags[0].position.y)*100); ROS_WARN("QR Y: %f", (msg->tags[0].position.y));
ROS_INFO("QR Z: %f cm", (msg->tags[0].position.z)*100); ROS_WARN("QR Z: %f", (msg->tags[0].position.z)); */
ROS_INFO("QR YAW: %f", tf::getYaw(msg->tags[0].orientation)); /* 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); // 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 target_frame="darwin/base_link";
std::string source_frame="darwin/head";
ros::Time target_time = ros::Time::now(); ros::Time target_time = ros::Time::now();
geometry_msgs::PoseStamped in; geometry_msgs::PoseStamped in;
...@@ -685,20 +635,23 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra ...@@ -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.position.z = msg->tags[0].position.z;
in.pose.orientation = msg->tags[0].orientation; in.pose.orientation = msg->tags[0].orientation;
tf_listener.transformPose(target_frame, in, out); tf_listener.transformPose(target_frame, in, out);
x_distance.qr_position=out.pose.position.x; 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; double z=out.pose.position.z;
tf::quaternionMsgToTF(out.pose.orientation,tf_quat); tf::quaternionMsgToTF(out.pose.orientation,tf_quat);
tf_rotation=tf::Matrix3x3(tf_quat); tf_rotation=tf::Matrix3x3(tf_quat);
tf_rotation.getRPY(roll,pitch,yaw); 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("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("Y: %f cm", (z_distance.qr_position)*100); //Distancia Z QR
ROS_INFO("YAW: %f cm", (orientation.qr_position)*100); ROS_INFO("YAW: %f cm", (orientation.qr_position)*100);
*/
} }
qr_detected=true; qr_detected=true;
} }
...@@ -713,78 +666,8 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra ...@@ -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) void CTrackingModule::node_config_update(Config &config, uint32_t level)
{ {
...@@ -850,7 +733,7 @@ void CTrackingModule::pid(TPID &axis, double max) //distancia z ...@@ -850,7 +733,7 @@ void CTrackingModule::pid(TPID &axis, double max) //distancia z
// ROS_INFO("time period: %f", time_period); // ROS_INFO("time period: %f", time_period);
//Calculate PID //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.derivative = (axis.error-axis.prev_error)/time_period;
//axis.integral = (axis.error*time_period); //+= //axis.integral = (axis.error*time_period); //+=
//res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative; //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 ...@@ -882,7 +765,7 @@ void CTrackingModule::pid_yaw(TPID &axis) //distancia z
time_period = (new_time-this->start_time).toSec(); time_period = (new_time-this->start_time).toSec();
//Calculate PID //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.derivative = (axis.error-axis.prev_error)/time_period;
//axis.integral = (axis.error*time_period); //+= //axis.integral = (axis.error*time_period); //+=
//res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative; //res += axis.kp*axis.error + axis.ki*axis.integral + axis.kd*axis.derivative;
...@@ -1014,90 +897,6 @@ void CTrackingModule::pid2(TPID , double &) //distancia z ...@@ -1014,90 +897,6 @@ void CTrackingModule::pid2(TPID , double &) //distancia z
this->start_time=ros::Time::now(); 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) void pid(TPID algo)
......
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