/examples/meka/src/mekajointinterface.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 6168b08c examples/meka/src/mekajointinterface.cpp

     //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();
-...
+    }
     //! 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);
-...
     //! 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);
+        }
+    }

Also available in: Unified diff