Merge branch 'smart_charger' of...

Merge branch 'smart_charger' of https://gitlab.iri.upc.edu/humanoides/darwin_robot into smart_charger

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

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)

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;
+    
     
     
     

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"

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 -->

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