valves.cpp

Go to the documentation of this file.

#include "sr_hand/hand/valves.h"
//our robot code

#include <sstream>
//messages


namespace shadowrobot
{
  Valves::Valves() :
    n_tilde("~"), publish_rate(0.0)
  {

    /* 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();
    }

    /* We need to attach the program to the hand as well, or fail if we cannot. */
    if (hand_init() < 0)
    {
      ROS_FATAL("Arm interface broken\n");
      ROS_BREAK();
    }

    // set publish frequency
    double publish_freq;
    n_tilde.param("publish_frequency", publish_freq, 10.0);
    publish_rate = ros::Rate(publish_freq);

#ifdef HAND_MUSCLE
    for (unsigned int i = 0; i < START_OF_ARM; ++i)
    {
      init_subs_and_pubs(i);
    }
#endif
#ifdef ARM
    for (unsigned int i = START_OF_ARM; i < NUM_HAND_JOINTS; ++i)
    {
      init_subs_and_pubs(i);
    }
#endif
  }

  Valves::~Valves()
  {
  }

  void Valves::init_subs_and_pubs(int index_joint)
  {
    std::vector<std::string> subname(2);
    subname[0] = "Flex";
    subname[1] = "Ext";

    int subname_index = 0;

    std::string name = hand_joints[index_joint].joint_name;
    for (unsigned int j = 0; j < hand_joints[index_joint].num_actuators; ++j)
    {
      subname_index = (hand_joints[index_joint].num_actuators == 1) ? 1 : j;

      //Pressure
      struct sensor s = hand_joints[index_joint].a.muscle.pressure[j];
      valves_sensors.push_back(s);
      std::stringstream ss_valve_name;
      ss_valve_name << hand_joints[index_joint].joint_name << "_"
                    << subname[subname_index] << "_Pressure";
      std::string valve_name = ss_valve_name.str();
      ROS_DEBUG("%s", valve_name.c_str());
      std::string topic = valve_name + "/status";
      valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
      topic = valve_name + "/cmd";
      valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1)));

      //Pressure_Target
      ss_valve_name.str(std::string());
      s = hand_joints[index_joint].a.muscle.pressure_target[j];
      valves_sensors.push_back(s);
      ss_valve_name << hand_joints[index_joint].joint_name << "_"
                    << subname[subname_index] << "_Target";
      valve_name = ss_valve_name.str();
      ROS_DEBUG("%s", valve_name.c_str());
      topic = valve_name + "/status";
      valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
      topic = valve_name + "/cmd";
      valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size() - 1 )));

      //Muscles
      ss_valve_name.str(std::string());
      s = hand_joints[index_joint].a.muscle.muscles[j];
      valves_sensors.push_back(s);
      ss_valve_name << hand_joints[index_joint].joint_name << "_"
                    << subname[subname_index];
      valve_name = ss_valve_name.str();
      ROS_DEBUG("%s", valve_name.c_str());
      topic = valve_name + "/status";
      valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
      topic = valve_name + "/cmd";
      valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1 )));

      //Fill
      ss_valve_name.str(std::string());
      s = hand_joints[index_joint].a.muscle.valves[j][FILL_VALVE];
      valves_sensors.push_back(s);
      ss_valve_name << hand_joints[index_joint].joint_name << "_"
                    << subname[subname_index] << "_Fill";
      valve_name = ss_valve_name.str();
      ROS_DEBUG("%s", valve_name.c_str());
      topic = valve_name + "/status";
      valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
      topic = valve_name + "/cmd";
      valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1)));

      //Empty
      ss_valve_name.str(std::string());
      s = hand_joints[index_joint].a.muscle.valves[j][EMPTY_VALVE];
      valves_sensors.push_back(s);
      ss_valve_name << hand_joints[index_joint].joint_name << "_"
                    << subname[subname_index] << "_Empty";
      valve_name = ss_valve_name.str();
      ROS_DEBUG("%s", valve_name.c_str());
      topic = valve_name + "/status";
      valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
      topic = valve_name + "/cmd";
      valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size() -1)));
    }

  }


  void Valves::valve_command(const std_msgs::Float64ConstPtr& msg, int index_valve)
  {
    //@fixme: do some clipping on the value first?
    robot_update_sensor(&(valves_sensors[index_valve]), msg->data);
  }

  void Valves::publish()
  {
    std_msgs::Float64 msg_valves;

    for( unsigned int i = 0; i < valves_sensors.size(); ++i )
    {
      msg_valves.data = robot_read_sensor(&(valves_sensors[i]));

      valves_publishers[i].publish(msg_valves);
    }

    ros::spinOnce();
    publish_rate.sleep();

  }
} //end namespace


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