valves.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_hand/hand/valves.h"
00028 //our robot code
00029 
00030 #include <sstream>
00031 //messages
00032 
00033 
00034 namespace shadowrobot
00035 {
00036   Valves::Valves() :
00037     n_tilde("~"), publish_rate(0.0)
00038   {
00039 
00040     /* We need to attach the program to the robot, or fail if we cannot. */
00041     if (robot_init() < 0)
00042     {
00043       ROS_FATAL("Robot interface broken\n");
00044       ROS_BREAK();
00045     }
00046 
00047     /* We need to attach the program to the hand as well, or fail if we cannot. */
00048     if (hand_init() < 0)
00049     {
00050       ROS_FATAL("Arm interface broken\n");
00051       ROS_BREAK();
00052     }
00053 
00054     // set publish frequency
00055     double publish_freq;
00056     n_tilde.param("publish_frequency", publish_freq, 10.0);
00057     publish_rate = ros::Rate(publish_freq);
00058 
00059 #ifdef HAND_MUSCLE
00060     for (unsigned int i = 0; i < START_OF_ARM; ++i)
00061     {
00062       init_subs_and_pubs(i);
00063     }
00064 #endif
00065 #ifdef ARM
00066     for (unsigned int i = START_OF_ARM; i < NUM_HAND_JOINTS; ++i)
00067     {
00068       init_subs_and_pubs(i);
00069     }
00070 #endif
00071   }
00072 
00073   Valves::~Valves()
00074   {
00075   }
00076 
00077   void Valves::init_subs_and_pubs(int index_joint)
00078   {
00079     std::vector<std::string> subname(2);
00080     subname[0] = "Flex";
00081     subname[1] = "Ext";
00082 
00083     int subname_index = 0;
00084 
00085     std::string name = hand_joints[index_joint].joint_name;
00086     for (unsigned int j = 0; j < hand_joints[index_joint].num_actuators; ++j)
00087     {
00089       subname_index = (hand_joints[index_joint].num_actuators == 1) ? 1 : j;
00090 
00091       //Pressure
00092       struct sensor s = hand_joints[index_joint].a.muscle.pressure[j];
00093       valves_sensors.push_back(s);
00094       std::stringstream ss_valve_name;
00095       ss_valve_name << hand_joints[index_joint].joint_name << "_"
00096                     << subname[subname_index] << "_Pressure";
00097       std::string valve_name = ss_valve_name.str();
00098       ROS_DEBUG("%s", valve_name.c_str());
00099       std::string topic = valve_name + "/status";
00100       valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
00101       topic = valve_name + "/cmd";
00102       valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1)));
00103 
00104       //Pressure_Target
00105       ss_valve_name.str(std::string());
00106       s = hand_joints[index_joint].a.muscle.pressure_target[j];
00107       valves_sensors.push_back(s);
00108       ss_valve_name << hand_joints[index_joint].joint_name << "_"
00109                     << subname[subname_index] << "_Target";
00110       valve_name = ss_valve_name.str();
00111       ROS_DEBUG("%s", valve_name.c_str());
00112       topic = valve_name + "/status";
00113       valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
00114       topic = valve_name + "/cmd";
00115       valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size() - 1 )));
00116 
00117       //Muscles
00118       ss_valve_name.str(std::string());
00119       s = hand_joints[index_joint].a.muscle.muscles[j];
00120       valves_sensors.push_back(s);
00121       ss_valve_name << hand_joints[index_joint].joint_name << "_"
00122                     << subname[subname_index];
00123       valve_name = ss_valve_name.str();
00124       ROS_DEBUG("%s", valve_name.c_str());
00125       topic = valve_name + "/status";
00126       valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
00127       topic = valve_name + "/cmd";
00128       valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1 )));
00129 
00130       //Fill
00131       ss_valve_name.str(std::string());
00132       s = hand_joints[index_joint].a.muscle.valves[j][FILL_VALVE];
00133       valves_sensors.push_back(s);
00134       ss_valve_name << hand_joints[index_joint].joint_name << "_"
00135                     << subname[subname_index] << "_Fill";
00136       valve_name = ss_valve_name.str();
00137       ROS_DEBUG("%s", valve_name.c_str());
00138       topic = valve_name + "/status";
00139       valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
00140       topic = valve_name + "/cmd";
00141       valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size()-1)));
00142 
00143       //Empty
00144       ss_valve_name.str(std::string());
00145       s = hand_joints[index_joint].a.muscle.valves[j][EMPTY_VALVE];
00146       valves_sensors.push_back(s);
00147       ss_valve_name << hand_joints[index_joint].joint_name << "_"
00148                     << subname[subname_index] << "_Empty";
00149       valve_name = ss_valve_name.str();
00150       ROS_DEBUG("%s", valve_name.c_str());
00151       topic = valve_name + "/status";
00152       valves_publishers.push_back(n_tilde.advertise<std_msgs::Float64> (topic, 2));
00153       topic = valve_name + "/cmd";
00154       valves_subscribers.push_back(n_tilde.subscribe<std_msgs::Float64>(topic, 10, boost::bind(&Valves::valve_command, this, _1, valves_sensors.size() -1)));
00155     }
00156 
00157   }
00158 
00159 
00172   void Valves::valve_command(const std_msgs::Float64ConstPtr& msg, int index_valve)
00173   {
00174     //@fixme: do some clipping on the value first?
00175     robot_update_sensor(&(valves_sensors[index_valve]), msg->data);
00176   }
00177 
00182   void Valves::publish()
00183   {
00184     std_msgs::Float64 msg_valves;
00185 
00186     for( unsigned int i = 0; i < valves_sensors.size(); ++i )
00187     {
00188       msg_valves.data = robot_read_sensor(&(valves_sensors[i]));
00189 
00190       valves_publishers[i].publish(msg_valves);
00191     }
00192 
00193     ros::spinOnce();
00194     publish_rate.sleep();
00195 
00196   }
00197 } //end namespace
00198 
00199 
00200 /* For the emacs weenies in the crowd.
00201 Local Variables:
00202    c-basic-offset: 2
00203 End:
00204 */


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:03:25