Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16629] pkg/trunk
|
From: <wg_...@us...> - 2009-06-03 02:22:30
|
Revision: 16629
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16629&view=rev
Author: wg_cmeyers
Date: 2009-06-03 02:01:30 +0000 (Wed, 03 Jun 2009)
Log Message:
-----------
Added command line option to power_node "--serial" to filter on a specific
serial number. The default is to listen and repond to all serial numbers.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node_simulator.cpp
pkg/trunk/pr2/qualification/scripts/power_board_cmd.py
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h 2009-06-03 02:00:11 UTC (rev 16628)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h 2009-06-03 02:01:30 UTC (rev 16629)
@@ -41,7 +41,7 @@
}
void setPowerMessage(const PowerMessage &newpmsg);
- Device() { pmsgset = false; tmsgset = false; };
+ Device(): message_time(0,0) { pmsgset = false; tmsgset = false; };
~Device() { };
private:
bool tmsgset;
@@ -54,7 +54,7 @@
class PowerBoard : public ros::Node
{
public:
- PowerBoard();
+ PowerBoard( unsigned int serial_number = 0 );
bool commandCallback( pr2_power_board::PowerBoardCommand::Request &req_,
pr2_power_board::PowerBoardCommand::Response &res_);
@@ -73,4 +73,5 @@
pr2_power_board::PowerBoardCommand::Response res_;
boost::mutex library_lock_;
ros::Time last_diagnostic_time;
+ unsigned int serial_number;
};
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/CMakeLists.txt 2009-06-03 02:00:11 UTC (rev 16628)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/CMakeLists.txt 2009-06-03 02:01:30 UTC (rev 16629)
@@ -2,4 +2,5 @@
rospack_add_executable(power_node_simulator power_node_simulator.cpp)
rospack_add_boost_directories()
-rospack_link_boost(power_node thread)
+rospack_link_boost(power_node thread program_options)
+
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp 2009-06-03 02:00:11 UTC (rev 16628)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp 2009-06-03 02:01:30 UTC (rev 16629)
@@ -41,6 +41,7 @@
#include <sstream>
#include <iostream>
#include <boost/thread/thread.hpp>
+#include <boost/program_options.hpp>
// Internet/Socket stuff
#include <sys/types.h>
@@ -58,6 +59,7 @@
#include "ros/console.h"
using namespace std;
+namespace po = boost::program_options;
// Keep a pointer to the last message recieved for
// Each board.
@@ -416,20 +418,33 @@
}
// Look for device serial number in list of devices...
- for (unsigned i = 0; i<Devices.size(); ++i) {
- if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
+ if(serial_number != 0) // when a specific serial is called out, ignore everything else
+ {
+ if(serial_number == msg->header.serial_num) //this should be our number
+ {
boost::mutex::scoped_lock(library_lock_);
- Devices[i]->message_time = ros::Time::now();
- Devices[i]->setPowerMessage(*msg);
- return 0;
+ Devices[0]->message_time = ros::Time::now();
+ Devices[0]->setPowerMessage(*msg);
}
}
+ else
+ {
+ for (unsigned i = 0; i<Devices.size(); ++i) {
+ if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
+ boost::mutex::scoped_lock(library_lock_);
+ Devices[i]->message_time = ros::Time::now();
+ Devices[i]->setPowerMessage(*msg);
+ return 0;
+ }
+ }
- // Add new device to list
- Device *newDevice = new Device();
- Devices.push_back(newDevice);
- newDevice->message_time = ros::Time::now();
- newDevice->setPowerMessage(*msg);
+ // Add new device to list
+ Device *newDevice = new Device();
+ Devices.push_back(newDevice);
+ newDevice->message_time = ros::Time::now();
+ newDevice->setPowerMessage(*msg);
+ }
+
return 0;
}
@@ -441,18 +456,31 @@
}
// Look for device serial number in list of devices...
- for (unsigned i = 0; i<Devices.size(); ++i) {
- if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
+ if(serial_number != 0) // when a specific serial is called out, ignore everything else
+ {
+ if(serial_number == msg->header.serial_num) //this should be our number
+ {
boost::mutex::scoped_lock(library_lock_);
- Devices[i]->setTransitionMessage(*msg);
- return 0;
+ Devices[0]->message_time = ros::Time::now();
+ Devices[0]->setTransitionMessage(*msg);
}
}
+ else
+ {
+ for (unsigned i = 0; i<Devices.size(); ++i) {
+ if (Devices[i]->getPowerMessage().header.serial_num == msg->header.serial_num) {
+ boost::mutex::scoped_lock(library_lock_);
+ Devices[i]->setTransitionMessage(*msg);
+ return 0;
+ }
+ }
- // Add new device to list
- Device *newDevice = new Device();
- Devices.push_back(newDevice);
- newDevice->setTransitionMessage(*msg);
+ // Add new device to list
+ Device *newDevice = new Device();
+ Devices.push_back(newDevice);
+ newDevice->setTransitionMessage(*msg);
+ }
+
return 0;
}
@@ -564,7 +592,7 @@
return 0;
}
-PowerBoard::PowerBoard(): ros::Node ("pr2_power_board")
+PowerBoard::PowerBoard( unsigned int serial_number ): ros::Node ("pr2_power_board")
{
ROSCONSOLE_AUTOINIT;
@@ -576,6 +604,14 @@
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
}
+ this->serial_number = serial_number;
+ if(serial_number != 0)
+ {
+ ROS_INFO("PowerBoard: created with serial number = %d", serial_number);
+ Device *newDevice = new Device();
+ Devices.push_back(newDevice);
+ }
+
advertiseService("power_board_control", &PowerBoard::commandCallback);
advertise<robot_msgs::DiagnosticMessage>("/diagnostics", 2);
}
@@ -963,11 +999,28 @@
int main(int argc, char** argv)
{
+ unsigned int serial_option;
+ po::options_description desc("Allowed options");
+ desc.add_options()
+ ("help", "this help message")
+ ("serial", po::value<unsigned int>(&serial_option)->default_value(0), "filter a specific serial number");
+
+ po::variables_map vm;
+ po::store(po::parse_command_line( argc, argv, desc), vm);
+ po::notify(vm);
+
+ if( vm.count("help"))
+ {
+ cout << desc << "\n";
+ return 1;
+ }
+
ros::init(argc, argv);
CreateAllInterfaces();
- myBoard = new PowerBoard();
+ myBoard = new PowerBoard(serial_option);
+
boost::thread getThread( &getMessages );
boost::thread sendThread( &sendMessages );
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node_simulator.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node_simulator.cpp 2009-06-03 02:00:11 UTC (rev 16628)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node_simulator.cpp 2009-06-03 02:01:30 UTC (rev 16629)
@@ -305,7 +305,7 @@
return 0;
}
-PowerBoard::PowerBoard(): ros::Node ("pr2_power_board")
+PowerBoard::PowerBoard( unsigned int serial_number ): ros::Node ("pr2_power_board")
{
ROSCONSOLE_AUTOINIT;
Modified: pkg/trunk/pr2/qualification/scripts/power_board_cmd.py
===================================================================
--- pkg/trunk/pr2/qualification/scripts/power_board_cmd.py 2009-06-03 02:00:11 UTC (rev 16628)
+++ pkg/trunk/pr2/qualification/scripts/power_board_cmd.py 2009-06-03 02:01:30 UTC (rev 16629)
@@ -84,7 +84,7 @@
is_first = False
for j in range(0, 3):
# Call power command service
- resp = control_proxy(j, power_cmd, 0)
+ resp = control_proxy(0, j, power_cmd, 0) # serial number of zero not valid, power_node must be started with specific serial number
rospy.logout('CMD: %s %s. Received return code %d' %
(j, power_cmd, resp.retval))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
