Revision 2aa96942 src/server/middleware_ros.cpp
| src/server/middleware_ros.cpp | ||
|---|---|---|
| 27 | 27 |
|
| 28 | 28 |
#include <boost/algorithm/string/classification.hpp> |
| 29 | 29 |
|
| 30 |
#include <std_msgs/Float32.h> |
|
| 30 | 31 |
#include <string> |
| 31 | 32 |
|
| 32 | 33 |
#include "humotion/server/middleware_ros.h" |
| ... | ... | |
| 35 | 36 |
|
| 36 | 37 |
//! constructor |
| 37 | 38 |
MiddlewareROS::MiddlewareROS(std::string scope, Controller *c) |
| 38 |
: Middleware(scope, c) {
|
|
| 39 |
: Middleware(scope, c), nh_(scope + "/humotion") {
|
|
| 39 | 40 |
ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "will use ros middleware");
|
| 41 |
debug_initialized_ = false; |
|
| 40 | 42 |
|
| 41 | 43 |
// start ros core? |
| 42 | 44 |
if (ros::isInitialized()) {
|
| ... | ... | |
| 58 | 60 |
} |
| 59 | 61 |
|
| 60 | 62 |
// create node handle |
| 61 |
ros::NodeHandle n; |
|
| 62 | 63 |
ros::NodeHandle pnh("~");
|
| 63 | 64 |
|
| 64 | 65 |
// set up subscribers |
| 65 |
mouth_target_subscriber_ = n.subscribe(scope + "/humotion/mouth/target", 150,
|
|
| 66 |
mouth_target_subscriber_ = nh_.subscribe("mouth/target", 150,
|
|
| 66 | 67 |
&MiddlewareROS::incoming_mouth_target , |
| 67 | 68 |
this, ros::TransportHints().unreliable()); |
| 68 |
gaze_target_subscriber_ = n.subscribe(scope + "/humotion/gaze/target", 150,
|
|
| 69 |
gaze_target_subscriber_ = nh_.subscribe("gaze/target", 150,
|
|
| 69 | 70 |
&MiddlewareROS::incoming_gaze_target, |
| 70 | 71 |
this, ros::TransportHints().unreliable()); |
| 71 | 72 |
|
| ... | ... | |
| 79 | 80 |
// fetch config |
| 80 | 81 |
humotion::server::Config *config = controller_->get_config(); |
| 81 | 82 |
|
| 83 |
// debug |
|
| 84 |
config->publish_internals = dyn_config.publish_internals; |
|
| 85 |
|
|
| 82 | 86 |
// saccade detection thresholds |
| 83 | 87 |
config->threshold_velocity_eye_saccade = dyn_config.threshold_velocity_eye_saccade; |
| 84 | 88 |
config->threshold_angle_neck_saccade = dyn_config.threshold_angle_neck_saccade; |
| 85 | 89 |
config->threshold_angle_omr_limit = dyn_config.threshold_angle_omr_limit; |
| 86 | 90 |
|
| 87 |
config->use_neck_target_instead_of_position_eye = dyn_config.use_neck_target_instead_of_position_eye; |
|
| 91 |
config->use_neck_target_instead_of_position_eye = |
|
| 92 |
dyn_config.use_neck_target_instead_of_position_eye; |
|
| 88 | 93 |
|
| 89 | 94 |
// neck motion generation configuration |
| 90 | 95 |
config->scale_velocity_neck = dyn_config.scale_velocity_neck; |
| ... | ... | |
| 141 | 146 |
} |
| 142 | 147 |
} |
| 143 | 148 |
|
| 149 |
//! init debug dataset publishers |
|
| 150 |
void MiddlewareROS::debug_initialize() {
|
|
| 151 |
if (debug_initialized_) {
|
|
| 152 |
return; |
|
| 153 |
} |
|
| 154 |
|
|
| 155 |
debug_initialized_ = true; |
|
| 156 |
} |
|
| 157 |
|
|
| 158 |
//! publish a debug dataset |
|
| 159 |
void MiddlewareROS::publish_debug_dataset(debug_data_t debug_data) {
|
|
| 160 |
if (!controller_->get_config()->publish_internals) {
|
|
| 161 |
// no debugging data enabled, return |
|
| 162 |
return; |
|
| 163 |
} |
|
| 164 |
|
|
| 165 |
debug_initialize(); |
|
| 166 |
|
|
| 167 |
// iterate over all debug variables and publish them |
|
| 168 |
debug_data_t::iterator it; |
|
| 169 |
for (it = debug_data.begin(); it != debug_data.end(); it++) {
|
|
| 170 |
publish_debug_data(it->first, it->second); |
|
| 171 |
} |
|
| 172 |
} |
|
| 173 |
|
|
| 174 |
void MiddlewareROS::publish_debug_data(std::string name, float value) {
|
|
| 175 |
debug_topic_map_t::iterator it = debug_topic_map.find(name); |
|
| 176 |
if (it == debug_topic_map.end()) {
|
|
| 177 |
// we have no publisher for this dataset, create one: |
|
| 178 |
ROS_DEBUG_STREAM("creating debug output stream " << name);
|
|
| 179 |
ros::Publisher pub = nh_.advertise<std_msgs::Float32>("debug_data/" + name, 1000);
|
|
| 180 |
|
|
| 181 |
std::pair<debug_topic_map_t::iterator, bool> ret; |
|
| 182 |
ret = debug_topic_map.insert(std::pair<std::string, ros::Publisher>(name, pub)); |
|
| 183 |
it = ret.first; |
|
| 184 |
} |
|
| 185 |
|
|
| 186 |
// in any case, it is now the publisher we want so publish data now |
|
| 187 |
std_msgs::Float32 msg; |
|
| 188 |
msg.data = value; |
|
| 189 |
|
|
| 190 |
// std::cout << "DEBUG HUMOTION " << name << " " << value << std::endl; |
|
| 191 |
|
|
| 192 |
// send it |
|
| 193 |
it->second.publish(msg); |
|
| 194 |
} |
|
| 195 |
|
|
| 144 | 196 |
//! callback to handle incoming mouth target |
| 145 | 197 |
void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg) {
|
| 146 | 198 |
// printf("> incoming mouth_target\n");
|
Also available in: Unified diff