diff --git a/automatic_charge/CMakeLists.txt b/automatic_charge/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9819225af70ba4216d2c802a2f733bf6f37d7537
--- /dev/null
+++ b/automatic_charge/CMakeLists.txt
@@ -0,0 +1,103 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(automatic_charge)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm humanoid_common_msgs iri_ros_tools humanoid_modules)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/AutomaticCharge.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_algorithm humanoid_common_msgs iri_ros_tools humanoid_modules
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/automatic_charge_alg.cpp src/automatic_charge_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/automatic_charge/caca b/automatic_charge/caca
new file mode 100644
index 0000000000000000000000000000000000000000..758f42589c7b4d9fa31d658a7fb64d442a3987e7
--- /dev/null
+++ b/automatic_charge/caca
@@ -0,0 +1,55 @@
+ case ht_tracking:
+ else
+ {
+ nav_module->nav_access.exit();
+ move_base_state=nav_module->move_base_client->getState();
+ nav_module->nav_access.enter();
+ if(move_base_state!=actionlib::SimpleClientGoalState::ACTIVE)
+ {
+ if(move_base_state==actionlib::SimpleClientGoalState::ABORTED)
+ {
+ ROS_ERROR("NavModule: goal aborted");
+ nav_module->status=NAV_MODULE_ABORTED;
+ }
+ else if(move_base_state==actionlib::SimpleClientGoalState::PREEMPTED)
+ {
+ ROS_WARN("NavModule: goal preempted");
+ nav_module->status=NAV_MODULE_PREEMPTED;
+ }
+ else if(move_base_state==actionlib::SimpleClientGoalState::SUCCEEDED)
+ {
+ ROS_DEBUG("NavModule: goal successfull");
+ nav_module->status=NAV_MODULE_SUCCESS;
+ }
+/* if(nav_module->new_goal)
+ {
+ if(nav_module->goal_name=="")
+ nav_module->state=nav_set_parameters;
+ else
+ nav_module->state=nav_get_goal_db;
+ nav_module->new_goal=false;
+ }
+ else*/
+ nav_module->running=false;
+ nav_module->state=nav_idle;
+
+ tracking=false;
+ walk=false;
+ current_state=walking;
+
+ }
+ else
+ {
+ ROS_DEBUG("NavModule: goal active");
+ nav_module->state=nav_action_wait;
+ }
+ }
+ if(nav_module->cancel_pending)
+ {
+ nav_module->cancel_pending=false;
+ nav_module->nav_access.exit();
+ nav_module->move_base_client->cancelGoal();
+ nav_module->nav_access.enter();
+ }
+
+ break;
\ No newline at end of file
diff --git a/automatic_charge/cfg/AutomaticCharge.cfg b/automatic_charge/cfg/AutomaticCharge.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..85774a8e638d02aafb9345dad1b9342266df411b
--- /dev/null
+++ b/automatic_charge/cfg/AutomaticCharge.cfg
@@ -0,0 +1,61 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='automatic_charge'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+gen.add("start", bool_t, 0, "Start automatic charge", False)
+gen.add("stop", bool_t, 0, "Stop automatic charge", False)
+
+gen.add("get_config", bool_t, 0, "Get smart charger configuration", False)
+gen.add("set_config", bool_t, 0, "Set smart charger configuration", False)
+gen.add("enable", bool_t, 0, "Enable/Disable smart charger module", False)
+gen.add("disable", bool_t, 0, "Disable smart charger module", False)
+gen.add("period", double_t, 0, "Period value", 1.5, 1.0, 5.0)
+gen.add("limit_current", double_t, 0, "limit current value", 0.512, 0.254, 1.024 )
+
+#search module
+gen.add("tilt_angle_search", double_t, 0, "Search tilt angle", 0.8, 0.0, 1.0)
+gen.add("head_speed", double_t, 0, "Servo speed (rad/s)", 1, 0, 6.2832)
+gen.add("head_acceleration", double_t, 0, "Servo acceleration (rad/s/s)", 0.3, 0, 6.2832)
+gen.add("yaw_step", double_t, 0, "Amplitud of yaw rotation (rad)", 0.1, 0, 0.2)
+gen.add("angle_rotation", double_t, 0, "Angle of rotation", 3.1415, 0.78, 3.1415)
+gen.add("error", double_t, 0, "Min error in rotation", 0.01, 0.01, 0.1)
+
+
+
+exit(gen.generate(PACKAGE, "AutomaticChargeAlgorithm", "AutomaticCharge"))
diff --git a/automatic_charge/cfg/AutomaticCharge2.cfg b/automatic_charge/cfg/AutomaticCharge2.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..94c9617648e7d49404963e87c1ea6c64d0cf3fff
--- /dev/null
+++ b/automatic_charge/cfg/AutomaticCharge2.cfg
@@ -0,0 +1,46 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='smart_charger'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("start", bool_t, 0, "enable guiding", False)
+gen.add("stop", bool_t, 0, "disable guiding", False)
+gen.add("location_id", str_t, 0, "goal name", "")
+
+exit(gen.generate(PACKAGE, "SmartChargerAlgorithm", "SmartCharger"))
diff --git a/automatic_charge/funciones b/automatic_charge/funciones
new file mode 100644
index 0000000000000000000000000000000000000000..e13f7d522b1a96a84a62d492049320e0bf1127aa
--- /dev/null
+++ b/automatic_charge/funciones
@@ -0,0 +1,39 @@
+------------------------------WALK_MODULE--------------------------------
+
+INCLUDE/
+
+
+#include <tibi_dabo_guide_nav/guide_module.h>
+
+CDarwinModule
+
+CPP/
+
+bool set_walk_params(void){
+
+ set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = 0.02;
+ set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = 0.005;
+ set_walk_params_srv_.request.params.ARM_SWING_GAIN = 1.5;
+ set_walk_params_srv_.request.params.PELVIS_OFFSET = 0.05;
+ set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = 0.23;
+ set_walk_params_srv_.request.params.X_OFFSET = -0.01;
+ set_walk_params_srv_.request.params.Y_OFFSET = 0.005;
+ set_walk_params_srv_.request.params.Z_OFFSET = 0.02;
+ set_walk_params_srv_.request.params.A_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.P_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.R_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.PERIOD_TIME = 0.6;
+ set_walk_params_srv_.request.params.DSP_RATIO = 0.1;
+ set_walk_params_srv_.request.params.STEP_FB_RATIO = 0.28;
+ set_walk_params_srv_.request.params.FOOT_HEIGHT = 0.04;
+ set_walk_params_srv_.request.params.MAX_VEL = 0.01;
+ set_walk_params_srv_.request.params.MAX_ROT_VEL = 0.5; //acceleració de rotació
+ ROS_INFO("DarwinVisionProve:: Sending New Request!");
+ if(set_walk_params_client_.call(set_walk_params_srv_))
+ {
+ if(set_walk_params_srv_.response.ret){
+ ROS_INFO("DarwinVisionProve:: Parameters WK changed successfully");
+
+
+
+}
\ No newline at end of file
diff --git a/automatic_charge/funciones.cpp b/automatic_charge/funciones.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4370f136a52e7e244bfe9b029bff0f23d656fceb
--- /dev/null
+++ b/automatic_charge/funciones.cpp
@@ -0,0 +1,159 @@
+//-----------------------------DARWIN MODULE-------------------------------
+
+#define NUM_SERVOS 32
+// joint names
+const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_pan"),
+ std::string("j_tilt"),
+ std::string("Servo21"),
+ std::string("Servo22"),
+ std::string("Servo23"),
+ std::string("Servo24"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30"),
+ std::string("Servo31")};
+
+bool set_servo_module(int servo_init, int servo_fin, string module)
+{
+ int i;
+
+ int num_servos=(servo_fin-servo_init)+1;
+ this->set_servo_modules_srv_.request.names.resize(num_servos);
+ this->set_servo_modules_srv_.request.modules.resize(num_servos);
+
+ for(i=servo_init;i<(servo_fin+1);i++)
+ {
+ this->set_servo_modules_srv_.request.names[i]=servo_names[i];
+ this->set_servo_modules_srv_.request.modules[i]=module;
+ }
+}
+
+
+//-----------------------------HEAD_TRACKING-------------------------------
+
+void head_tracking_start_action(pan_target, tilt_target)
+{
+ if(config.start_tracking)
+ {
+ if(this->tracking)
+ ROS_WARN("The tracking is already active");
+ else
+ {
+ // send the first target angles
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=pan_target;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt_target;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+ // start the tracking
+ this->head_follow_targetMakeActionRequest();
+ }
+ config.start_tracking=false;
+ }
+}
+
+
+//------------------------------WALK_MODULE--------------------------------
+
+//INCLUDE/
+
+
+#include <tibi_dabo_guide_nav/guide_module.h>
+
+CWalkModule walk_module;
+
+//CPP/
+
+
+bool set_walk_params(void){
+
+ set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = 0.02;
+ set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = 0.005;
+ set_walk_params_srv_.request.params.ARM_SWING_GAIN = 1.5;
+ set_walk_params_srv_.request.params.PELVIS_OFFSET = 0.05;
+ set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = 0.23;
+ set_walk_params_srv_.request.params.X_OFFSET = -0.01;
+ set_walk_params_srv_.request.params.Y_OFFSET = 0.005;
+ set_walk_params_srv_.request.params.Z_OFFSET = 0.02;
+ set_walk_params_srv_.request.params.A_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.P_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.R_OFFSET = 0.0;
+ set_walk_params_srv_.request.params.PERIOD_TIME = 0.6;
+ set_walk_params_srv_.request.params.DSP_RATIO = 0.1;
+ set_walk_params_srv_.request.params.STEP_FB_RATIO = 0.28;
+ set_walk_params_srv_.request.params.FOOT_HEIGHT = 0.04;
+ set_walk_params_srv_.request.params.MAX_VEL = 0.01;
+ set_walk_params_srv_.request.params.MAX_ROT_VEL = 0.5; //acceleració de rotació
+ ROS_INFO("DarwinVisionProve:: Sending New Request!");
+ if(set_walk_params_client_.call(set_walk_params_srv_))
+ {
+ if(set_walk_params_srv_.response.ret){
+ ROS_INFO("DarwinVisionProve:: Walk parameters changed successfully");
+ return true;
+ }else{
+ ROS_INFO("DarwinVisionProve::Error changing parameters ");
+ return false;
+ }
+ }else{
+ ROS_INFO("DarwinVisionProve:: Error calling server");
+ return false;
+ }
+
+}
+
+
+void start_walking(z_amplitud, y_amplitud, a_amplitud)
+{
+ this->cmd_vel_Twist_msg_.linear.x=config.X_AMPLITUDE/config.PERIOD_TIME;
+ this->cmd_vel_Twist_msg_.linear.y=config.Y_AMPLITUDE/config.PERIOD_TIME;
+ 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=config.A_AMPLITUDE/config.PERIOD_TIME;
+ this->walking=true;
+}
+
+//Walk module thread
+void WalkClientAlgNode::mainNodeThread(void)
+{
+ // [publish messages]
+ if(this->walk)
+ {
+ if(
+
+
+ this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+
+}
+
+
+
+//---------------------------MOVE JOINTS---------------------------
+
+//Move joints to a position
+void joint_trajectory()
+{
+}
+
+
+
+
diff --git a/automatic_charge/head_tracking.cpp b/automatic_charge/head_tracking.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ea3838feb99e2dc5de88b1299136704853d4675
--- /dev/null
+++ b/automatic_charge/head_tracking.cpp
@@ -0,0 +1,565 @@
+
+//-----------------------------HEAD_TRACKING-------------------------------
+
+#include darwin/head_tracking_module.h
+
+CHeadTrackingModule::CHeadTrackingModule(ros::NodeHandle &nh, const std::string &prefix) :
+/* guide_nh(nh,prefix),
+ hri_module(guide_nh, "get_hri","hri_client",prefix+"_hri"),
+ nav_module(guide_nh,"get_goal","move_base","set_params",prefix+"_nav"),
+ dyn_reconf(guide_nh),
+ tf_listener(ros::Duration(10.f)),
+ rate(DEFAULT_RATE) */
+{
+ state=ht_idle;
+ tracking=false;
+ new_goal=false;
+}
+
+void start_nav(void)
+{
+ //start_qr_pose=true; //Activar deteccion de qr (actualizacion de pan y tilt por qr_pose)
+ posicion_final_z=posicion_z;
+ orientacion=orientacion;
+ new_goal=true; //Activar maquina de estados
+}
+
+/*
+void head_tracking_start_action(pan_target, tilt_target)
+{
+ if(config.start_tracking)
+ {
+ if(this->tracking)
+ ROS_WARN("The tracking is already active");
+ else
+ {
+ // send the first target angles
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=pan_target;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt_target;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+ // start the tracking
+ this->head_follow_targetMakeActionRequest();
+ }
+ config.start_tracking=false;
+ }
+}
+*/
+
+bool HeadTrackingClientAlgNode::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(this->head_follow_target_client_.waitForServer()) //isServerConnected()?
+ {
+ ROS_DEBUG("NavModule::move_baseMakeActionRequest: Server is Available!");
+
+ this->head_follow_target_client_.sendGoal(this->head_follow_target_goal_,
+ boost::bind(&HeadTrackingClientAlgNode::head_follow_targetDone, this, _1, _2),
+ boost::bind(&HeadTrackingClientAlgNode::head_follow_targetActive, this),
+ boost::bind(&HeadTrackingClientAlgNode::head_follow_targetFeedback, this, _1));
+ this->tracking=true;
+ // this->alg_.lock();
+ // ROS_DEBUG("HeadTrackingClientAlgNode::MakeActionRequest: Goal Sent.");
+ return true;
+ }
+ else
+ {
+ // this->alg_.lock();
+ /ROS_DEBUG("HeadTrackingClientAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
+ return false;
+ }
+}
+
+void get_head_tracking_goal(pan_target, tilt_target)
+{
+ this->head_follow_target_goal_.target_pan=this->pan_target;
+ this->head_follow_target_goal_.target_tilt=this->tilt_target;
+ this->head_follow_target_goal_.pan_range.resize(2);
+ this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
+ this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
+ this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
+ this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
+
+}
+
+void get_pan_tilt_pid(void)
+{
+ if(first)
+ {
+ ROS_INFO("QrHeadTrackingAlgNode:: Sending New Request!");
+ if (get_tilt_pid_client_.call(get_tilt_pid_srv_))
+ {
+ this->config_.tilt_p=this->get_tilt_pid_srv_.response.p;
+ this->config_.tilt_i=this->get_tilt_pid_srv_.response.i;
+ this->config_.tilt_d=this->get_tilt_pid_srv_.response.d;
+ this->config_.tilt_i_clamp = this->get_tilt_pid_srv_.response.i_clamp;
+ }
+ else
+ {
+ ROS_INFO("QrHeadTrackingAlgNode:: Failed to Call Server on topic get_tilt_pid ");
+ }
+
+ ROS_INFO("QrHeadTrackingAlgNode:: Sending New Request!");
+ if (get_pan_pid_client_.call(get_pan_pid_srv_))
+ {
+ this->config_.pan_p=this->get_pan_pid_srv_.response.p;
+ this->config_.pan_i=this->get_pan_pid_srv_.response.i;
+ this->config_.pan_d=this->get_pan_pid_srv_.response.d;
+ this->config_.pan_i_clamp = this->get_pan_pid_srv_.response.i_clamp;
+ }
+ else
+ {
+ ROS_INFO("QrHeadTrackingAlgNode:: Failed to Call Server on topic get_pan_pid ");
+ }
+ first=false;
+ }
+}
+
+void send_head_target(pan_angle, tilt_angle) //con joints?
+{
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=this->pan_angle;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=this->tilt_angle;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+}
+
+
+//----------------------------------------------------------------------------------------------------------
+
+#define NUM_SERVOS 32
+// joint names
+const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
+ std::string("j_pan"),
+ std::string("j_tilt"),
+ std::string("Servo21"),
+ std::string("Servo22"),
+ std::string("Servo23"),
+ std::string("Servo24"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30"),
+ std::string("Servo31")};
+
+
+
+#include "qr_head_tracking_alg_node.h"
+
+
+QrHeadTrackingAlgNode::QrHeadTrackingAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<QrHeadTrackingAlgorithm>(),
+ head_follow_target_client_("head_follow_target", true)
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+ this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
+
+ // [init subscribers]
+ this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &QrHeadTrackingAlgNode::joint_states_callback, this);
+ pthread_mutex_init(&this->joint_states_mutex_,NULL);
+
+ this->qr_pose_subscriber_ = this->public_node_handle_.subscribe("qr_pose", 1, &QrHeadTrackingAlgNode::qr_pose_callback, this);
+ pthread_mutex_init(&this->qr_pose_mutex_,NULL);
+
+ // [init services]
+
+ // [init clients]
+ set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
+
+ get_tilt_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_pid>("get_tilt_pid");
+
+ get_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_pid>("get_pan_pid");
+
+ set_tilt_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_tilt_pid");
+
+ set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
+
+ // [init action servers]
+
+ // [init action clients]
+
+ this->tracking=false;
+ this->pan_angle=0.0;
+ this->current_pan_angle=0.0;
+ this->tilt_angle=0.0;
+ this->current_tilt_angle=0.0;
+ this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
+ this->set_servo_modules_srv_.request.names.resize(2);
+ this->set_servo_modules_srv_.request.modules.resize(2);
+ this->set_servo_modules_srv_.request.names[0]="j_pan";
+ this->set_servo_modules_srv_.request.modules[0]="head";
+ this->set_servo_modules_srv_.request.names[1]="j_tilt";
+ this->set_servo_modules_srv_.request.modules[1]="head";
+}
+
+QrHeadTrackingAlgNode::~QrHeadTrackingAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->joint_states_mutex_);
+ pthread_mutex_destroy(&this->qr_pose_mutex_);
+}
+
+void QrHeadTrackingAlgNode::mainNodeThread(void)
+{
+ static bool first=true;
+
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+ if(first){
+ get_pan_tilt_pid();
+
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+
+//meterlo en qr_pose_callback
+ this->alg_.lock();
+
+ this->alg_.unlock();
+
+
+ //STATE MACHINE
+ switch(this->current_state)
+ {
+ case NAV_IDLE:
+ if(new_goal)
+ {
+ ROS_INFO("Head tracking: New goal");
+ get_head_tracking_goal(); //angulos pan y tilt iniciales + angulos maximos y minimos
+ if(head_follow_targetMakeActionRequest())
+ {
+ current_state=NAV_SEARCH_QR;
+ new_goal=false;
+ }
+ }
+ break;
+
+ case NAV_SEARCH_QR:
+ //set_params(); //Set distancia y orientacion respecto qr
+ //get_head_tracking_goal();
+ //if(head_follow_targetMakeActionRequest()){
+ //walk=true;
+ // current_state=ht_tracking;
+ // tracking=true;
+
+ qr_enable=true; //Hablitar qr_pose_callback
+ if(qr_detected>0) //QR detectado
+ {
+ qr_id
+ current_state=NAV_FOLLOW_QR;
+ }
+ else
+ {
+ for(i=0; i<1.5; i=i+0.1)
+ {
+ send_head_target(pan_inicial+i, tilt_inicial+i);
+ }
+
+ }
+ break;
+
+/* case nav_head_tracking:
+
+ tracking=false;
+ walk=false;
+ current_state=walking;
+
+ break;
+
+ case walking:
+ if(lejosposicion_final)
+ {
+ if(angle_pan>1)
+ {
+ start_walking(girar);
+ }
+ else if(angle_pan<1)
+ {
+ start_walking(girar);
+ }
+ else
+ {
+ start_walking(delante);
+ }
+ }
+ else
+ {
+ stop_walking();
+ current_state=end;
+ }
+ break;
+
+ */
+
+ case NAV_FOLLOW_QR:
+ if(qr_detected>0)
+ {
+ if(posicion_z<posicion_final_z) //comprobar si esta a la distancia correcta de la estacion de carga -- añadir orientacion
+ current_state=END;
+ else
+ {
+
+ }
+ }
+ else
+ {
+ current_state=NAV_SEARCH_QR;
+ break;
+
+
+}
+
+/* [subscriber callbacks] */
+void QrHeadTrackingAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ unsigned int i;
+
+ ROS_INFO("QrHeadTrackingAlgNode::joint_states_callback: New Message Received");
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->joint_states_mutex_enter();
+
+ for (i = 0; i < msg->name.size() ; ++i){
+ if (msg->name[i]=="j_pan"){
+ this->current_pan_angle=msg->position[i];
+ }
+ else if (msg->name[i]=="j_tilt"){
+ this->current_tilt_angle=(msg->position[i]);
+ }
+ }
+ ROS_DEBUG("angle_pan: %f angle_tilt: %f",this->current_pan_angle,this->current_tilt_angle);
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->joint_states_mutex_exit();
+}
+
+void QrHeadTrackingAlgNode::joint_states_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->joint_states_mutex_);
+}
+
+void QrHeadTrackingAlgNode::joint_states_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->joint_states_mutex_);
+}
+
+void QrHeadTrackingAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
+{
+ ROS_INFO("QrHeadTrackingAlgNode::qr_pose_callback: New Message Received");
+ ROS_INFO("Detected %d QR tags",(int)msg->tags.size());
+ this->qr_pose_mutex_enter();
+ if(msg->tags.size()>0)
+ {
+ qr_detected=msg->tags.size();
+ position_z=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);
+ 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)
+ {
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=this->pan_angle;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=this->tilt_angle;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+ }
+
+ }
+ this->qr_pose_mutex_exit();
+}
+
+void QrHeadTrackingAlgNode::qr_pose_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->qr_pose_mutex_);
+}
+
+void QrHeadTrackingAlgNode::qr_pose_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->qr_pose_mutex_);
+}
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+void QrHeadTrackingAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
+{
+ alg_.lock();
+ if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
+ ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetDone: Goal Achieved!");
+ else
+ ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetDone: %s", state.toString().c_str());
+ this->tracking=false;
+
+ //copy & work with requested result
+ alg_.unlock();
+}
+
+void QrHeadTrackingAlgNode::head_follow_targetActive()
+{
+ alg_.lock();
+ //ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetActive: Goal just went active!");
+ alg_.unlock();
+}
+
+void QrHeadTrackingAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
+{
+ alg_.lock();
+ //ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetFeedback: Got Feedback!");
+
+ bool feedback_is_ok = true;
+ ROS_INFO("Current angles: pan %f of %f, tilt %f of %f",feedback->current_pan,this->pan_angle,feedback->current_tilt,this->tilt_angle);
+
+ //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("QrHeadTrackingAlgNode::head_follow_targetFeedback: Cancelling Action!");
+ }
+ alg_.unlock();
+}
+
+/* [action requests] */
+bool QrHeadTrackingAlgNode::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(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
+ {
+ this->head_follow_target_client_.waitForServer();
+ //ROS_DEBUG("QrHeadTrackingAlgNode::head_follow_targetMakeActionRequest: Server is Available!");
+ //send a goal to the action server
+ //head_follow_target_goal_.data = my_desired_goal;
+ this->head_follow_target_client_.waitForServer();
+ this->head_follow_target_goal_.target_pan=this->pan_angle;
+ this->head_follow_target_goal_.target_tilt=this->tilt_angle;
+ this->head_follow_target_goal_.pan_range.resize(2);
+ this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
+ this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
+ this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
+ this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
+ this->head_follow_target_client_.sendGoal(head_follow_target_goal_,
+ boost::bind(&QrHeadTrackingAlgNode::head_follow_targetDone, this, _1, _2),
+ boost::bind(&QrHeadTrackingAlgNode::head_follow_targetActive, this),
+ boost::bind(&QrHeadTrackingAlgNode::head_follow_targetFeedback, this, _1));
+ this->tracking=true;
+ // this->alg_.lock();
+ // ROS_DEBUG("QrHeadTrackingAlgNode::MakeActionRequest: Goal Sent.");
+ return true;
+ }
+ else
+ {
+ // this->alg_.lock();
+ // ROS_DEBUG("QrHeadTrackingAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
+ return false;
+ }
+}
+
+void QrHeadTrackingAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+ if(config.start_tracking)
+ {
+ if(this->tracking)
+ ROS_WARN("The tracking is already active");
+ else
+ {
+ // send the first target angles
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=this->pan_angle;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=this->tilt_angle;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+ // start the tracking
+ this->head_follow_targetMakeActionRequest();
+ }
+ config.start_tracking=false;
+ }
+ else if(config.stop_tracking)
+ {
+ if(this->tracking)
+ this->head_follow_target_client_.cancelGoal();
+ config.stop_tracking=false;
+ }
+ else if(config.update_pan_pid)
+ {
+ set_pan_pid_srv_.request.p = config.pan_p;
+ set_pan_pid_srv_.request.i = config.pan_i;
+ set_pan_pid_srv_.request.d = config.pan_d;
+ set_pan_pid_srv_.request.i_clamp = config.pan_i_clamp;
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Sending New Request!");
+ if(this->set_pan_pid_client_.call(set_pan_pid_srv_))
+ {
+ if(this->set_pan_pid_srv_.response.ret)
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Pan pid changed successfully");
+ else
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Impossible to change pan pid");
+ }
+ else
+ {
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Failed to Call Server on topic set_pan_pid ");
+ }
+ config.update_pan_pid=false;
+ }
+ else if(config.update_tilt_pid)
+ {
+ set_tilt_pid_srv_.request.p = config.tilt_p;
+ set_tilt_pid_srv_.request.i = config.tilt_i;
+ set_tilt_pid_srv_.request.d = config.tilt_d;
+ set_tilt_pid_srv_.request.i_clamp = config.tilt_i_clamp;
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Sending New Request!");
+ if (this->set_tilt_pid_client_.call(set_tilt_pid_srv_))
+ {
+ if(this->set_tilt_pid_srv_.response.ret)
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Tilt pid changed successfully");
+ else
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Impossible to change tilt pid");
+ }
+ else
+ {
+ ROS_INFO("DarwinHeadTrackingClientAlgNode:: Failed to Call Server on topic set_tilt_pid ");
+ }
+ config.update_tilt_pid=false;
+ }
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void QrHeadTrackingAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<QrHeadTrackingAlgNode>(argc, argv, "qr_head_tracking_alg_node");
+}
diff --git a/automatic_charge/include/automatic_charge_alg.h b/automatic_charge/include/automatic_charge_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..47c8b4b627239c0a2ecc21563c9193ce4da3b4f3
--- /dev/null
+++ b/automatic_charge/include/automatic_charge_alg.h
@@ -0,0 +1,131 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _automatic_charge_alg_h_
+#define _automatic_charge_alg_h_
+
+#include <automatic_charge/AutomaticChargeConfig.h>
+
+//include automatic_charge_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class AutomaticChargeAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the AutomaticChargeConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the AutomaticChargeConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef automatic_charge::AutomaticChargeConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ AutomaticChargeAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all automatic_charge_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~AutomaticChargeAlgorithm(void);
+};
+
+#endif
diff --git a/automatic_charge/include/automatic_charge_alg_node.h b/automatic_charge/include/automatic_charge_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..4226f4466af1b11cbab4bd0f2ae726b0d2574272
--- /dev/null
+++ b/automatic_charge/include/automatic_charge_alg_node.h
@@ -0,0 +1,189 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _automatic_charge_alg_node_h_
+#define _automatic_charge_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "automatic_charge_alg.h"
+
+#include <humanoid_modules/search_module.h>
+
+// [publisher subscriber headers]
+//#include <darwin_smart_charger_client/smart_charger_data.h>
+#include <humanoid_common_msgs/smart_charger_data.h>
+
+// [service client headers]
+#include <humanoid_common_msgs/get_smart_charger_config.h>
+#include <humanoid_common_msgs/set_smart_charger_config.h>
+
+// [action server client headers]
+
+
+typedef enum {IDLE,
+ SOC,
+ SEARCH,
+ TRACK,
+ GO_TO_CHARGE,
+ CONNECT,
+ DISCONNECT,
+ END} automatic_charge_states_t;
+
+typedef enum {IS_IDLE,
+ RUNNING,
+ SUCCEEDED,
+ ABORTED}status;
+
+typedef enum {WALKING,
+ GRIPPERS}status2;
+
+typedef enum {UNKNOWN,
+ CONNECTED,
+ DISCONNECTED}batt_status;
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<AutomaticChargeAlgorithm>
+{
+ private:
+ // [publisher attributes]
+
+ // [subscriber attributes]
+ ros::Subscriber smart_charger_data_subscriber_;
+ void smart_charger_data_callback(const humanoid_common_msgs::smart_charger_data::ConstPtr& msg);
+ pthread_mutex_t smart_charger_data_mutex_;
+ void smart_charger_data_mutex_enter(void);
+ void smart_charger_data_mutex_exit(void);
+
+
+ // [service attributes]
+
+ // [client attributes]
+ ros::ServiceClient get_smart_charger_config_client_;
+ humanoid_common_msgs::get_smart_charger_config get_smart_charger_config_srv_;
+
+ ros::ServiceClient set_smart_charger_config_client_;
+ humanoid_common_msgs::set_smart_charger_config set_smart_charger_config_srv_;
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ //State machine state
+ automatic_charge_states_t state;
+ status current_status;
+
+ //Start and stop module variables
+ bool start;
+ bool stop;
+ // std::string location_id;
+
+ batt_status battery_status;
+ uint16_t low_battery_limit;
+ uint16_t high_battery_limit;
+ bool charged;
+ bool discharged;
+
+ humanoid_common_msgs::smart_charger_data charger_data; //saves data received from smart_charger_data_callback
+
+
+ CSearchModule search_module;
+
+
+ //FUNCTIONS
+ // void process_data(void);
+
+
+
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ AutomaticChargeAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~AutomaticChargeAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+
+ void config_smart_charger(void);
+};
+
+#endif
diff --git a/automatic_charge/launch/automatic_charge_sim.launch b/automatic_charge/launch/automatic_charge_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..386bc737d8ad4e71a71398afb5ab50f81814baff
--- /dev/null
+++ b/automatic_charge/launch/automatic_charge_sim.launch
@@ -0,0 +1,86 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="charge_env" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find darwin_description)/launch/charge_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+ <!-- launch the action client node -->
+ <node name="automatic_charge"
+ pkg="automatic_charge"
+ type="automatic_charge"
+ output="screen"
+ ns="/darwin">
+<!-- <param name="joint_trajectory/enable_timeout" value="false"/>
+ <param name="joint_trajectory/rate_hz" value="5.0"/>
+ <param name="/walk/rate_hz" value="5.0"/>
+ <param name="walk/cmd_vel_rate_hz" value="50.0"/>
+ -->
+ <remap from="/darwin/automatic_charge/smart_charger_data"
+ to="/darwin/robot/smart_charger_data"/>
+ <remap from="/darwin/automatic_charge/get_smart_charger_config"
+ to="/darwin/robot/get_smart_charger_config"/>
+ <remap from="/darwin/automatic_charge/set_smart_charger_config"
+ to="/darwin/robot/set_smart_charger_config"/>
+<!-- Search module
+ <remap from="/darwin/automatic_charge/search_module/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/automatic_charge/search_module/head_follow_target"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/automatic_charge/search_module/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/automatic_charge/search_module/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/automatic_charge/search_module/qr_pose"
+ to="/qr_detector/qr_pose"/>
+ <remap from="/darwin/automatic_charge/search_module/odom"
+ to="/darwin/robot/odom" /> -->
+<!-- Walk module -->
+ <remap from="/darwin/automatic_charge/search_module/walk/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/automatic_charge/search_module/walk/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/automatic_charge/search_module/walk/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/automatic_charge/search_module/walk/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/automatic_charge/search_module/walk/joint_states"
+ to="/darwin/joint_states"/>
+<!-- Joints module
+ <remap from="/darwin/automatic_charge/search_module/joint_trajectory/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/automatic_charge/search_module/joint_trajectory/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/automatic_charge/search_module/joint_trajectory/joint_trajectory_action"
+ to="/darwin/robot/joint_trajectory"/> -->
+ </node>
+
+ <node pkg="qr_detector"
+ name="qr_detector"
+ type="qr_detector"
+ output="screen">
+ <param name="qr_x" value="0.14"/>
+ <param name="qr_y" value="0.14"/>
+ <param name="camera_frame" value="/darwin/camera_link"/>d
+ <remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
+ <remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
+ </node>
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/camera/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+
+</launch>
\ No newline at end of file
diff --git a/automatic_charge/package.xml b/automatic_charge/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7b5cd249eacec1490228bfde5439364ad1292ae8
--- /dev/null
+++ b/automatic_charge/package.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+ <name>automatic_charge</name>
+ <version>0.0.0</version>
+ <description>The automatic_charge package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="igarcia@todo.todo">igarcia</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/smart_charger</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
+ <build_depend>iri_ros_tools</build_depend>
+ <build_depend>humanoid_modules</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
+ <run_depend>iri_ros_tools</run_depend>
+ <run_depend>humanoid_modules</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/automatic_charge/src/automatic_charge_alg.cpp b/automatic_charge/src/automatic_charge_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39b82737c65d412ed8af7f75555b260e1dff02f1
--- /dev/null
+++ b/automatic_charge/src/automatic_charge_alg.cpp
@@ -0,0 +1,23 @@
+#include "automatic_charge_alg.h"
+
+AutomaticChargeAlgorithm::AutomaticChargeAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+AutomaticChargeAlgorithm::~AutomaticChargeAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void AutomaticChargeAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// AutomaticChargeAlgorithm Public API
diff --git a/automatic_charge/src/automatic_charge_alg_node.cpp b/automatic_charge/src/automatic_charge_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..44cb93b67e597d137eaf0ceb5542a002c5ecff3f
--- /dev/null
+++ b/automatic_charge/src/automatic_charge_alg_node.cpp
@@ -0,0 +1,350 @@
+#include "automatic_charge_alg_node.h"
+
+AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<AutomaticChargeAlgorithm>(),
+ search_module("search_module")
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+
+ // [init subscribers]
+ this->smart_charger_data_subscriber_ = this->public_node_handle_.subscribe("smart_charger_data", 1, &AutomaticChargeAlgNode::smart_charger_data_callback, this);
+ pthread_mutex_init(&this->smart_charger_data_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+ get_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_smart_charger_config>("get_smart_charger_config");
+
+ set_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_smart_charger_config>("set_smart_charger_config");
+
+ // [init action servers]
+
+ // [init action clients]
+
+ this->state = IDLE;
+ this->current_status = IS_IDLE;
+
+ this->start=false;
+ this->stop=false;
+ // this->location_id="";
+
+ this->low_battery_limit=10;
+ this->high_battery_limit=10;
+ this->battery_status=DISCONNECTED;
+ this->charged=false;
+ this->discharged=false;
+}
+
+AutomaticChargeAlgNode::~AutomaticChargeAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->smart_charger_data_mutex_);
+}
+
+void AutomaticChargeAlgNode::mainNodeThread(void)
+{
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+
+ this->alg_.lock();
+ if(this->stop)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Automatic charge CANCELED!");
+ this->state=IDLE;
+ }
+ else
+ {
+ switch(this->state)
+ {
+ case IDLE: ROS_INFO("AutomaticChargeAlgNode: state IDLE");
+ if(this->start)
+ {
+ this->start=false;
+ this->state=SOC;
+ }
+ else
+ this->state=IDLE;
+ break;
+
+ case SOC: ROS_INFO("AutomaticChargeAlgNode: state SOC");
+ if(battery_status==DISCONNECTED)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Batteries disconnected!");
+ this->state=END;
+ }
+ if(battery_status=CONNECTED)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Batteries connected");
+ if(this->discharged)
+ {
+ this->state=SEARCH;
+ this->search_module.start_search(config_.angle_rotation, config_.error);
+ }
+ else if(this->charged)
+ this->state=DISCONNECT;
+ else
+ this->state=SOC;
+ }
+ if(battery_status=UNKNOWN)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Unknown state of batteries!");
+ this->state=END;
+ }
+ break;
+
+ case SEARCH: ROS_INFO("AutomaticChargeAlgNode: state SEARCH");
+ if(!this->search_module.is_finished())
+ this->state=SEARCH;
+ else
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Search completed");
+ if(this->search_module.get_status()==SEARCH_MODULE_SUCCESS)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: Search completed successfully!");
+ //this->track_module.start_tracking();
+ this->state=TRACK;
+ }
+ else
+ this->state=END;
+ }
+ break;
+
+ case TRACK: ROS_INFO("AutomaticChargeAlgNode: state TRACK");
+ //if(!this->track_module.is_finished())
+ this->state=END;
+ break;
+
+ case END: ROS_INFO("AutomaticChargeAlgNode: state END");
+ //posibles errores, estado etc
+ this->state=IDLE;
+ break;
+ }
+ }
+}
+
+/* [subscriber callbacks] */
+void AutomaticChargeAlgNode::smart_charger_data_callback(const humanoid_common_msgs::smart_charger_data::ConstPtr& msg)
+{
+ //ROS_INFO("AutomaticChargeAlgNode::smart_charger_data_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ this->alg_.lock();
+ this->smart_charger_data_mutex_enter();
+
+ //Save message received to local variable
+ this->charger_data=*msg;
+
+ if(charger_data.batt_status=="Disconnected")
+ {
+ this->battery_status=DISCONNECTED;
+ }
+ if(charger_data.batt_status=="Connected")
+ {
+ this->battery_status=CONNECTED;
+ charged=false;
+ if(charger_data.avg_time_empty<low_battery_limit)
+ discharged=true;
+ }
+ if(charger_data.batt_status=="Connected and charging")
+ {
+ this->battery_status=CONNECTED;
+ discharged=false;
+ if(charger_data.avg_time_full<high_battery_limit)
+ charged=true;
+ }
+ if(charger_data.batt_status=="Unknown")
+ {
+ battery_status=UNKNOWN;
+ }
+
+
+ //unlock previously blocked shared variables
+ this->alg_.unlock();
+ this->smart_charger_data_mutex_exit();
+}
+
+void AutomaticChargeAlgNode::smart_charger_data_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_data_mutex_);
+}
+
+void AutomaticChargeAlgNode::smart_charger_data_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_data_mutex_);
+}
+
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void AutomaticChargeAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+ this->config_=config;
+
+ if(config.start)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: START!");
+ this->start=config.start;
+ config.start=false;
+ }
+
+ if(config.stop)
+ {
+ ROS_INFO("AutomaticChargeAlgNode: STOP!");
+ this->stop=config.stop;
+ config.stop=false;
+ }
+ // this->location_id=config.location_id;
+ if(config.set_config)
+ {
+ this->config_smart_charger();
+ config.set_config=false;
+ }
+
+ this->alg_.unlock();
+}
+
+void AutomaticChargeAlgNode::addNodeDiagnostics(void)
+{
+}
+
+
+
+//FUNCTIONS
+/*
+void AutomaticChargeAlgNode::process_data(void)
+{
+ if(charger_data.batt_status=="Disconnected")
+ {
+ battery_status=DISCONNECTED;
+ }
+ if(charger_data.batt_status=="Connected")
+ {
+ battery_status=CONNECTED;
+ charged=false;
+ if(charger_data.avg_time_empty<low_battery_limit)
+ discharged=true;
+ }
+ if(charger_data.batt_status=="Connected and charging")
+ {
+ battery_status=CONNECTED;
+ discharged=false;
+ if(charger_data.avg_time_full<high_battery_limit)
+ charged=true;
+ }
+ if(charger_data.batt_status=="Unknown")
+ {
+ battery_status=UNKNOWN;
+ }
+
+}
+*/
+
+void AutomaticChargeAlgNode::config_smart_charger(void)
+{
+ //Get smart charger configuration
+ if(config_.get_config)
+ {
+ ROS_INFO("SmartChargerClientAlgNode:: Sending New Request! GET CONFIG");
+ if(get_smart_charger_config_client_.call(get_smart_charger_config_srv_))
+ {
+ // period=get_smart_charger_config_srv_.response.charger_config.period;
+ // current=get_smart_charger_config_srv_.response.charger_config.current;
+ if(get_smart_charger_config_srv_.response.charger_config.enable)
+ ROS_INFO_STREAM("SmartChargerClientAlgNode:: Smart charger enabled:");
+
+ if(get_smart_charger_config_srv_.response.charger_config.disable)
+ ROS_INFO_STREAM("SmartChargerClientAlgNode:: Smart charger disabled:");
+
+ ROS_INFO_STREAM("SmartChargerClientAlgNode:: Smart charger period:" << get_smart_charger_config_srv_.response.charger_config.period);
+ ROS_INFO_STREAM("SmartChargerClientAlgNode:: Smart charger current:" << get_smart_charger_config_srv_.response.charger_config.current);
+ //else
+ // ROS_INFO("SmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_get_period ");
+ }
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to Call Server on topic get_smart_charger_period ");
+ config_.get_config=false;
+ }
+
+ //Set smart charger configuration
+ if(config_.set_config)
+ {
+ if(config_.enable)
+ {
+ set_smart_charger_config_srv_.request.charger_config.enable=true;
+ set_smart_charger_config_srv_.request.charger_config.disable=false;
+ set_smart_charger_config_srv_.request.charger_config.period=config_.period;
+ set_smart_charger_config_srv_.request.charger_config.current=config_.limit_current;
+
+ ROS_INFO("SmartChargerClientAlgNode:: Sending New Request! ENABLE");
+ if(set_smart_charger_config_client_.call(set_smart_charger_config_srv_))
+ {
+ //if return ok
+ if(this->set_smart_charger_config_srv_.response.success)
+ ROS_INFO("SmartChargerClientAlgNode:: Smart charger enabled");
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to enable smart charger module ");
+ }
+ config_.enable=false;
+ }
+
+//Disable smart charger module
+ else if(config_.disable)
+ {
+ set_smart_charger_config_srv_.request.charger_config.enable=false;
+ set_smart_charger_config_srv_.request.charger_config.disable=true;
+ set_smart_charger_config_srv_.request.charger_config.period=config_.period;
+ set_smart_charger_config_srv_.request.charger_config.current=config_.limit_current;
+
+ ROS_INFO("SmartChargerClientAlgNode:: Sending New Request! DISABLE");
+ if(set_smart_charger_config_client_.call(set_smart_charger_config_srv_))
+ {
+ //if return ok
+ if(this->set_smart_charger_config_srv_.response.success)
+ ROS_INFO("SmartChargerClientAlgNode:: Smart charger disabled");
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to disable smart charger module ");
+ }
+ config_.disable=false;
+ }
+// Set period and limit current
+ else
+ {
+ set_smart_charger_config_srv_.request.charger_config.enable=false;
+ set_smart_charger_config_srv_.request.charger_config.disable=false;
+ set_smart_charger_config_srv_.request.charger_config.period=config_.period;
+ set_smart_charger_config_srv_.request.charger_config.current=config_.limit_current;
+
+ ROS_INFO("SmartChargerClientAlgNode:: Sending New Request!");
+ if(set_smart_charger_config_client_.call(set_smart_charger_config_srv_))
+ {
+ //if return ok
+ if(this->set_smart_charger_config_srv_.response.success)
+ ROS_INFO("SmartChargerClientAlgNode:: Smart charger configured");
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to configure smart charger module ");
+ }
+ }
+ config_.set_config=false;
+ }
+}
+
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<AutomaticChargeAlgNode>(argc, argv, "automatic_charge_alg_node");
+}
diff --git a/humanoid_common/package.xml b/humanoid_common/package.xml
index 8eab9eacbdfaaa190970cf74f68f6020e73d41e2..47febb24175e610391262514aec7f71f71268f16 100755
--- a/humanoid_common/package.xml
+++ b/humanoid_common/package.xml
@@ -52,6 +52,7 @@
<run_depend>humanoid_modules</run_depend>
<run_depend>joints_cart_client</run_depend>
<run_depend>smart_charger_client</run_depend>
+ <run_depend>automatic_charge</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>