Revision 0c8d22a5 src/server/middleware_ros.cpp
| src/server/middleware_ros.cpp | ||
|---|---|---|
| 25 | 25 |
* Excellence Initiative. |
| 26 | 26 |
*/ |
| 27 | 27 |
|
| 28 |
#include "server/middleware_ros.h" |
|
| 29 |
|
|
| 30 | 28 |
#include <boost/algorithm/string/classification.hpp> |
| 31 |
using namespace std; |
|
| 32 |
using namespace boost; |
|
| 33 |
using namespace humotion; |
|
| 34 |
using namespace humotion::server; |
|
| 29 |
|
|
| 30 |
#include <string> |
|
| 31 |
|
|
| 32 |
#include "humotion/server/middleware_ros.h" |
|
| 33 |
|
|
| 34 |
using humotion::server::MiddlewareROS; |
|
| 35 | 35 |
|
| 36 | 36 |
//! constructor |
| 37 |
MiddlewareROS::MiddlewareROS(string scope, Controller *c) |
|
| 38 |
: Middleware(scope, c){
|
|
| 37 |
MiddlewareROS::MiddlewareROS(std::string scope, Controller *c)
|
|
| 38 |
: Middleware(scope, c) {
|
|
| 39 | 39 |
|
| 40 | 40 |
printf("> using ROS middleware\n");
|
| 41 | 41 |
|
| 42 |
//start ros core? |
|
| 43 |
if (ros::isInitialized()){
|
|
| 44 |
//oh another process is doing ros comm, fine, we do not need to call it |
|
| 42 |
// start ros core?
|
|
| 43 |
if (ros::isInitialized()) {
|
|
| 44 |
// oh another process is doing ros comm, fine, we do not need to call it
|
|
| 45 | 45 |
tick_necessary = false; |
| 46 |
}else{
|
|
| 47 |
//we have to take care of ros |
|
| 48 |
printf("> no active ros middleware, calling ros::init() and we will call tick() periodically!\n");
|
|
| 49 |
string node_name = "humotion_server__"+ scope; |
|
| 46 |
} else {
|
|
| 47 |
// we have to take care of ros |
|
| 48 |
printf("> no active ros middleware, calling ros::init() "
|
|
| 49 |
"and we will call tick() periodically!\n"); |
|
| 50 |
std::string node_name = "humotion_server__"+ scope; |
|
| 50 | 51 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end()); |
| 51 |
printf("> registering on ROS as node %s\n",node_name.c_str());
|
|
| 52 |
printf("> registering on ROS as node %s\n", node_name.c_str());
|
|
| 52 | 53 |
ros::M_string no_remapping; |
| 53 | 54 |
ros::init(no_remapping, node_name); |
| 54 | 55 |
tick_necessary = true; |
| 55 | 56 |
} |
| 56 | 57 |
|
| 57 |
//create node handle |
|
| 58 |
// create node handle
|
|
| 58 | 59 |
ros::NodeHandle n; |
| 59 | 60 |
|
| 60 |
//set up subscribers: |
|
| 61 |
mouth_target_subscriber = n.subscribe(scope + "/humotion/mouth/target", 150, &MiddlewareROS::incoming_mouth_target , this, ros::TransportHints().unreliable()); |
|
| 62 |
gaze_target_subscriber = n.subscribe(scope + "/humotion/gaze/target", 150, &MiddlewareROS::incoming_gaze_target, this, ros::TransportHints().unreliable()); |
|
| 63 |
|
|
| 61 |
// set up subscribers |
|
| 62 |
mouth_target_subscriber = n.subscribe(scope + "/humotion/mouth/target", 150, |
|
| 63 |
&MiddlewareROS::incoming_mouth_target , |
|
| 64 |
this, ros::TransportHints().unreliable()); |
|
| 65 |
gaze_target_subscriber = n.subscribe(scope + "/humotion/gaze/target", 150, |
|
| 66 |
&MiddlewareROS::incoming_gaze_target, |
|
| 67 |
this, ros::TransportHints().unreliable()); |
|
| 64 | 68 |
} |
| 65 | 69 |
|
| 66 | 70 |
//! destructor |
| 67 |
MiddlewareROS::~MiddlewareROS(){
|
|
| 71 |
MiddlewareROS::~MiddlewareROS() {
|
|
| 68 | 72 |
} |
| 69 | 73 |
|
| 70 | 74 |
//! connection ok? |
| 71 | 75 |
//! \return true if conn is alive |
| 72 |
bool MiddlewareROS::ok(){
|
|
| 76 |
bool MiddlewareROS::ok() {
|
|
| 73 | 77 |
return ros::ok(); |
| 74 | 78 |
} |
| 75 | 79 |
|
| 76 | 80 |
//! do a single tick |
| 77 |
void MiddlewareROS::tick(){
|
|
| 78 |
if (tick_necessary){
|
|
| 81 |
void MiddlewareROS::tick() {
|
|
| 82 |
if (tick_necessary) {
|
|
| 79 | 83 |
ros::spinOnce(); |
| 80 | 84 |
} |
| 81 | 85 |
} |
| 82 | 86 |
|
| 83 | 87 |
//! callback to handle incoming mouth target |
| 84 |
void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg){
|
|
| 85 |
//printf("> incoming mouth_target\n");
|
|
| 88 |
void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg) {
|
|
| 89 |
// printf("> incoming mouth_target\n");
|
|
| 86 | 90 |
MouthState mouth_state; |
| 87 | 91 |
|
| 88 | 92 |
mouth_state.position_left = msg->position.left; |
| ... | ... | |
| 98 | 102 |
|
| 99 | 103 |
|
| 100 | 104 |
//! callback to handle incoming gaze target |
| 101 |
void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){
|
|
| 105 |
void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg) {
|
|
| 102 | 106 |
GazeState gaze_state; |
| 103 |
//printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %d = %s\n", msg->pan, msg->tilt, msg->roll, msg->type, msg->type==humotion::gaze::GAZETYPE_ABSOLUTE?"ABSOLUTE":"RELATIVE");
|
|
| 107 |
// printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %d = %s\n",
|
|
| 108 |
// msg->pan, msg->tilt, msg->roll, msg->type, |
|
| 109 |
// msg->type==humotion::gaze::GAZETYPE_ABSOLUTE?"ABSOLUTE":"RELATIVE"); |
|
| 104 | 110 |
|
| 105 | 111 |
gaze_state.pan = msg->pan; |
| 106 | 112 |
gaze_state.tilt = msg->tilt; |
| ... | ... | |
| 120 | 126 |
gaze_state.eyeblink_request_left = msg->eyeblink_request_left; |
| 121 | 127 |
gaze_state.eyeblink_request_right = msg->eyeblink_request_right; |
| 122 | 128 |
|
| 123 |
if (msg->gaze_type == humotion::gaze::GAZETYPE_ABSOLUTE){
|
|
| 129 |
if (msg->gaze_type == humotion::gaze::GAZETYPE_ABSOLUTE) {
|
|
| 124 | 130 |
gaze_state.gaze_type = GazeState::GAZETYPE_ABSOLUTE; |
| 125 |
}else{
|
|
| 131 |
} else {
|
|
| 126 | 132 |
gaze_state.gaze_type = GazeState::GAZETYPE_RELATIVE; |
| 127 | 133 |
} |
| 128 | 134 |
|
| 129 | 135 |
gaze_state.timestamp.set(msg->gaze_timestamp.sec, msg->gaze_timestamp.nsec); |
| 130 | 136 |
|
| 131 | 137 |
controller->set_gaze_target(gaze_state); |
| 132 |
|
|
| 133 | 138 |
} |
| 134 | 139 |
|
Also available in: Unified diff