sr_ronex_utilities.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2013, Shadow Robot Company, All rights reserved.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 3.0 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library.
 */

#pragma once

#include <sstream>
#include <bitset>
#include <ros/ros.h>
#include <boost/lexical_cast.hpp>

namespace ronex
{
  static inline bool check_bit(uint16_t data, size_t index)
  {
    // x8 because sizeof returns size in bytes not bits
    return std::bitset<sizeof(uint16_t)*8>(data).test(index);
  }

  static inline void set_bit(uint32_t &data, size_t index, bool value)
  {
    //x8 because sizeof returns size in bytes not bits
    std::bitset<sizeof(uint32_t)*8> tmp(data);
    tmp.set(index, value);
    data = static_cast<uint32_t>(tmp.to_ulong());
  }

  static inline void set_bit(uint16_t &data, size_t index, bool value)
  {
    //x8 because sizeof returns size in bytes not bits
    std::bitset<sizeof(uint16_t)*8> tmp(data);
    tmp.set(index, value);
    data = static_cast<uint16_t>(tmp.to_ulong());
  }

  static inline int get_ronex_param_id(std::string ronex_id)
  {
    std::string param;

    int ronex_parameter_id = 0;
    while( true )
    {
      std::stringstream ss;
      ss << "/ronex/devices/" << ronex_parameter_id << "/ronex_id";
      if(ros::param::get(ss.str(), param) )
      {
        if( ronex_id.compare("") != 0 )
        {
          if( ronex_id.compare(param) == 0)
          {
            return ronex_parameter_id;
          }
        }
        ++ronex_parameter_id;
      }
      else
      {
        if( ronex_id.compare("") != 0)
        {
          //we were looking for a specific ronex and didn't find it -> return -1
          return -1;
        }
        return ronex_parameter_id;
      }
    }

    return -1;
  }

   static inline std::string get_ronex_devices_string(int ronex_parameter_id, std::string part)
   {
     std::string key("/ronex/devices/");
     key += boost::lexical_cast<std::string>(ronex_parameter_id);
     key += "/";
     key += part;
     return key;
   }
}

/* For the emacs weenies in the crowd.
   Local Variables:
   c-basic-offset: 2
   End:
*/