/src/server/joint_interface.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 0c8d22a5 src/server/joint_interface.cpp

     * Excellence Initiative.
     */
     #include "server/joint_interface.h"
     #include "server/controller.h"
     using namespace std;
     using namespace humotion;
     using namespace humotion::server;
     using namespace boost;
     #include "humotion/server/joint_interface.h"
     #include "humotion/server/controller.h"
     using boost::mutex;
     using humotion::server::JointInterface;
     //! constructor
     JointInterface::JointInterface(){
     JointInterface::JointInterface() {
         framerate = 50.0;
         mouth_enabled = false;
         gaze_enabled  = false;
+    }
     //! destructor
     JointInterface::~JointInterface(){
     JointInterface::~JointInterface() {
+    }
     //! set joint target position
     //! \param joint_id of joint
     //! \param float position
     void JointInterface::set_target(int joint_id, float position, float velocity){
     void JointInterface::set_target(int joint_id, float position, float velocity) {
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         //update current value
         // update current value
         joint_target_position_[joint_id] = position;
         joint_target_velocity_[joint_id] = velocity;
+    }
     //! fetch target position
     //! \param joint_id of joint
     float JointInterface::get_target_position(int joint_id){
     float JointInterface::get_target_position(int joint_id) {
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         return joint_target_position_[joint_id];
+    }
     //! fetch target velocity
     //! \param joint_id of joint
     float JointInterface::get_target_velocity(int joint_id){
     float JointInterface::get_target_velocity(int joint_id) {
         assert(joint_id < JOINT_ID_ENUM_SIZE);
         return joint_target_velocity_[joint_id];
+    }
-...
     //! \param joint name
     //! \param position
     //! \param timestamp when the position was measured
     void JointInterface::store_incoming_position(int joint_id, float position, Timestamp timestamp){
         //lock the tsd_list for this access. by doing this we assure
         //that no other thread accessing this element can diturb the
         //following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex);
         //printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
     void JointInterface::store_incoming_position(int joint_id, float position, Timestamp timestamp) {
         // lock the tsd_list for this access. by doing this we assure
         // that no other thread accessing this element can diturb the
         // following atomic instructions:
         mutex::scoped_lock sl(joint_ts_position_map_access_mutex);
         // printf("> humotion: incoming joint position for joint id 0x%02X "
         // "= %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
         joint_ts_position_map[joint_id].insert(timestamp, position);
         incoming_position_count++;
-...
     //! return incoming position data count & clear this counter
     //! this can be used as a keep alive status check
     //! \return number of incoming joint positions since the last call
     unsigned int JointInterface::get_and_clear_incoming_position_count(){
     unsigned int JointInterface::get_and_clear_incoming_position_count() {
         unsigned int i = incoming_position_count;
         incoming_position_count = 0;
         return i;
-...
     //! \param joint name
     //! \param speed
     //! \param timestamp when the position was measured
     void JointInterface::store_incoming_velocity(int joint_id, float velocity, Timestamp timestamp){
         //lock the tsd_list for this access. by doing this we assure
         //that no other thread accessing this element can diturb the
         //following atomic instructions:
     void JointInterface::store_incoming_velocity(int joint_id, float velocity, Timestamp timestamp) {
         // lock the tsd_list for this access. by doing this we assure
         // that no other thread accessing this element can disturb the
         // following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex);
         //printf("> humotion: incoming joint velocity for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,velocity,timestamp.to_seconds());
         // printf("> humotion: incoming joint velocity for joint id 0x%02X = %4.2f "
         // "(ts=%.2f)\n",joint_id,velocity,timestamp.to_seconds());
         joint_ts_speed_map[joint_id].insert(timestamp, velocity);
+    }
     //! return the timestamped float for the given joints position
     TimestampedList JointInterface::get_ts_position(int joint_id){
         //lock the tsd_list for this access. by doing this we assure
         //that no other thread accessing this element can disturb the
         //following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex);
     humotion::TimestampedList JointInterface::get_ts_position(int joint_id) {
         // lock the tsd_list for this access. by doing this we assure
         // that no other thread accessing this element can disturb the
         // following atomic instructions
         mutex::scoped_lock sl(joint_ts_position_map_access_mutex);
         //search map:
         // search map
         joint_tsl_map_t::iterator it = joint_ts_position_map.find(joint_id);
         if (it == joint_ts_position_map.end()){
         if (it == joint_ts_position_map.end()) {
             printf("> humotion: no ts_position for joint id 0x%02X found\n", joint_id);
             return TimestampedList();
+        }
         //ok fine, we found our value:
         // ok fine, we found the requested value
         return it->second;
+    }
     //! return the timestamped float for the given joints speed
     TimestampedList JointInterface::get_ts_speed(int joint_id){
         //lock the tsd_list for this access. by doing this we assure
         //that no other thread accessing this element can diturb the
         //following atomic instructions:
         mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex);
     humotion::TimestampedList JointInterface::get_ts_speed(int joint_id) {
         // lock the tsd_list for this access. by doing this we assure
         // that no other thread accessing this element can diturb the
         // following atomic instructions
         mutex::scoped_lock sl(joint_ts_speed_map_access_mutex);
         //search map:
         // search map
         joint_tsl_map_t::iterator it = joint_ts_speed_map.find(joint_id);
         if (it == joint_ts_speed_map.end()){
         if (it == joint_ts_speed_map.end()) {
             printf("> humotion: no ts_speed for joint id 0x%02X found\n", joint_id);
             return TimestampedList();
             return humotion::TimestampedList();
+        }
         //ok fine, we found our value:
         // ok fine, we found our value
         return it->second;
+    }
     //! set framerate
     void JointInterface::set_framerate(float f){
     void JointInterface::set_framerate(float f) {
         framerate = f;
+    }
     //! enable all mouth joints
     void JointInterface::enable_mouth_joints(){
         //already enabled? skip this
         if (mouth_enabled){
     void JointInterface::enable_mouth_joints() {
         // already enabled? skip this
         if (mouth_enabled) {
             return;
+        }
-...
     //! disable all mouth joints
     void JointInterface::disable_mouth_joints(){
         //already disabled? skip this
         if (!mouth_enabled){
     void JointInterface::disable_mouth_joints() {
         // already disabled? skip this
         if (!mouth_enabled) {
             return;
+        }
-...
+    }
     //! enable all gaze joints
     void JointInterface::enable_gaze_joints(){
         //already enabled? skip this
         if (gaze_enabled){
     void JointInterface::enable_gaze_joints() {
         // already enabled? skip this
         if (gaze_enabled) {
             return;
+        }
-...
+    }
     //! disable all gaze joints
     void JointInterface::disable_gaze_joints(){
         //already disabled? skip this
         if (!gaze_enabled){
     void JointInterface::disable_gaze_joints() {
         // already disabled? skip this
         if (!gaze_enabled) {
             return;
+        }
-...
     //! fetch maximum allowed joint position
     //! \return max position
     float JointInterface::get_joint_max(int joint_id){
     float JointInterface::get_joint_max(int joint_id) {
         assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
         return joint_max[joint_id];
+    }
     //! fetch minimum allowed joint position
     //! \return min position
     float JointInterface::get_joint_min(int joint_id){
     float JointInterface::get_joint_min(int joint_id) {
         assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
         return joint_min[joint_id];
+    }
     //! check if joint position map is empty
     //! \return true if empty
     bool JointInterface::get_joint_position_map_empty(){
     bool JointInterface::get_joint_position_map_empty() {
         return (joint_ts_position_map.empty());
+    }

Also available in: Unified diff