commit 6168b08c90d52055a5cd50486092fda36145b54b
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Mon Jan 18 09:24:21 2016 +0100

    added controler state subscriber in order to en/disable the jointstate output

diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
index 5be7725..eec74ba 100644
--- a/examples/meka/src/mekajointinterface.cpp
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -4,6 +4,28 @@ using namespace std;
 //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
 #define POSITION_CONTROL 1
 
+void MekaJointInterface::incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state){
+    //incoming controller status
+    for (unsigned int i = 0; i<control_state.group_name.size(); i++){
+        if (control_state.group_name[i] == "head"){
+            //enable/disable based on controller status
+            if (control_state.state[i] == m3meka_msgs::M3ControlStates::START){
+                //controller up and running
+                if (!controller_enabled){
+                    printf("> incoming control state (%d), enabling jointstate output\n",control_state.state[i]);
+                    controller_enabled = true;
+                }
+            }else{
+                //controller in estop/stopped etc
+                if (controller_enabled){
+                    printf("> incoming control state (%d), DISABLING jointstate output\n",control_state.state[i]);
+                    controller_enabled = false;
+                }
+            }
+        }
+    }
+}
+
 void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
     //fetch current timestamp
     double timestamp = msg.header.stamp.toSec();
@@ -75,17 +97,22 @@ void MekaJointInterface::store_dummy_data(int id, double timestamp){
 }
 
 //! constructor
-MekaJointInterface::MekaJointInterface(string _input_scope, string _output_scope) : humotion::server::JointInterface(){
+MekaJointInterface::MekaJointInterface(string _input_scope, string control_scope, string _output_scope) : humotion::server::JointInterface(){
     input_scope = _input_scope;
     output_scope = _output_scope;
 
+    controller_enabled = false;
+
     //subscribe to meka joint states
     int argc = 0;
     ros::init(argc, (char**)NULL, "meka_humotion");
     ros::NodeHandle n;
 
     printf("> listening on jointstates on '%s'\n",input_scope.c_str());
-    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates , this);
+    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates, this);
+
+    printf("> listening on controlstates on '%s'\n",control_scope.c_str());
+    control_state_subscriber = n.subscribe(control_scope, 150, &MekaJointInterface::incoming_controlstate, this);
 
     printf("> sending targets on '%s'\n", output_scope.c_str());
     target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(output_scope, 100);
@@ -121,71 +148,24 @@ void MekaJointInterface::publish_target_position(int e){
 
 //! actually execute the scheduled motion commands
 void MekaJointInterface::execute_motion(){
-    //build msg
-    trajectory_msgs::JointTrajectory msg;
-    msg.joint_names.push_back("head_j0");
-    msg.joint_names.push_back("head_j1");
-
-    trajectory_msgs::JointTrajectoryPoint p;
-    p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);
-    //pan joint is inverted!
-    p.positions.push_back(-joint_target[ID_NECK_PAN] * M_PI / 180.0);
-    //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);
-
-    p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
-
-    msg.points.push_back(p);
-
-    target_publisher.publish(msg);
-
-    /*
-     void MekaJointInterface::store_min_max(int id, float min, float max){
-header:
-  seq: 636
-  stamp:
-    secs: 0
-    nsecs: 0
-  frame_id: ''
-joint_names: ['head_j0', 'head_j1']
-points:
-  -
-    positions: [-0.31, 0.01954768762234005]
-    velocities: []
-    accelerations: []
-    effort: []
-    time_from_start:
-      secs: 1
-      nsecs: 0
-
-     */
-
-    #if 0
-    // set up neck and eye motion commands:
-    if (POSITION_CONTROL){
-        //position control
-        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
-            set_target_in_positionmode(i, target_angle[i]);
-        }
-    }else{
-        //velocity control
-        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
-            set_target_in_velocitymode(i, target_angle[i]);
-        }
-    }
-    //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
-
+    if (controller_enabled){
+        //build msg
+        trajectory_msgs::JointTrajectory msg;
+        msg.joint_names.push_back("head_j0");
+        msg.joint_names.push_back("head_j1");
 
-    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
-    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
+        trajectory_msgs::JointTrajectoryPoint p;
+        p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);
+        //pan joint is inverted!
+        p.positions.push_back(-joint_target[ID_NECK_PAN] * M_PI / 180.0);
+        //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);
 
-    //eyebrows are set using a special command as well:
-    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
-    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
+        p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
 
-    //mouth
-    set_mouth();
+        msg.points.push_back(p);
 
-    #endif
+        target_publisher.publish(msg);
+    }
 }
 
 
