sr_real_tactile_sensor.cpp

Go to the documentation of this file.

#include "sr_tactile_sensors/sr_real_tactile_sensor.hpp"

namespace shadowrobot
{
/**********************************
 *         TACTILE SENSOR         *
 **********************************/
  SrRealTactileSensor::SrRealTactileSensor(std::string name,
                                           std::string touch_name ) :
    SrGenericTactileSensor(name, touch_name)
  {
    /* We need to attach the program to the robot, or fail if we cannot. */
    if (robot_init() < 0)
    {
      ROS_FATAL("Robot interface broken\n");
      ROS_BREAK();
    }

    res_touch = robot_name_to_sensor(touch_name.c_str(), &sensor_touch);

    if(res_touch)
    {
      ROS_ERROR("Can't open sensor %s", touch_name.c_str());
    }
  }

  SrRealTactileSensor::~SrRealTactileSensor()
  {}

  double SrRealTactileSensor::get_touch_data()
  {
    if(res_touch)
      return -1000.0;

    return robot_read_sensor(&sensor_touch);
  }


/**********************************
 *     TACTILE SENSOR MANAGER     *
 **********************************/
  SrRealTactileSensorManager::SrRealTactileSensorManager() :
    SrTactileSensorManager()
  {
    std::vector<std::vector<std::string> > all_names = get_all_names();

    for( unsigned int i=0; i < all_names[0].size() ; ++i)
    {
      tactile_sensors.push_back(
        boost::shared_ptr<SrRealTactileSensor>(
          new SrRealTactileSensor(all_names[0][i],
                                  all_names[1][i]) ));
    }
  }

  SrRealTactileSensorManager::~SrRealTactileSensorManager()
  {}
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "sr_tactile_sensor");
  ros::NodeHandle n;

  shadowrobot::SrRealTactileSensorManager tact_sens_mgr;

  while( ros::ok() )
    tact_sens_mgr.publish_all();

  return 0;
}

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