[Personalrobots-commit] SF.net SVN: personalrobots:[22618] pkg/trunk/stacks

[<rob...@us...>]

Revision: 22618
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22618&view=rev
Author:   rob_wheeler
Date:     2009-08-21 23:20:41 +0000 (Fri, 21 Aug 2009)

Log Message:
-----------
Publish power board state on a separate topic

Modified Paths:
--------------
    pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/include/power_node.h
    pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/manifest.xml
    pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/src/power_node/power_node.cpp

Added Paths:
-----------
    pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerBoardState.msg

Added: pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerBoardState.msg
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerBoardState.msg	                        (rev 0)
+++ pkg/trunk/stacks/pr2_common/pr2_msgs/msg/PowerBoardState.msg	2009-08-21 23:20:41 UTC (rev 22618)
@@ -0,0 +1,8 @@
+Header header
+string name
+# Circuit States:
+#  STATE_NOPOWER, STATE_STANDBY, STATE_PUMPING, STATE_ON, STATE_DISABLED
+int8[3] circuit_state
+float64[3] circuit_voltage
+int8 run_stop
+int8 wireless_stop

Modified: pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/include/power_node.h
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/include/power_node.h	2009-08-21 23:19:11 UTC (rev 22617)
+++ pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/include/power_node.h	2009-08-21 23:20:41 UTC (rev 22618)
@@ -60,7 +60,7 @@
 
     void init();
     void collectMessages();
-    void sendDiagnostic();
+    void sendMessages();
     int collect_messages();
     int process_message(const PowerMessage *msg, int len);
     int process_transition_message(const TransitionMessage *msg, int len);
@@ -72,7 +72,8 @@
   private:
     ros::NodeHandle node_handle;
     ros::ServiceServer service;
-    ros::Publisher pub;
+    ros::Publisher diags_pub;
+    ros::Publisher state_pub;
 
     pr2_power_board::PowerBoardCommand::Request req_;
     pr2_power_board::PowerBoardCommand::Response res_;

Modified: pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/manifest.xml	2009-08-21 23:19:11 UTC (rev 22617)
+++ pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/manifest.xml	2009-08-21 23:20:41 UTC (rev 22618)
@@ -7,6 +7,7 @@
   <depend package="rospy" />
   <depend package="diagnostic_updater" />
   <depend package="diagnostic_msgs" />
+  <depend package="pr2_msgs" />
   <export> 
     <cpp cflags="-I${prefix}/srv/cpp" />   
   </export> 

Modified: pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/src/power_node/power_node.cpp
===================================================================
--- pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/src/power_node/power_node.cpp	2009-08-21 23:19:11 UTC (rev 22617)
+++ pkg/trunk/stacks/pr2_power_drivers/pr2_power_board/src/power_node/power_node.cpp	2009-08-21 23:20:41 UTC (rev 22618)
@@ -56,6 +56,7 @@
 #include "power_node.h"
 #include "diagnostic_msgs/DiagnosticArray.h"
 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
+#include "pr2_msgs/PowerBoardState.h"
 #include "rosconsole/macros_generated.h"
 #include "ros/ros.h"
 
@@ -641,7 +642,8 @@
 
   service = node_handle.advertiseService("power_board_control", &PowerBoard::commandCallback, this);
 
-  pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
+  diags_pub = node_handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
+  state_pub = node_handle.advertise<pr2_msgs::PowerBoardState>("/power_board_state", 2);
 }
 
 bool PowerBoard::commandCallback(pr2_power_board::PowerBoardCommand::Request &req_,
@@ -661,7 +663,7 @@
   }
 }
 
-void PowerBoard::sendDiagnostic()
+void PowerBoard::sendMessages()
 {
   ros::Rate r(1);
   while(node_handle.ok())
@@ -837,9 +839,20 @@
       msg_out.status.push_back(stat);
 
       //ROS_DEBUG("Publishing ");
-      pub.publish(msg_out);
+      diags_pub.publish(msg_out);
+
+      pr2_msgs::PowerBoardState state_msg;
+      state_msg.name = stat.name;
+      state_msg.circuit_voltage[0] = status->CB0_voltage;
+      state_msg.circuit_voltage[1] = status->CB1_voltage;
+      state_msg.circuit_voltage[2] = status->CB2_voltage;
+      state_msg.circuit_state[0] = status->CB0_state;
+      state_msg.circuit_state[1] = status->CB1_state;
+      state_msg.circuit_state[2] = status->CB2_state;
+      state_msg.run_stop = status->estop_status;
+      state_msg.wireless_stop = status->estop_button_status;
+      state_pub.publish(state_msg);
     }
-
   }
 }
 
@@ -850,7 +863,7 @@
 
 void sendMessages()
 {
-  myBoard->sendDiagnostic();
+  myBoard->sendMessages();
 }
 
 // CloseAll


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.