Revision 0c8d22a5 src/client/middleware_ros.cpp
| src/client/middleware_ros.cpp | ||
|---|---|---|
| 25 | 25 |
* Excellence Initiative. |
| 26 | 26 |
*/ |
| 27 | 27 |
|
| 28 |
#include "client/middleware_ros.h" |
|
| 28 |
#include <boost/algorithm/string/classification.hpp> |
|
| 29 |
#include <string> |
|
| 29 | 30 |
|
| 30 |
#include "humotion/mouth.h"
|
|
| 31 |
#include "humotion/client/middleware_ros.h"
|
|
| 31 | 32 |
#include "humotion/gaze.h" |
| 32 |
#include <boost/algorithm/string/classification.hpp> |
|
| 33 |
#include "humotion/mouth.h" |
|
| 34 |
|
|
| 33 | 35 |
|
| 34 |
using namespace std; |
|
| 35 |
using namespace boost; |
|
| 36 |
using namespace humotion; |
|
| 37 |
using namespace humotion::client;
|
|
| 36 |
// using namespace std;
|
|
| 37 |
// using namespace boost;
|
|
| 38 |
// using namespace humotion;
|
|
| 39 |
using humotion::client::MiddlewareROS;
|
|
| 38 | 40 |
|
| 39 | 41 |
//! constructor |
| 40 |
MiddlewareROS::MiddlewareROS(string scope) : Middleware(scope){
|
|
| 41 |
//start ros core |
|
| 42 |
if (!ros::isInitialized()){
|
|
| 42 |
MiddlewareROS::MiddlewareROS(std::string scope) : Middleware(scope) {
|
|
| 43 |
// start ros core
|
|
| 44 |
if (!ros::isInitialized()) {
|
|
| 43 | 45 |
tick_necessary = true; |
| 44 |
string node_name = "humotion_client__"+ base_scope; |
|
| 46 |
std::string node_name = "humotion_client__"+ base_scope;
|
|
| 45 | 47 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end()); |
| 46 | 48 |
|
| 47 | 49 |
ros::M_string no_remapping; |
| 48 | 50 |
ros::init(no_remapping, node_name); |
| 49 |
}else{
|
|
| 50 |
//another ros thread takes care of spinning
|
|
| 51 |
} else {
|
|
| 52 |
// another ros thread already takes care of spinning
|
|
| 51 | 53 |
tick_necessary = false; |
| 52 | 54 |
} |
| 53 | 55 |
|
| 54 |
//create node handle |
|
| 56 |
// create node handle
|
|
| 55 | 57 |
ros::NodeHandle n; |
| 56 | 58 |
|
| 57 |
//set up publishers: |
|
| 58 |
mouth_target_publisher = n.advertise<humotion::mouth>(base_scope + "/humotion/mouth/target", 100); |
|
| 59 |
gaze_target_publisher = n.advertise<humotion::gaze>(base_scope + "/humotion/gaze/target", 100); |
|
| 60 |
|
|
| 59 |
// set up publishers |
|
| 60 |
mouth_target_publisher = n.advertise<humotion::mouth>(base_scope + |
|
| 61 |
"/humotion/mouth/target", 100); |
|
| 62 |
gaze_target_publisher = n.advertise<humotion::gaze>(base_scope + |
|
| 63 |
"/humotion/gaze/target", 100); |
|
| 61 | 64 |
} |
| 62 | 65 |
|
| 63 | 66 |
//! destructor |
| 64 |
MiddlewareROS::~MiddlewareROS(){
|
|
| 67 |
MiddlewareROS::~MiddlewareROS() {
|
|
| 65 | 68 |
} |
| 66 | 69 |
|
| 67 | 70 |
//! connection ok? |
| 68 | 71 |
//! \return true if conn is alive |
| 69 |
bool MiddlewareROS::ok(){
|
|
| 72 |
bool MiddlewareROS::ok() {
|
|
| 70 | 73 |
return ros::ok(); |
| 71 | 74 |
} |
| 72 | 75 |
|
| 73 | 76 |
//! do a single tick |
| 74 |
void MiddlewareROS::tick(){
|
|
| 75 |
if (tick_necessary){
|
|
| 77 |
void MiddlewareROS::tick() {
|
|
| 78 |
if (tick_necessary) {
|
|
| 76 | 79 |
ros::spinOnce(); |
| 77 | 80 |
} |
| 78 | 81 |
} |
| 79 | 82 |
|
| 80 | 83 |
|
| 81 | 84 |
//! send mouth target to server |
| 82 |
void MiddlewareROS::send_mouth_target(){
|
|
| 83 |
//build target packet:
|
|
| 85 |
void MiddlewareROS::send_mouth_target() {
|
|
| 86 |
// build target packet
|
|
| 84 | 87 |
humotion::mouth msg; |
| 85 | 88 |
|
| 86 |
//set timestamp |
|
| 89 |
// set timestamp
|
|
| 87 | 90 |
msg.header.stamp = ros::Time::now(); |
| 88 | 91 |
|
| 89 | 92 |
msg.position.left = mouth_state.position_left; |
| ... | ... | |
| 95 | 98 |
msg.opening.right = mouth_state.opening_right; |
| 96 | 99 |
|
| 97 | 100 |
|
| 98 |
//add position to send queue |
|
| 101 |
// add position to send queue
|
|
| 99 | 102 |
mouth_target_publisher.publish(msg); |
| 100 | 103 |
|
| 101 |
//allow ros to handle data |
|
| 104 |
// allow ros to handle data
|
|
| 102 | 105 |
tick(); |
| 103 | 106 |
} |
| 104 | 107 |
|
| 105 | 108 |
//! send mouth target to server |
| 106 |
void MiddlewareROS::send_gaze_target(){
|
|
| 107 |
//build target packet:
|
|
| 109 |
void MiddlewareROS::send_gaze_target() {
|
|
| 110 |
// build target packet
|
|
| 108 | 111 |
humotion::gaze msg; |
| 109 | 112 |
|
| 110 |
//set timestamp |
|
| 113 |
// set timestamp
|
|
| 111 | 114 |
msg.header.stamp = ros::Time::now(); |
| 112 | 115 |
|
| 113 | 116 |
msg.pan = gaze_state.pan; |
| ... | ... | |
| 128 | 131 |
msg.eyeblink_request_left = gaze_state.eyeblink_request_left; |
| 129 | 132 |
msg.eyeblink_request_right = gaze_state.eyeblink_request_right; |
| 130 | 133 |
|
| 131 |
if (gaze_state.gaze_type == GazeState::GAZETYPE_ABSOLUTE){
|
|
| 134 |
if (gaze_state.gaze_type == GazeState::GAZETYPE_ABSOLUTE) {
|
|
| 132 | 135 |
msg.gaze_type = humotion::gaze::GAZETYPE_ABSOLUTE; |
| 133 |
}else{
|
|
| 136 |
} else {
|
|
| 134 | 137 |
msg.gaze_type = humotion::gaze::GAZETYPE_RELATIVE; |
| 135 | 138 |
} |
| 136 | 139 |
msg.gaze_timestamp.sec = gaze_state.timestamp.sec; |
| 137 | 140 |
msg.gaze_timestamp.nsec = gaze_state.timestamp.nsec; |
| 138 | 141 |
|
| 139 |
//add position to send queue |
|
| 142 |
// add position to send queue
|
|
| 140 | 143 |
gaze_target_publisher.publish(msg); |
| 141 | 144 |
|
| 142 |
//allow ros to handle data |
|
| 145 |
// allow ros to handle data
|
|
| 143 | 146 |
tick(); |
| 144 | 147 |
} |
| 145 | 148 |
|
Also available in: Unified diff