Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22723] pkg/trunk/stacks
|
From: <wg_...@us...> - 2009-08-24 19:08:51
|
Revision: 22723
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22723&view=rev
Author: wg_cmeyers
Date: 2009-08-24 19:08:39 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
Finished up the power monitor, is now collecting some data from the battery server.
Modified Paths:
--------------
pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp
pkg/trunk/stacks/pr2_power_drivers/power_monitor/src/power_monitor.cpp
Added Paths:
-----------
pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/Makefile
Modified: pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg 2009-08-24 19:08:19 UTC (rev 22722)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/msg/BatteryServer.msg 2009-08-24 19:08:39 UTC (rev 22723)
@@ -1,6 +1,7 @@
Header header
uint32 MAX_BAT_COUNT=4
uint32 MAX_BAT_REG=48
+int32 id # unique ID for each battery server
# Battery System Stats
int32 lastTimeSystem #epoch time
uint16 timeLeft # in minutes
Added: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/Makefile
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/Makefile (rev 0)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/Makefile 2009-08-24 19:08:39 UTC (rev 22723)
@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp 2009-08-24 19:08:19 UTC (rev 22722)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.cpp 2009-08-24 19:08:39 UTC (rev 22723)
@@ -69,9 +69,10 @@
* the GPS.
*/
-ocean::ocean ( int debug)
+ocean::ocean ( int id, int debug)
{
debuglevel = debug;
+ server.id = id;
server.battery.resize(4);
}
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h 2009-08-24 19:08:19 UTC (rev 22722)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/ocean.h 2009-08-24 19:08:39 UTC (rev 22723)
@@ -35,7 +35,7 @@
};
- ocean (int debug = 0);
+ ocean (int id, int debug = 0);
~ocean ();
int run ();
Modified: pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp 2009-08-24 19:08:19 UTC (rev 22722)
+++ pkg/trunk/stacks/pr2_power_drivers/ocean_battery_driver/src/ocean_battery_driver/server.cpp 2009-08-24 19:08:39 UTC (rev 22723)
@@ -23,8 +23,6 @@
using namespace willowgarage::ocean;
namespace po = boost::program_options;
-boost::mutex data_lock;
-
float toFloat(const int &value)
{
int tmp = value;
@@ -79,12 +77,6 @@
myThread->join();
}
- float getVoltage(int bat)
- {
- boost::mutex::scoped_lock lock(data_lock);
- return battery_voltage[bat];
- }
-
private:
ros::NodeHandle handle;
@@ -93,7 +85,6 @@
std::string serial_device;
volatile bool stopRequest;
boost::shared_ptr<boost::thread> myThread;
- float battery_voltage[4];
void run()
{
@@ -111,7 +102,7 @@
diagnostic_updater::DiagnosticStatusWrapper stat;
Time lastTime, currentTime;
Duration MESSAGE_TIME(10,0); //the message output rate
- ocean os(debug_level);
+ ocean os( majorID, debug_level);
os.initialize(serial_device.c_str());
lastTime = Time::now();
@@ -138,7 +129,6 @@
stat.level = 0;
stat.message = "OK";
-#if 1
stat.add("Time Remaining (min)", os.server.timeLeft);
stat.add("Average charge (percent)", os.server.averageCharge );
stat.add("Time since update (s)", (currentTime.sec - os.server.lastTimeSystem));
@@ -182,15 +172,8 @@
msg_out.status.push_back(stat);
-#if 1
- {
- boost::mutex::scoped_lock lock(data_lock);
- battery_voltage[xx] = toFloat(os.server.battery[xx].batReg[0x9]);
- } //end mutex lock
-#endif
}
}
-#endif
pub.publish(msg_out);
msg_out.status.clear();
Modified: pkg/trunk/stacks/pr2_power_drivers/power_monitor/src/power_monitor.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/power_monitor/src/power_monitor.cpp 2009-08-24 19:08:19 UTC (rev 22722)
+++ pkg/trunk/stacks/pr2_power_drivers/power_monitor/src/power_monitor.cpp 2009-08-24 19:08:39 UTC (rev 22723)
@@ -24,36 +24,77 @@
void batteryUpdate( const boost::shared_ptr<pr2_msgs::BatteryServer const> &bat)
{
- ROS_INFO("Received battery message: voltage=%f", toFloat(bat->battery[0].batReg[0x9]));
+ boost::mutex::scoped_lock lock(vLock);
+ ROS_DEBUG("Received battery message: voltage=%f", toFloat(bat->battery[0].batReg[0x9]));
+
+ batteryServers[bat->id] = bat; //add results to our map
};
- void send()
+ void send(const ros::TimerEvent &e)
{
- #if 0
- float min_voltage = server_list[0].getVoltage(0);
- for(int xx = 1; xx < 4; ++xx)
+ float min_voltage(1000.0);
+ int acCount(0);
+ //float totalCurrent[batteryServers.size()];
+ float totalPower(0.0);
+ unsigned int minTime(10000);
+
+ boost::mutex::scoped_lock lock(vLock);
+ map< int, boost::shared_ptr<pr2_msgs::BatteryServer const> >::iterator itr = batteryServers.begin();
+ for( ; itr != batteryServers.end(); ++itr )
{
- if(server_list[0].getVoltage(xx) < min_voltage)
- min_voltage = server_list[0].getVoltage(xx);
+ const pr2_msgs::BatteryServer *bat = itr->second.get();
+
+ ROS_DEBUG("------------------------------------");
+ ROS_DEBUG("BATTERY %d", bat->id);
+
+ if(bat->powerPresent == 0xF) //All four batteries show AC present
+ ++acCount;
+
+
+ for(unsigned int xx = 0; xx < bat->MAX_BAT_COUNT; ++xx)
+ {
+ float voltage = toFloat(bat->battery[xx].batReg[0x9]);
+ ROS_DEBUG(" voltage=%f", voltage);
+ if(voltage < min_voltage)
+ min_voltage = voltage;
+
+ float current = toFloat(bat->battery[xx].batReg[0xA]);
+ ROS_DEBUG(" current=%f", current);
+ ROS_DEBUG(" power=%f", current * voltage);
+ totalPower += (current * voltage);
+
+ unsigned tte = bat->battery[xx].batReg[0x12];
+ if(tte < minTime) //search for battery is least time remaining
+ minTime = tte;
+ }
+
+ //totalCurrent[bat->id] = tmpCurrent;
+ //cout << " totalCurrent=" << tmpCurrent << "\n";
}
- cout << "min_voltage=" << min_voltage << endl;
- #endif
+ ROS_DEBUG("PowerMonitor::min_voltage=%f", min_voltage);
+ ROS_DEBUG(" totalPower=%f", totalPower);
- powerState.power_consumption = 0;
- powerState.time_remaining = 60;
+ powerState.power_consumption = totalPower;
+ powerState.time_remaining = minTime;
powerState.prediction_method = "fuel guage";
- powerState.AC_present = 1;
+ powerState.AC_present = acCount;
pub.publish(powerState);
};
+
PowerMonitor()
{
//handle = new ros::NodeHandle();
ros::NodeHandle handle;
+ double freq = 0.1;
+ handle.getParam("/power_monitor/frequency", freq, 0.1);
+
pub = handle.advertise<pr2_msgs::PowerState>("power_state", 5);
sub = handle.subscribe("battery/server", 10, &PowerMonitor::batteryUpdate, this);
+
+ timer = handle.createTimer(ros::Duration(1/freq), &PowerMonitor::send, this);
};
@@ -61,6 +102,9 @@
pr2_msgs::PowerState powerState;
ros::Publisher pub;
ros::Subscriber sub;
+ ros::Timer timer;
+ boost::mutex vLock;
+ std::map< int, boost::shared_ptr<pr2_msgs::BatteryServer const> > batteryServers;
};
int main(int argc, char** argv)
@@ -68,17 +112,8 @@
ros::init(argc, argv, "power_monitor");
PowerMonitor monitor;
- ros::Rate rate(1);
- ros::NodeHandle handle;
- while(handle.ok())
- {
- rate.sleep();
- ros::spinOnce();
+ ros::spin();
-
- monitor.send();
- }
-
-
+ return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
