diff --git a/humanoid_common_msgs/msg/smart_charger_data.msg b/humanoid_common_msgs/msg/smart_charger_data.msg
index 95d386bc7620130d42c2d693035543e8ce0e4e41..f9e45080b9df97a4093ffd9538c96b050337516a 100644
--- a/humanoid_common_msgs/msg/smart_charger_data.msg
+++ b/humanoid_common_msgs/msg/smart_charger_data.msg
@@ -1,5 +1,5 @@
Header header
-uint16 avg_time_empty
-uint16 avg_time_full
+float32 avg_time_empty
+float32 avg_time_full
string batt_status
diff --git a/smart_charger_client/include/smart_charger_client_alg_node.h b/smart_charger_client/include/smart_charger_client_alg_node.h
index a3ceac556465e2b3c41cfbbff300f21253ccfc79..1206b031e57360c9296d9e1027fa1e3000c2c565 100644
--- a/smart_charger_client/include/smart_charger_client_alg_node.h
+++ b/smart_charger_client/include/smart_charger_client_alg_node.h
@@ -29,6 +29,7 @@
#include "smart_charger_client_alg.h"
// [publisher subscriber headers]
+#include <humanoid_common_msgs/smart_charger_data.h>
// [service client headers]
#include <humanoid_common_msgs/get_smart_charger_config.h>
@@ -36,6 +37,8 @@
// [action server client headers]
+typedef enum{IDLE, HOLA
+}states;
/**
* \brief IRI ROS Specific Algorithm Class
*
@@ -46,6 +49,11 @@ class SmartChargerClientAlgNode : public algorithm_base::IriBaseAlgorithm<SmartC
// [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]
@@ -67,6 +75,9 @@ class SmartChargerClientAlgNode : public algorithm_base::IriBaseAlgorithm<SmartC
* This variable has all the driver parameters defined in the cfg config file.
* Is updated everytime function config_update() is called.
*/
+
+ states state;
+
Config config_;
public:
/**
diff --git a/smart_charger_client/launch/smart_charger_client_sim.launch b/smart_charger_client/launch/smart_charger_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..007c9ae0f202d78683f011411238f9cdd4667061
--- /dev/null
+++ b/smart_charger_client/launch/smart_charger_client_sim.launch
@@ -0,0 +1,48 @@
+<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>
+
+ <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/robot/get_smart_charger_config"/>
+ <remap from="/smart_charger_client/set_smart_charger_config"
+ to="/darwin/robot/set_smart_charger_config"/>
+ <remap from="/smart_charger_client/smart_charger_data"
+ to="/darwin/robot/smart_charger_data"/>
+ </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/smart_charger_client/src/smart_charger_client_alg_node.cpp b/smart_charger_client/src/smart_charger_client_alg_node.cpp
index 427465a9d1dc1f1f05c3545dca021f6b884a7eae..00cec3419cee63f52aea006e47752524351ce6dc 100644
--- a/smart_charger_client/src/smart_charger_client_alg_node.cpp
+++ b/smart_charger_client/src/smart_charger_client_alg_node.cpp
@@ -1,3 +1,6 @@
+
+//AÑADIR SMART CHARGER DATA SUBSCRIBER!!
+
#include "smart_charger_client_alg_node.h"
SmartChargerClientAlgNode::SmartChargerClientAlgNode(void) :
@@ -9,6 +12,8 @@ SmartChargerClientAlgNode::SmartChargerClientAlgNode(void) :
// [init publishers]
// [init subscribers]
+ this->smart_charger_data_subscriber_ = this->public_node_handle_.subscribe("smart_charger_data", 1, &SmartChargerClientAlgNode::smart_charger_data_callback, this);
+ pthread_mutex_init(&this->smart_charger_data_mutex_,NULL);
// [init services]
@@ -21,6 +26,8 @@ SmartChargerClientAlgNode::SmartChargerClientAlgNode(void) :
// [init action servers]
// [init action clients]
+
+ state=IDLE;
}
SmartChargerClientAlgNode::~SmartChargerClientAlgNode(void)
@@ -61,9 +68,57 @@ void SmartChargerClientAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
+
}
/* [subscriber callbacks] */
+void SmartChargerClientAlgNode::smart_charger_data_callback(const humanoid_common_msgs::smart_charger_data::ConstPtr& msg)
+{
+ ROS_INFO("SmartChargerClientAlgNode::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(msg->batt_status=="Disconnected")
+ {
+ ROS_INFO("Battery disconnected");
+ }
+ if(msg->batt_status=="Connected")
+ {
+ ROS_INFO("Battery connected");
+ // if(msg->avg_time_empty < low_battery_limit)
+ ROS_INFO("Avg time to empty:");
+ }
+ if(msg->batt_status=="Connected and charging")
+ {
+ ROS_INFO("Battery connected and charging");
+ // if(charger_data.avg_time_full<high_battery_limit)
+ ROS_INFO("Avg time to full:");
+ }
+ if(msg->batt_status=="Unknown")
+ {
+ ROS_INFO("Battery status unkown");
+ }
+
+
+ //unlock previously blocked shared variables
+// this->alg_.unlock();
+ // this->smart_charger_data_mutex_exit();
+}
+
+void SmartChargerClientAlgNode::smart_charger_data_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_data_mutex_);
+}
+
+void SmartChargerClientAlgNode::smart_charger_data_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_data_mutex_);
+}
/* [service callbacks] */
@@ -95,7 +150,7 @@ void SmartChargerClientAlgNode::node_config_update(Config &config, uint32_t leve
// 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 ");
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to Call Server on topic get_smart_charger_config ");
config.get_config=false;
}
@@ -118,6 +173,8 @@ void SmartChargerClientAlgNode::node_config_update(Config &config, uint32_t leve
else
ROS_INFO("SmartChargerClientAlgNode:: Failed to enable smart charger module ");
}
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to call server to enable smart charger module ");
config.enable=false;
}
@@ -138,6 +195,8 @@ void SmartChargerClientAlgNode::node_config_update(Config &config, uint32_t leve
else
ROS_INFO("SmartChargerClientAlgNode:: Failed to disable smart charger module ");
}
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to call server to disable smart charger module ");
config.disable=false;
}
// Set period and limit current
@@ -157,6 +216,8 @@ void SmartChargerClientAlgNode::node_config_update(Config &config, uint32_t leve
else
ROS_INFO("SmartChargerClientAlgNode:: Failed to configure smart charger module ");
}
+ else
+ ROS_INFO("SmartChargerClientAlgNode:: Failed to call server to config smart charger module ");
}
config.set_config=false;
}