diff --git a/humanoid_common/package.xml b/humanoid_common/package.xml
index 7c5140f989f2bed54f90a5e70fadd19bb5e46ae4..8eab9eacbdfaaa190970cf74f68f6020e73d41e2 100755
--- a/humanoid_common/package.xml
+++ b/humanoid_common/package.xml
@@ -51,6 +51,7 @@
<run_depend>teleop</run_depend>
<run_depend>humanoid_modules</run_depend>
<run_depend>joints_cart_client</run_depend>
+ <run_depend>smart_charger_client</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/humanoid_common_msgs/CMakeLists.txt b/humanoid_common_msgs/CMakeLists.txt
index 22437b4378fed0e5ea48b1757669bc1d2d238e8b..46f31017271d0e61b4b18d987686e326ed8d4f22 100644
--- a/humanoid_common_msgs/CMakeLists.txt
+++ b/humanoid_common_msgs/CMakeLists.txt
@@ -47,6 +47,8 @@ add_message_files(
tag_pose_array.msg
buttons.msg
leds.msg
+ smart_charger_config.msg
+ smart_charger_data.msg
)
## Generate services in the 'srv' folder
@@ -57,6 +59,8 @@ add_service_files(
set_servo_modules.srv
set_pid.srv
get_pid.srv
+ set_smart_charger_config.srv
+ get_smart_charger_config.srv
)
## Generate actions in the 'action' folder
diff --git a/humanoid_common_msgs/msg/smart_charger_config.msg b/humanoid_common_msgs/msg/smart_charger_config.msg
new file mode 100644
index 0000000000000000000000000000000000000000..f239c3b9d46521103e515515f4fea29898410b94
--- /dev/null
+++ b/humanoid_common_msgs/msg/smart_charger_config.msg
@@ -0,0 +1,4 @@
+bool enable
+bool disable
+float32 period
+float32 current
\ No newline at end of file
diff --git a/humanoid_common_msgs/msg/smart_charger_data.msg b/humanoid_common_msgs/msg/smart_charger_data.msg
new file mode 100644
index 0000000000000000000000000000000000000000..95d386bc7620130d42c2d693035543e8ce0e4e41
--- /dev/null
+++ b/humanoid_common_msgs/msg/smart_charger_data.msg
@@ -0,0 +1,5 @@
+Header header
+uint16 avg_time_empty
+uint16 avg_time_full
+string batt_status
+
diff --git a/humanoid_common_msgs/srv/get_smart_charger_config.srv b/humanoid_common_msgs/srv/get_smart_charger_config.srv
new file mode 100644
index 0000000000000000000000000000000000000000..a732dcb8018f62c8305e31ea8185017cd49b5bcd
--- /dev/null
+++ b/humanoid_common_msgs/srv/get_smart_charger_config.srv
@@ -0,0 +1,2 @@
+---
+humanoid_common_msgs/smart_charger_config charger_config
diff --git a/humanoid_common_msgs/srv/set_smart_charger_config.srv b/humanoid_common_msgs/srv/set_smart_charger_config.srv
new file mode 100644
index 0000000000000000000000000000000000000000..2d5b5e1e97684337d2a638251e4ace599cbbeac0
--- /dev/null
+++ b/humanoid_common_msgs/srv/set_smart_charger_config.srv
@@ -0,0 +1,3 @@
+humanoid_common_msgs/smart_charger_config charger_config
+---
+bool success
\ No newline at end of file
diff --git a/smart_charger_client/CMakeLists.txt b/smart_charger_client/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..846d14010cb9428d395e816ebe0581c20f2c758e
--- /dev/null
+++ b/smart_charger_client/CMakeLists.txt
@@ -0,0 +1,101 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(smart_charger_client)
+
+## 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)
+
+## 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/SmartChargerClient.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
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+# 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/smart_charger_client_alg.cpp src/smart_charger_client_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# 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/smart_charger_client/cfg/SmartChargerClient.cfg b/smart_charger_client/cfg/SmartChargerClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..67f3f44b8ce3f6d8e980e5a970cf396690344bac
--- /dev/null
+++ b/smart_charger_client/cfg/SmartChargerClient.cfg
@@ -0,0 +1,49 @@
+#! /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_client'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+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 )
+
+
+exit(gen.generate(PACKAGE, "SmartChargerClientAlgorithm", "SmartChargerClient"))
diff --git a/smart_charger_client/include/smart_charger_client_alg.h b/smart_charger_client/include/smart_charger_client_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..d63b14d05df76c0a22f8578a912f160ccabdc8ab
--- /dev/null
+++ b/smart_charger_client/include/smart_charger_client_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 _smart_charger_client_alg_h_
+#define _smart_charger_client_alg_h_
+
+#include <smart_charger_client/SmartChargerClientConfig.h>
+
+//include smart_charger_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class SmartChargerClientAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the SmartChargerClientConfig. 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 SmartChargerClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef smart_charger_client::SmartChargerClientConfig 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.
+ */
+ SmartChargerClientAlgorithm(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 smart_charger_client_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~SmartChargerClientAlgorithm(void);
+};
+
+#endif
diff --git a/smart_charger_client/include/smart_charger_client_alg_node.h b/smart_charger_client/include/smart_charger_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3ceac556465e2b3c41cfbbff300f21253ccfc79
--- /dev/null
+++ b/smart_charger_client/include/smart_charger_client_alg_node.h
@@ -0,0 +1,130 @@
+// 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 _smart_charger_client_alg_node_h_
+#define _smart_charger_client_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "smart_charger_client_alg.h"
+
+// [publisher subscriber headers]
+
+// [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]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class SmartChargerClientAlgNode : public algorithm_base::IriBaseAlgorithm<SmartChargerClientAlgorithm>
+{
+ private:
+ // [publisher attributes]
+
+ // [subscriber attributes]
+
+ // [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]
+
+ /**
+ * \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.
+ */
+ SmartChargerClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~SmartChargerClientAlgNode(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]
+};
+
+#endif
diff --git a/smart_charger_client/launch/smart_charger_client.launch b/smart_charger_client/launch/smart_charger_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..27e3948fa914ab74be2b8d4b53fa94a07b5b8ad7
--- /dev/null
+++ b/smart_charger_client/launch/smart_charger_client.launch
@@ -0,0 +1,50 @@
+<launch>
+
+ <arg name="reconfigure"
+ default="true"/>
+
+ <node pkg="smart_charger_client"
+ type="smart_charger_client"
+ name="smart_charger_client"
+ output="screen">
+ <param name="get_config"
+ type="bool"
+ value="false"/>
+ <param name="set_config"
+ type="bool"
+ value="false"/>
+ <param name="enable"
+ type="bool"
+ value="false"/>
+ <param name="disable"
+ type="bool"
+ value="false"/>
+ <param name="period"
+ type="double"
+ value="1.5"/>
+ <param name="limit_current"
+ type="double"
+ value="0.512"/>
+ <remap from="/smart_charger_client/get_smart_charger_config"
+ to="/darwin/get_smart_charger_config"/>
+ <remap from="/smart_charger_client/set_smart_charger_config"
+ to="/darwin/set_smart_charger_config"/>
+ <remap from="/smart_charger_client/smart_charger_data"
+ to="/darwin/smart_charger_data"/>
+ </node>
+
+ <node if="$(arg reconfigure)"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ name="rqt_reconfigure"/>
+
+ <node name="darwin"
+ pkg="darwin_driver"
+ type="darwin_driver"
+ output="screen">
+ <param name="darwin_serial" value="A603LOBS"/>
+ <param name="darwin_baudrate" value="1000000"/>
+ <param name="darwin_id" value="2"/>
+ </node>
+
+</launch>
\ No newline at end of file
diff --git a/smart_charger_client/package.xml b/smart_charger_client/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2872fcb1bd113a816255ad9f9db58fdccf7247a0
--- /dev/null
+++ b/smart_charger_client/package.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+ <name>smart_charger_client</name>
+ <version>0.0.0</version>
+ <description>The smart_charger_client 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_client</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>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/smart_charger_client/src/smart_charger_client_alg.cpp b/smart_charger_client/src/smart_charger_client_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4ee8069688f40d18700ba5e2e85dd13b844b806
--- /dev/null
+++ b/smart_charger_client/src/smart_charger_client_alg.cpp
@@ -0,0 +1,23 @@
+#include "smart_charger_client_alg.h"
+
+SmartChargerClientAlgorithm::SmartChargerClientAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+SmartChargerClientAlgorithm::~SmartChargerClientAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void SmartChargerClientAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// SmartChargerClientAlgorithm Public API
diff --git a/smart_charger_client/src/smart_charger_client_alg_node.cpp b/smart_charger_client/src/smart_charger_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..427465a9d1dc1f1f05c3545dca021f6b884a7eae
--- /dev/null
+++ b/smart_charger_client/src/smart_charger_client_alg_node.cpp
@@ -0,0 +1,179 @@
+#include "smart_charger_client_alg_node.h"
+
+SmartChargerClientAlgNode::SmartChargerClientAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<SmartChargerClientAlgorithm>()
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [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]
+}
+
+SmartChargerClientAlgNode::~SmartChargerClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+void SmartChargerClientAlgNode::mainNodeThread(void)
+{
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+ //get_smart_charger_config_srv_.request.data = my_var;
+ //ROS_INFO("SmartChargerClientAlgNode:: Sending New Request!");
+ //if (get_smart_charger_config_client_.call(get_smart_charger_config_srv_))
+ //{
+ //ROS_INFO("SmartChargerClientAlgNode:: Response: %s", get_smart_charger_config_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("SmartChargerClientAlgNode:: Failed to Call Server on topic get_smart_charger_config ");
+ //}
+
+
+ //set_smart_charger_config_srv_.request.data = my_var;
+ //ROS_INFO("SmartChargerClientAlgNode:: Sending New Request!");
+ //if (set_smart_charger_config_client_.call(set_smart_charger_config_srv_))
+ //{
+ //ROS_INFO("SmartChargerClientAlgNode:: Response: %s", set_smart_charger_config_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("SmartChargerClientAlgNode:: Failed to Call Server on topic set_smart_charger_config ");
+ //}
+
+
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void SmartChargerClientAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+
+ //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;
+ }
+
+
+
+
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void SmartChargerClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<SmartChargerClientAlgNode>(argc, argv, "smart_charger_client_alg_node");
+}