diff --git a/track_charge_station/CMakeLists.txt b/track_charge_station/CMakeLists.txt index 0fe1840be27da541d95ad847b393af3c60069a73..00b6ffe839aae4e108a6d4fa9d35fe1da8a4ba49 100644 --- a/track_charge_station/CMakeLists.txt +++ b/track_charge_station/CMakeLists.txt @@ -6,7 +6,7 @@ find_package(catkin REQUIRED) # ******************************************************************** # Add catkin additional components here # ******************************************************************** -find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs) +find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -60,7 +60,7 @@ catkin_package( # ******************************************************************** # Add ROS and IRI ROS run time dependencies # ******************************************************************** - CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs + CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools # ******************************************************************** # Add system and labrobotica run time dependencies here # ******************************************************************** @@ -76,6 +76,7 @@ catkin_package( # ******************************************************************** include_directories(include) include_directories(${catkin_INCLUDE_DIRS}) +include_directories(${iriutils_INCLUDE_DIRS}) # include_directories(${<dependency>_INCLUDE_DIR}) ## Declare a cpp library diff --git a/track_charge_station/cfg/TrackChargeStation.cfg b/track_charge_station/cfg/TrackChargeStation.cfg index 578970da730022f2c611d4d82997586f9e4719eb..3f7f1e0a84c321f2bd2b20d03687512c66c737b2 100755 --- a/track_charge_station/cfg/TrackChargeStation.cfg +++ b/track_charge_station/cfg/TrackChargeStation.cfg @@ -49,7 +49,7 @@ gen.add("z_kp", double_t, 0, " gen.add("z_ki", double_t, 0, "Desired ki", 0.0, 0.0, 0.28) gen.add("z_kd", double_t, 0, "Desired kd", 0.0, 0.0, 0.28) gen.add("period", double_t, 0, "Period PID", 1.0, 0.0, 100) -gen.add("z_error", double_t, 0, "Minimun error in distance z", 1.0, 0.0, 10) +gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 1.0, 0.0, 10) #Walk gen.add("Y_SWAP_AMPLITUDE", double_t, 0, "Y swap amplitude (m)", 0.025, 0, 0.050) gen.add("Z_SWAP_AMPLITUDE", double_t, 0, "X swap amplitude (m)", 0.005, 0, 0.050) diff --git a/track_charge_station/include/track_charge_station_alg_node.h b/track_charge_station/include/track_charge_station_alg_node.h index 710b1a296bb8aa29b6d4158574e5f6e0a0c8f911..7db52a1109280b3f74212c50b74f0aee9043cb25 100644 --- a/track_charge_station/include/track_charge_station_alg_node.h +++ b/track_charge_station/include/track_charge_station_alg_node.h @@ -25,6 +25,7 @@ #ifndef _track_charge_station_alg_node_h_ #define _track_charge_station_alg_node_h_ +#include <iri_ros_tools/watchdog.h> #include <iri_base_algorithm/iri_base_algorithm.h> #include "track_charge_station_alg.h" @@ -134,6 +135,7 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC double qr_position_z; double qr_position_x; double qr_position_y; + CROSWatchdog watchdog_qr_lost; //PID TPID z_distance; TPID x_distance; @@ -141,6 +143,10 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC double pid; double pid_out; double z_distance_target; + ros::Time start_time; + ros::Time new_time; + //ros::Duration time_period; + double time_period; //walk //añadir a la estructura TPID? double cmd_vel_x; @@ -149,7 +155,8 @@ class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackC double z_error; double x_error; double rot_error; - double z_min_error; + double z_min_tolerance; + diff --git a/track_charge_station/launch/sim_track_charge_station.launch b/track_charge_station/launch/sim_track_charge_station.launch index c0f7cd392de8939bbc62bd08a1fc2a5c9af36abb..34c2c0c6ce5123f801976cfb666e6e6cc80c1bbc 100644 --- a/track_charge_station/launch/sim_track_charge_station.launch +++ b/track_charge_station/launch/sim_track_charge_station.launch @@ -19,8 +19,6 @@ type="track_charge_station" output="screen" ns="/darwin"> - - <remap from="/darwin/track_charge_station/cmd_vel" to="/darwin/robot/cmd_vel"/> <remap from="/darwin/track_charge_station/set_pan_pid" diff --git a/track_charge_station/package.xml b/track_charge_station/package.xml index 7468aa8e5aba276d4dcb0f15b340873ce4dfc329..170d809c8e74bad4b03e1dcfa260b3ca4dfa20ee 100644 --- a/track_charge_station/package.xml +++ b/track_charge_station/package.xml @@ -46,12 +46,14 @@ <build_depend>actionlib</build_depend> <build_depend>geometry_msgs</build_depend> <build_depend>humanoid_common_msgs</build_depend> + <build_depend>iri_ros_tools</build_depend> <run_depend>iri_base_algorithm</run_depend> <run_depend>sensor_msgs</run_depend> <run_depend>trajectory_msgs</run_depend> <run_depend>actionlib</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>humanoid_common_msgs</run_depend> + <run_depend>iri_ros_tools</run_depend> <!-- The export tag contains other, unspecified, tags --> diff --git a/track_charge_station/src/track_charge_station_alg_node.cpp b/track_charge_station/src/track_charge_station_alg_node.cpp index a02709d412c241a82fa51987dce11fb8cec8dd6d..ac68f8ee3ad8fd101ff1c6361090f2b924e50cc2 100644 --- a/track_charge_station/src/track_charge_station_alg_node.cpp +++ b/track_charge_station/src/track_charge_station_alg_node.cpp @@ -195,7 +195,6 @@ void TrackChargeStationAlgNode::mainNodeThread(void) ROS_INFO("IDLE"); if(new_goal) { - //Set servo modules? //Start tracking QR this->tracking=true; set_servo_module("walk"); //Servos to walk module @@ -222,30 +221,30 @@ void TrackChargeStationAlgNode::mainNodeThread(void) //Comprobar si se ha perdido QR y actualizar pid (con señal bool) case QR: - //Volver a la maquina de estados SmQrSearchAlgNode - /* if(counter>50) - { - ROS_INFO("QR lost"); - this->tracking=false; - this->head_follow_target_client_.cancelGoal(); + // if(counter>50) + if(this->watchdog_qr_lost.is_active()) + { + ROS_INFO("QR lost"); + this->tracking=false; + this->head_follow_target_client_.cancelGoal(); //Volver a la maquina de estados SmQrSearchAlgNode - state=IDLE; - } - else */if(end){ - ROS_INFO("Stop State machine"); - this->tracking=false; - this->head_follow_target_client_.cancelGoal(); - end=false; - state=IDLE; - } - else - { - ROS_INFO("TRACKING"); + state=IDLE; + } + /* else if(end){ + ROS_INFO("Stop State machine"); + this->tracking=false; + this->head_follow_target_client_.cancelGoal(); + end=false; + state=IDLE; + } */ + else + { + ROS_INFO("TRACKING"); //counter=0; //pid(); //send_cmd_vel(pid_out); - state=WALK; - } + state=WALK; + } break; case WALK: @@ -253,7 +252,7 @@ void TrackChargeStationAlgNode::mainNodeThread(void) //Distancia z estacion carga // pid(z_distance, &z_error, &z_cmd_vel); //Actualizar PID // pid(x_distance, &x_error, &x_cmd_vel); -// if(z_error<z_min_error) //Si el error de la distancia z es menor para ese cmd_vel (y) +// if(z_error<z_min_tolerance) //Si el error de la distancia z es menor para ese cmd_vel (y) // z_cmd_vel=0.0; //mantener los otros cmd_vel pero parar el de la direccion y // if(x_error<x_desired) // x_cmd_vel=0.0; @@ -269,9 +268,9 @@ void TrackChargeStationAlgNode::mainNodeThread(void) pid2(z_distance, cmd_vel_y); - if(z_distance.error<z_min_error) + if(z_distance.error<z_min_tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia) { - cmd_vel_y=0.0; + cmd_vel_y=0.0; //No mover eje y state=SIT; } else @@ -354,11 +353,11 @@ void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag { send_head_target(pan_angle, tilt_angle); } - //watchdog_qr_lost.reset(ros::Duration(10)); - counter=0; + watchdog_qr_lost.reset(ros::Duration(5)); + //counter=0; }else{ qr_detected=false; - counter++; + //counter++; } @@ -458,7 +457,7 @@ void TrackChargeStationAlgNode::node_config_update(Config &config, uint32_t leve z_distance_target=config.z_target; // x_distance_target=config.x_target; //orientation_target=config.yaw_target; - z_min_error=config.z_error; + z_min_tolerance=config.z_tolerance; //x_min_error=config.x_error; //rot_min_error=config.yaw_error; new_goal=true; @@ -566,6 +565,10 @@ void TrackChargeStationAlgNode::pid(void) //distancia z void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z { + this->new_time=ros::Time::now(); + //this->time_period = (new_time-this->start_time); + time_period = (new_time-this->start_time).toSec(); + z_distance.error = qr_position_z-z_distance_target; // if(z_distance.error<1) // { @@ -573,8 +576,8 @@ void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z // } // else // { - z_distance.derivative = (z_distance.error-z_distance.prev_error)/z_distance.period; - z_distance.integral = (z_distance.error*z_distance.period); //+= + z_distance.derivative = (z_distance.error-z_distance.prev_error)/this->time_period; + z_distance.integral = (z_distance.error*this->time_period); //+= //pid += z_distance.kp*z_distance.error + z_distance.ki*z_distance.integral + z_distance.kd*z_distance.derivative; pid = z_distance.kp*z_distance.error; z_distance.prev_error=z_distance.error; @@ -589,6 +592,7 @@ void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z //return false; //} + this->start_time=ros::Time::now(); } //Head tracking