phidget_sensors_node.cpp
Go to the documentation of this file.
00001 
00060 #include <ros/ros.h>
00061 #include <cob_phidgets/phidget_manager.h>
00062 #include <cob_phidgets/phidgetik_ros.h>
00063 
00064 int main(int argc, char **argv)
00065 {
00066         //init ros
00067         ros::init(argc, argv, "cob_phidgets");
00068 
00069         int freq;
00070         std::string update_mode;
00071         std::vector<std::shared_ptr<Phidget>> phidgets;
00072         ros::NodeHandle nodeHandle;
00073         ros::NodeHandle nh("~");
00074         std::map<int, std::pair<std::string, XmlRpc::XmlRpcValue> > phidget_params_map;
00075         std::map<int, std::pair<std::string, XmlRpc::XmlRpcValue> >::iterator phidget_params_map_itr;
00076 
00077         //get default params from server
00078         nh.param<int>("frequency",freq, 30);
00079         nh.param<std::string>("update_mode", update_mode, "polling");
00080 
00081         //get board params from server
00082         XmlRpc::XmlRpcValue phidget_params;
00083         if(nh.getParam("boards",phidget_params))
00084         {
00085                 for(auto& board : phidget_params)
00086                 {
00087                         std::string board_name = board.first;
00088                         ROS_DEBUG("Found Board with name '%s' in params", board_name.c_str());
00089                         if(!board.second.hasMember("serial_num"))
00090                         {
00091                                 ROS_ERROR("Board '%s' has no serial_num param in phidget yaml config",board_name.c_str());
00092                                 continue;
00093                         }
00094                         XmlRpc::XmlRpcValue board_desc = board.second;
00095                         ros::NodeHandle boards_nh(nh, "boards");
00096                         ros::NodeHandle sensors_nh(boards_nh, board_name);
00097                         XmlRpc::XmlRpcValue board_param;
00098                         if(!sensors_nh.getParam("sensors", board_param))
00099                         {
00100                                 ROS_ERROR("Board '%s' has no sensors param in phidget yaml config",board_name.c_str());
00101                                 continue;
00102                         }
00103                         phidget_params_map.insert(std::make_pair(board_desc["serial_num"],
00104                                         std::make_pair(board_name, board_param)));
00105                 }
00106         }
00107         else
00108         {
00109                 ROS_WARN("No params for the phidget boards on the Paramserver using default name/port values");
00110         }
00111 
00112         //look for attached devices
00113         PhidgetManager* manager = new PhidgetManager();
00114         auto devices = manager->getAttachedDevices();
00115         delete manager;
00116 
00117         //set the update method
00118         PhidgetIK::SensingMode sensMode;
00119         if(update_mode == "event")
00120                 sensMode = PhidgetIK::SensingMode::EVENT;
00121         else if(update_mode == "polling")
00122                 sensMode = PhidgetIK::SensingMode::POLLING;
00123         else
00124         {
00125                 ROS_WARN("Unknown update mode '%s' use polling instead", update_mode.c_str());
00126                 sensMode = PhidgetIK::SensingMode::POLLING;
00127         }
00128 
00129         //if no devices attached exit
00130         if(devices.size() > 0)
00131         {
00132                 //loop over all found devices and init them
00133                 for(auto& device : devices)
00134                 {
00135                         phidget_params_map_itr = phidget_params_map.find(device.serial_num);
00136                         XmlRpc::XmlRpcValue *sensors_param = (phidget_params_map_itr != phidget_params_map.end()) ? &((*phidget_params_map_itr).second.second) : nullptr;
00137                         std::string name;
00138                         if(sensors_param != nullptr)
00139                                 name = (*phidget_params_map_itr).second.first;
00140                         else
00141                         {
00142                                 ROS_WARN("Could not find parameters for Board with serial: %d. Using default params!", device.serial_num);
00143                                 std::stringstream ss; ss << device.serial_num;
00144                                 name = ss.str();
00145                         }
00146                         phidgets.push_back(
00147                                 std::make_shared<PhidgetIKROS>(nodeHandle, device.serial_num, name, sensors_param, sensMode));
00148                 }
00149 
00150                 ros::Rate loop_rate(freq);
00151                 while (ros::ok())
00152                 {
00153                         //update devices if update method is polling
00154                         if(sensMode == PhidgetIK::SensingMode::POLLING)
00155                         {
00156                                 for(auto& phidget : phidgets)
00157                                 {
00158                                         phidget->update();
00159                                 }
00160                         }
00161                         ros::spinOnce();
00162                         loop_rate.sleep();
00163                 }
00164         }
00165         else
00166         {
00167                 ROS_ERROR("Phidget Manager could not find any attached devices");
00168         }
00169 
00170         return 0;
00171 }


cob_phidgets
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 12:46:06