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>
#include <string>

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;
}
}  // namespace ronex

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