Revision 2cf3c285

View differences:

client/cpp/CMakeLists.txt
125 125 
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/include)
126 126 
LINK_DIRECTORIES(${LIBDIR})
127 127 
IF (ROS_FOUND)
128 
add_dependencies(${HLRC_CLIENT_LIBNAME} ${catkin_EXPORTED_TARGETS} hlc_server_gencpp)
128 
    add_dependencies(${HLRC_CLIENT_LIBNAME} ${catkin_EXPORTED_TARGETS} hlrc_server_gencpp)
129 129 
ENDIF (ROS_FOUND)
130 130 

  		




  131
  131
  
    
target_link_libraries(${HLRC_CLIENT_LIBNAME} ${RSB_LIBRARIES} ${RST_LIBRARIES} ${catkin_LIBRARIES})


  		












  

    
      server/CMakeLists.txt
    
  





  55
  55
  
    
IF (catkin_FOUND)


  		




  56
  56
  
    
    SET(ROS_FOUND 1)


  		




  57
  57
  
    

  		




  58
  
  
    
    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs message_generation genmsg actionlib_msgs actionlib)


  		




  
  58
  
    
    find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs message_generation genmsg actionlib_msgs actionlib tf2_ros tf2_eigen angles)


  		




  
  59
  
    
    find_package(PkgConfig REQUIRED)


  		




  
  60
  
    
    pkg_search_module(EIGEN3 REQUIRED eigen3)


  		




  59
  61
  
    

  		




  60
  62
  
    
    message(STATUS "ROS Support is ON")


  		




  61
  63
  
    
    add_definitions(-DROS_SUPPORT=1)


  		




  ......




  111
  113
  
    
        message("> using humotion includes from " ${humotion_INCLUDE_DIRS})


  		




  112
  114
  
    
ELSE (humotion_FOUND)


  		




  113
  115
  
    
        message(FATAL_ERROR "> error: can not find libhumotion")


  		




  114
  
  
    

  		




  115
  116
  
    
endif (humotion_FOUND)


  		




  116
  117
  
    

  		




  117
  118
  
    
###################################


  		




  ......




  127
  128
  
    
ENDIF (AO_FOUND)


  		




  128
  129
  
    

  		




  129
  130
  
    
#build 


  		




  130
  
  
    
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${AO_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS})


  		




  
  131
  
    
include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${AO_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS})


  		




  131
  132
  
    
link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${AO_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})


  		




  132
  133
  
    

  		




  133
  134
  
    
#build dummy list of header files


  		












  

    
      server/include/MiddlewareROS.h
    
  





  31
  31
  
    
#ifdef ROS_SUPPORT


  		




  32
  32
  
    
    #include "ros/ros.h"


  		




  33
  33
  
    
    #include <actionlib/client/simple_action_client.h>


  		




  
  34
  
    
    #include <tf2_ros/transform_listener.h>


  		




  34
  35
  
    

  		




  35
  36
  
    
    //messages


  		




  36
  37
  
    
    #include "hlrc_server/phoneme.h"


  		




  ......




  81
  82
  
    
    //boost::shared_ptr<ros::NodeHandle> node_handle;


  		




  82
  83
  
    
    ros::NodeHandle *ros_node_handle;


  		




  83
  84
  
    
    bool tick_necessary;


  		




  84
  
  
    
    /*ros::ServiceServer animation_service;


  		




  85
  
  
    
    ros::ServiceServer utterance_service;


  		




  86
  
  
    
    ros::ServiceServer default_emotion_service;


  		




  87
  
  
    
    ros::ServiceServer current_emotion_service;


  		




  88
  
  
    
    ros::ServiceServer gaze_service;


  		




  89
  
  
    
    ros::ServiceServer tts_service;*/


  		




  
  85
  
    
    //listen to tf tree


  		




  
  86
  
    
    tf2_ros::Buffer tfBuffer;


  		




  
  87
  
    
    boost::shared_ptr<tf2_ros::TransformListener> tfListener;


  		




  
  88
  
    

  		




  
  89
  
    
    //FIXME: These pointers are never destroyed. Shouldn't they be shared_ptrs?


  		




  90
  90
  
    
    AnimationCallbackWrapper *animation_action_server;


  		




  91
  91
  
    
    UtteranceCallbackWrapper *utterance_action_server;


  		




  92
  92
  
    
    EmotionCallbackWrapper *current_emotion_action_server;


  		




  ......




  98
  98
  
    
    actionlib::SimpleActionClient<hlrc_server::ttsAction> *tts_ac;


  		




  99
  99
  
    
#endif


  		




  100
  100
  
    
};


  		




  101
  
  
    

  		




  102
  
  
    

  		




  103
  
  
    

  		




  104
  
  
    

  		












  

    
      server/include/ROS/LookatCallbackWrapperROS.h
    
  





  29
  29
  
    
#pragma once


  		




  30
  30
  
    
#include "hlrc_server/lookattargetAction.h"


  		




  31
  31
  
    
#include "CallbackWrapperROS.h"


  		




  
  32
  
    
#include <tf2_ros/buffer.h>


  		




  
  33
  
    
#include <tf2_eigen/tf2_eigen.h>


  		




  
  34
  
    
#include <angles/angles.h>


  		




  32
  35
  
    

  		




  33
  36
  
    
//callback handler incoming lookat requests:


  		




  34
  37
  
    
class LookatCallbackWrapper : CallbackWrapper<hlrc_server::lookattargetAction>{


  		




  35
  38
  
    
protected:


  		




  36
  39
  
    
    hlrc_server::lookattargetFeedback feedback;


  		




  37
  40
  
    
    hlrc_server::lookattargetResult result;


  		




  
  41
  
    
    const tf2_ros::Buffer& tfBuffer;


  		




  
  42
  
    
    const std::string FLOBI_BASE_LINK="BASE_LINK";


  		




  
  43
  
    
    Eigen::Affine3d eig_base_to_eyes; // base to center of eyes


  		




  
  44
  
    
    double eye_distance; // distance from center of eyes to eye frame


  		




  38
  45
  
    

  		




  39
  46
  
    
public:


  		




  40
  47
  
    

  		




  41
  
  
    
    LookatCallbackWrapper(Middleware *mw, std::string scope, std::string name)


  		




  
  48
  
    
    LookatCallbackWrapper(Middleware *mw, std::string scope, std::string name, const tf2_ros::Buffer &tfBuffer)


  		




  42
  49
  
    
       : CallbackWrapper<hlrc_server::lookattargetAction>(mw, scope, name, boost::bind(&LookatCallbackWrapper::call, this, _1))


  		




  
  50
  
    
       , tfBuffer(tfBuffer)


  		




  43
  51
  
    
    {


  		




  44
  
  
    
        //


  		




  
  52
  
    
        eye_distance = 0.15 / 2;


  		




  45
  53
  
    
    }


  		




  46
  54
  
    

  		




  47
  55
  
    
    void call(const GoalConstPtr &goal){


  		




  48
  56
  
    
        feedback.result = 1;


  		




  49
  57
  
    

  		




  50
  58
  
    
        hlrc_server::lookattargetGoalConstPtr request = goal;


  		




  
  59
  
    
        const geometry_msgs::Point &p = request->point;


  		




  
  60
  
    

  		




  
  61
  
    
        Eigen::Vector3d target(p.x, p.y, p.z);


  		




  
  62
  
    
        try {


  		




  
  63
  
    
            if (!request->header.frame_id.empty()) { // only consider tf, when frame_id is non-empty


  		




  
  64
  
    
                geometry_msgs::TransformStamped source_to_base =


  		




  
  65
  
    
                    // lookupTransform can throw


  		




  
  66
  
    
                    tfBuffer.lookupTransform(FLOBI_BASE_LINK, request->header.frame_id,


  		




  
  67
  
    
                                             request->header.stamp, ros::Duration(0.01));


  		




  
  68
  
    

  		




  
  69
  
    
                Eigen::Affine3d eig_source_to_eyes;


  		




  
  70
  
    
                eig_source_to_eyes = tf2::transformToEigen(source_to_base) * eig_base_to_eyes;


  		




  
  71
  
    
                target = eig_source_to_eyes * target;


  		




  
  72
  
    
            } else {


  		




  
  73
  
    
                // otherwise we interpret target directly w.r.t. eye center


  		




  
  74
  
    
            }


  		




  
  75
  
    
        } catch (const std::exception &e) {


  		




  
  76
  
    
            // Failed to resolve tf?


  		




  
  77
  
    
            result.result = 0;


  		




  
  78
  
    
            as_.setAborted(result, e.what());


  		




  
  79
  
    
            return;


  		




  
  80
  
    
        }


  		




  
  81
  
    

  		




  
  82
  
    
        // compute pan + tilt: let point z-axis of eyes frames towards target


  		




  51
  83
  
    
        humotion::GazeState gaze_state;


  		




  
  84
  
    
        double distance = target.norm();


  		




  
  85
  
    
        if (distance > 1e-3) {


  		




  
  86
  
    
            target /= distance;


  		




  52
  87
  
    

  		




  53
  
  
    
        // fill in lookat


  		




  54
  
  
    
        gaze_state.pan   = 20;


  		




  55
  
  
    
        gaze_state.tilt  = 30;


  		




  56
  
  
    
        gaze_state.roll  = 10;


  		




  
  88
  
    
            // pan = rotation about x-axis: angle of projection of target onto yz-plane to z-axis


  		




  
  89
  
    
            gaze_state.pan = 90-angles::to_degrees(atan2(target.z(), target.y()));


  		




  
  90
  
    
            // tilt = rotation about y-axis: angle between target and yz-plane


  		




  
  91
  
    
            Eigen::Vector2d projection(target.y(), target.z());


  		




  
  92
  
    
            gaze_state.tilt = angles::to_degrees(atan2(target.x(), projection.norm()));


  		




  
  93
  
    
        }


  		




  
  94
  
    
        // fill in rest of gaze_state


  		




  
  95
  
    
        gaze_state.roll  = request->roll;


  		




  
  96
  
    
        gaze_state.vergence  = angles::to_degrees(atan2(eye_distance, distance));


  		




  57
  97
  
    

  		




  58
  98
  
    
        // use timestamp from request


  		




  59
  99
  
    
        gaze_state.timestamp.set(request->header.stamp.sec, request->header.stamp.nsec);


  		




  60
  100
  
    
        gaze_state.gaze_type = humotion::GazeState::GAZETYPE_ABSOLUTE;


  		




  61
  101
  
    

  		




  62
  
  
    
        gaze_state.vergence  = 0;


  		




  63
  
  
    
        gaze_state.pan_offset = 0;


  		




  64
  
  
    
        gaze_state.tilt_offset = 0;


  		




  65
  
  
    

  		




  66
  102
  
    
        // processing


  		




  67
  
  
    
        feedback.result = 1;


  		




  68
  103
  
    
        as_.publishFeedback(feedback);


  		




  69
  104
  
    

  		




  70
  105
  
    
        // send to application:


  		












  

    
      server/package.xml
    
  





  36
  36
  
    
  <build_depend>geometry_msgs</build_depend>


  		




  37
  37
  
    
  <build_depend>actionlib_msgs</build_depend>


  		




  38
  38
  
    
  <build_depend>humotion</build_depend>


  		




  
  39
  
    
  <build_depend>tf2_ros</build_depend>


  		




  
  40
  
    
  <build_depend>tf2_eigen</build_depend>


  		




  
  41
  
    
  <build_depend>eigen</build_depend>


  		




  
  42
  
    
  <build_depend>angles</build_depend>


  		




  39
  43
  
    
  <!-- Use run_depend for packages you need at runtime: -->


  		




  40
  44
  
    
  <run_depend>std_msgs</run_depend>


  		




  41
  45
  
    
  <run_depend>geometry_msgs</run_depend>


  		




  42
  46
  
    
  <run_depend>actionlib_msgs</run_depend>


  		




  
  47
  
    
  <run_depend>tf2_ros</run_depend>


  		




  43
  48
  
    
  <!-- Use test_depend for packages you need only for testing: -->


  		




  44
  49
  
    
  <!--   <test_depend>gtest</test_depend> -->


  		




  45
  50
  
    

  		












  

    
      server/src/MiddlewareROS.cpp
    
  





  72
  72
  
    
    printf("> registering on ROS as node %s\n",node_name.c_str());


  		




  73
  73
  
    
    if (ros::isInitialized()){


  		




  74
  74
  
    
        //ros is already active, no need to take care of that


  		




  
  75
  
    
        //FIXME: Instead of deciding based on ros::isInitialized() to tick or not,


  		




  
  76
  
    
        //we shoud use our own MessageQueue and AsyncSpinner.


  		




  75
  77
  
    
        tick_necessary = false;


  		




  76
  78
  
    
    }else{


  		




  77
  79
  
    
        printf("> ROS not initialized, calling ros::init()\n");


  		




  ......




  86
  88
  
    
    ros::NodeHandle n;


  		




  87
  89
  
    

  		




  88
  90
  
    
    printf("> setting up ROS services...\n");


  		




  89
  
  
    
    //FIXME//init_callback("speech",    LocalServer::CallbackPtr(new SpeechCallbackWrapper(this)));


  		




  90
  
  
    

  		




  91
  91
  
    
    //ros::AsyncSpinner *spinner = new ros::AsyncSpinner(4); // Use 4 threads


  		




  92
  92
  
    
    //spinner->start();


  		




  93
  93
  
    

  		




  
  94
  
    
    tfListener.reset(new tf2_ros::TransformListener(tfBuffer));


  		




  
  95
  
    
    tfBuffer.clear();


  		




  
  96
  
    

  		




  94
  97
  
    
    animation_action_server = new AnimationCallbackWrapper(this, scope, "animation");


  		




  95
  98
  
    
    utterance_action_server = new UtteranceCallbackWrapper(this, scope, "utterance");


  		




  96
  99
  
    
    current_emotion_action_server = new EmotionCallbackWrapper(this, scope, "currentEmotion");


  		




  97
  100
  
    
    default_emotion_action_server = new EmotionCallbackWrapper(this, scope, "defaultEmotion");


  		




  98
  101
  
    
    gaze_action_server = new GazeCallbackWrapper(this, scope, "gaze");


  		




  99
  
  
    
    lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat");


  		




  
  102
  
    
    lookat_action_server = new LookatCallbackWrapper(this, scope, "lookat", tfBuffer);


  		




  100
  103
  
    
    mouth_action_server = new MouthCallbackWrapper(this, scope, "mouth");


  		




  101
  104
  
    
    speech_action_server = new SpeechCallbackWrapper(this, scope, "speech");


  		




  102
  105
  
    

  		




  ......




  106
  109
  
    
        printf("> tts action service not available, will not do any TTS !\n");


  		




  107
  110
  
    
    }


  		




  108
  111
  
    

  		




  109
  
  
    

  		




  110
  
  
    

  		




  111
  112
  
    
    printf("> init done\n");


  		




  112
  113
  
    
}


  		




  113
  114
  
    

  		












Also available in:  Unified diff