Revision 277050c7
| cfg/humotion.cfg | ||
|---|---|---|
| 1 | 1 |
#!/usr/bin/env python |
| 2 |
import re |
|
| 2 | 3 |
PACKAGE = "humotion" |
| 3 | 4 |
from dynamic_reconfigure.parameter_generator_catkin import * |
| 4 | 5 |
|
| 5 |
gen = ParameterGenerator() |
|
| 6 |
def fetch_default(param_name): |
|
| 7 |
for line in open("../src/server/config.cpp"):
|
|
| 8 |
if param_name in line: |
|
| 9 |
result = re.findall("=\s*([\d.]+)", line)
|
|
| 10 |
print result |
|
| 11 |
if result: |
|
| 12 |
return float(result[0]) |
|
| 13 |
else: |
|
| 14 |
print("ERROR: could not find parameter %s in config.cpp" % (param_name))
|
|
| 15 |
sys.exit(1) |
|
| 16 |
|
|
| 17 |
def add_entry(group, param_name, descr, min, max): |
|
| 18 |
default_val = fetch_default(param_name) |
|
| 19 |
if (default_val > max): |
|
| 20 |
print("ERROR: default value %f for %s exceeds max value (%f)" % (default_val, param_name, max))
|
|
| 21 |
sys.exit(1) |
|
| 22 |
if (default_val < min): |
|
| 23 |
print("ERROR: default value %f for %s is under min value (%f)" % (default_val, param_name, min))
|
|
| 24 |
sys.exit(1) |
|
| 25 |
|
|
| 26 |
group.add(param_name, double_t, 0, descr, default_val, min, max) |
|
| 6 | 27 |
|
| 7 |
general_group = gen.add_group("general")
|
|
| 8 |
general_group.add("eye_saccade_velocity_threshold", double_t, 0, "eye velocity threshold for saccade detection (in deg/s)", 15.0, 1.0, 30.0)
|
|
| 9 |
general_group.add("neck_saccade_threshold", double_t, 0, "magnitude of gaze change that triggers neck saccade (in deg)", 15.0, 1.0, 30.0)
|
|
| 10 |
general_group.add("neck_saccade_omr_trigger", double_t, 0, "a deflection exceeding <VAL> * OMR will trigger a correction neck saccade", 0.95, 0.1, 1.0)
|
|
| 28 |
gen = ParameterGenerator() |
|
| 11 | 29 |
|
| 12 |
neck_group = gen.add_group("neck")
|
|
| 13 |
neck_group.add("neck_max_acceleration", int_t, 0, "maximum neck acceleration limit (in deg/s^2)", 1000, 100, 4000)
|
|
| 14 |
neck_group.add("neck_max_velocity", int_t, 0, "maximum neck velocity limit (in deg/s)", 1000, 100, 800)
|
|
| 15 |
neck_group.add("neck_velocity_scale", double_t, 0, "scaling factor for neck accelerations (1.0 = full human velocities)", 0.7, 0.1, 1.0)
|
|
| 30 |
general_group = gen.add_group("thresholds")
|
|
| 31 |
add_entry(general_group, "threshold_velocity_eye_saccade", "velocity threshold for eye saccade detection (in deg/s)", 1.0, 30.0)
|
|
| 32 |
add_entry(general_group, "threshold_angle_neck_saccade", "magnitude of gaze change that triggers neck saccade (in deg)", 1.0, 30.0)
|
|
| 33 |
add_entry(general_group, "threshold_angle_omr_limit", "threshold for a deflection that triggers a correction neck saccade (in percent of OMR)", 0.1, 1.0)
|
|
| 16 | 34 |
|
| 17 |
#float max_speed = (CONST_GUITTON87_A * distance_abs + CONST_GUITTON87_B); |
|
| 18 |
neck_group.add("neck_velocity_lin_eq_const_a", double_t, 0, "constant A (scale) for linear velocity equation (guitton 4.39)", 4.39 / 2.0, 1.0, 10.0)
|
|
| 19 |
neck_group.add("neck_velocity_lin_eq_const_b", double_t, 0, "constant B (offset) for linear velocity equation (guitton 4.39)", 106.0 / 2.0, 1, 200)
|
|
| 20 | 35 |
|
| 21 |
#others |
|
| 22 |
neck_group.add("neck_breath_period", int_t, 0, "breath period (inhale+pause+exhale) in ms)", 3*1500, 3000, 5000)
|
|
| 23 |
neck_group.add("neck_breath_amplitude", double_t, 0, "amplitude of head tilt deflection during breath (given in deg)", 1.0, 0.0, 5.0)
|
|
| 36 |
neck_group = gen.add_group("neck")
|
|
| 37 |
add_entry(neck_group, "scale_velocity_neck", "scaling factor for neck velocity (in percent, 1.0 = full human velocities)", 0.1, 1.0) |
|
| 38 |
add_entry(neck_group, "scale_acceleration_neck", "scaling factor for neck acceleration (in percent, 1.0 = full human acceleration)", 0.1, 1.0) |
|
| 39 |
add_entry(neck_group, "limit_velocity_neck", "limit for neck velocity (in deg/s)", 100.0, 800.0) |
|
| 40 |
add_entry(neck_group, "limit_acceleration_neck", "limit for neck acceleration (in deg/s^2)", 100.0, 10000.0) |
|
| 24 | 41 |
|
| 25 | 42 |
eye_group = gen.add_group("eye")
|
| 26 |
eye_group.add("eye_max_acceleration", int_t, 0, "maximum eye acceleration limit (in deg/s^2)", 80000, 1000, 80000)
|
|
| 27 |
eye_group.add("eye_max_velocity", int_t, 0, "maximum eye velocity limit (in deg/s)", 700, 100, 700)
|
|
| 28 |
eye_group.add("eye_velocity_scale", double_t, 0, "scaling factor for eye accelerations (1.0 = full human velocities)", 1.0, 0.1, 1.0)
|
|
| 43 |
add_entry(eye_group, "scale_velocity_eye", "scaling factor for eye velocity (in percent, 1.0 = full human velocities)", 0.1, 1.0) |
|
| 44 |
add_entry(eye_group, "scale_acceleration_eye", "scaling factor for eye acceleration (in percent, 1.0 = full human acceleration)", 0.1, 1.0) |
|
| 45 |
add_entry(eye_group, "limit_velocity_eye", "limit for eye velocity (in deg/s)", 100.0, 1000.0) |
|
| 46 |
add_entry(eye_group, "limit_acceleration_eye", "limit for eye acceleration (in deg/s^2)", 100.0, 80000.0) |
|
| 29 | 47 |
|
| 30 | 48 |
eyeblink_group = gen.add_group("eyeblink")
|
| 31 |
eyeblink_group.add("eyeblink_duration", int_t, 0, "eyeblink duration (in ms)", 150, 50, 500)
|
|
| 32 |
eyeblink_group.add("eyeblink_every_min", int_t, 0, "eyeblink every n ms, lower bound (in ms)", 2000, 1000, 10000)
|
|
| 33 |
eyeblink_group.add("eyeblink_every_max", int_t, 0, "eyeblink every n ms, upper bound (in ms)", 10000, 1000, 20000)
|
|
| 34 |
eyeblink_group.add("eyeblink_blocking_time", int_t, 0, "eyeblink blocked timeout (in ms)", 1000, 100, 5000)
|
|
| 35 |
|
|
| 36 |
|
|
| 37 |
|
|
| 38 |
|
|
| 39 |
#eyelid: |
|
| 40 |
# static const float SACCADE_SPEED_THRESHOLD; |
|
| 41 |
# static const float EYEBLINK_DURATION_MS; |
|
| 42 |
# static const float EYEBLINK_EYERY_MS_MIN; |
|
| 43 |
# static const float EYEBLINK_EYERY_MS_MAX; |
|
| 44 |
# static const float EYEBLINK_BLOCKING_TIME; |
|
| 45 |
# |
|
| 46 |
|
|
| 47 |
# static const float MOUTH_MIN_OPENING; |
|
| 48 |
# |
|
| 49 |
#neck |
|
| 50 |
# static const float CONST_GUITTON87_A; |
|
| 51 |
# static const float CONST_GUITTON87_B; |
|
| 52 |
# |
|
| 53 |
# static const float CONST_BREATH_PERIOD; |
|
| 54 |
# static const float CONST_BREATH_AMPLITUDE; |
|
| 55 |
# |
|
| 56 |
#misc for neck and eyes: |
|
| 57 |
#max accel |
|
| 58 |
#v_scale |
|
| 59 |
|
|
| 60 |
#timing_mode_enum = gen.enum([ |
|
| 61 |
# gen.const("free_running", int_t, 0, "camera acquires images at a maximum possible framerate"),
|
|
| 62 |
# gen.const("frame_rate", int_t, 1, "please refer to xiAPI Frame Rate Control support page")], "enum to configure timing mode")
|
|
| 63 |
#gen.add("acq_timing_mode", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=timing_mode_enum)######
|
|
| 64 |
# |
|
| 65 |
#gen.add("framerate", double_t, 0, "Defines frames per second of sensor. see also acq_timint_mode", 30, 1.0, 200.0)
|
|
| 66 |
# |
|
| 67 |
#gen.add("gain", double_t, 0, "Gain in dB", 0, -1.5, 6)
|
|
| 68 |
#gen.add("exposure", double_t, 0, "Exposure time in us", 26, 26, 1e+04)
|
|
| 69 |
#gen.add("aeag", bool_t, 0, "Automatic exposure/gain", False)
|
|
| 70 |
#gen.add("aeag_roi_offset_x", int_t, 0, "Automatic exposure/gain ROI offset X", 0, 0, 2040)
|
|
| 71 |
#gen.add("aeag_roi_offset_y", int_t, 0, "Automatic exposure/gain ROI offset Y", 0, 0, 1080)
|
|
| 72 |
#gen.add("aeag_roi_width", int_t, 0, "Automatic exposure/gain ROI Width", 2040, 0, 2040)
|
|
| 73 |
#gen.add("aeag_roi_height", int_t, 0, "Automatic exposure/gain ROI Height", 1080, 0, 1080)
|
|
| 74 |
#gen.add("bpc", bool_t, 0, "Correction of bad pixels", False)
|
|
| 75 |
#gen.add("auto_wb", bool_t, 0, "Automatic white balance", False)
|
|
| 76 |
#gen.add("width", int_t, 0, "Width of the Image provided by the device (in pixels)", 2040, 16, 2040)
|
|
| 77 |
#gen.add("height", int_t, 0, "Height of the Image provided by the device (in pixels)", 1080, 2, 1080)
|
|
| 78 |
#gen.add("offsetX", int_t, 0, "Horizontal offset from the origin to the area of interest (in pixels)", 0, 0, 2040)
|
|
| 79 |
##gen.add("offsetY", int_t, 0, "Vertical offset from the origin to the area of interest (in pixels)", 0, 0, 1080)
|
|
| 80 |
# |
|
| 81 |
#gen.add("recent_frame", bool_t, 0, "Retrieve the most recent frame instead of the next in buffer", False);
|
|
| 49 |
add_entry(eyeblink_group, "eyeblink_duration", "duration for an eyeblink (in seconds)", 0.01, 1.0) |
|
| 50 |
add_entry(eyeblink_group, "eyeblink_periodic_distribution_lower", "lower bound for probalistic eyeblink distribution (in seconds)", 0.1, 100.0) |
|
| 51 |
add_entry(eyeblink_group, "eyeblink_periodic_distribution_upper", "upper bound for probalistic eyeblink distribution (in seconds)", 0.1, 100.0) |
|
| 52 |
add_entry(eyeblink_group, "eyeblink_probability_after_saccade", "probability for an eyeblink after a saccade (in percent)", 0.01, 1.0) |
|
| 53 |
add_entry(eyeblink_group, "eyeblink_blocked_time", "blocking time for further eyeblinks (in seconds)", 0.1, 100.0) |
|
| 54 |
|
|
| 55 |
breath_group = gen.add_group("breath")
|
|
| 56 |
add_entry(breath_group, "breath_period", "duration for a full breath periond: inhale, pause and exhale (in seconds)", 1.0, 100.0) |
|
| 57 |
add_entry(breath_group, "breath_amplitude", "amplitude for breath animation (in deg)", 0.0, 10.0) |
|
| 82 | 58 |
|
| 83 | 59 |
exit(gen.generate(PACKAGE, "humotion", "humotion")) |
| 84 | 60 |
|
| include/humotion/server/controller.h | ||
|---|---|---|
| 52 | 52 |
void set_gaze_target(GazeState s); |
| 53 | 53 |
void set_mouth_target(MouthState s); |
| 54 | 54 |
void set_activated(void); |
| 55 |
Config* get_config(); |
|
| 55 | 56 |
|
| 56 | 57 |
private: |
| 57 | 58 |
void add_motion_generator(MotionGenerator *m); |
| src/server/config.cpp | ||
|---|---|---|
| 109 | 109 |
eyeblink_probability_after_saccade = 0.33; |
| 110 | 110 |
|
| 111 | 111 |
// blocking time where further eyeblinks are suppressed, value given in seconds |
| 112 |
eyeblink_blocked_time = 1000.0;
|
|
| 112 |
eyeblink_blocked_time = 1.0; |
|
| 113 | 113 |
} |
| src/server/controller.cpp | ||
|---|---|---|
| 222 | 222 |
(*it)->set_mouth_target(s); |
| 223 | 223 |
} |
| 224 | 224 |
} |
| 225 |
|
|
| 226 |
//! access the configuration |
|
| 227 |
Config* Controller::get_config() {
|
|
| 228 |
return config_; |
|
| 229 |
} |
|
| src/server/middleware_ros.cpp | ||
|---|---|---|
| 36 | 36 |
//! constructor |
| 37 | 37 |
MiddlewareROS::MiddlewareROS(std::string scope, Controller *c) |
| 38 | 38 |
: Middleware(scope, c) {
|
| 39 |
|
|
| 40 |
printf("> using ROS middleware\n");
|
|
| 39 |
ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "will use ros middleware");
|
|
| 41 | 40 |
|
| 42 | 41 |
// start ros core? |
| 43 | 42 |
if (ros::isInitialized()) {
|
| ... | ... | |
| 45 | 44 |
tick_necessary_ = false; |
| 46 | 45 |
} else {
|
| 47 | 46 |
// we have to take care of ros |
| 48 |
printf("> no active ros middleware, calling ros::init() "
|
|
| 49 |
"and we will call tick() periodically!\n"); |
|
| 47 |
ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "no active ros middleware, will call ros::init() "
|
|
| 48 |
"now and we will call tick() periodically!\n"); |
|
| 49 |
|
|
| 50 | 50 |
std::string node_name = "humotion_server__"+ scope; |
| 51 | 51 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end()); |
| 52 |
printf("> registering on ROS as node %s\n", node_name.c_str());
|
|
| 52 |
|
|
| 53 |
ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "registering on ROS as node " << node_name.c_str());
|
|
| 54 |
|
|
| 53 | 55 |
ros::M_string no_remapping; |
| 54 | 56 |
ros::init(no_remapping, node_name); |
| 55 | 57 |
tick_necessary_ = true; |
| ... | ... | |
| 72 | 74 |
} |
| 73 | 75 |
|
| 74 | 76 |
//! callback for incoming dynamic reconfigure requests: |
| 75 |
void MiddlewareROS::dynamic_reconfigure_callback(const humotion::humotionConfig &config, |
|
| 77 |
void MiddlewareROS::dynamic_reconfigure_callback(const humotion::humotionConfig &dyn_config,
|
|
| 76 | 78 |
uint32_t level) {
|
| 77 |
/*// ignore incoming requests as long cam is not set up properly |
|
| 78 |
if (!hasValidHandle()) {
|
|
| 79 |
return; |
|
| 80 |
} |
|
| 81 |
|
|
| 82 |
// use some tricks to iterate through all config entries: |
|
| 83 |
std::vector<ximea_camera::xiAPIConfig::AbstractParamDescriptionConstPtr>::const_iterator _i; |
|
| 84 |
for (_i = config.__getParamDescriptions__().begin(); |
|
| 85 |
_i != config.__getParamDescriptions__().end(); ++_i) {
|
|
| 86 |
boost::any val; |
|
| 87 |
boost::shared_ptr<const ximea_camera::xiAPIConfig::AbstractParamDescription> |
|
| 88 |
description = *_i; |
|
| 89 |
|
|
| 90 |
// fetch actual value: |
|
| 91 |
description->getValue(config, val); |
|
| 92 |
|
|
| 93 |
// std::cout << description->name << " " << description->type << "\n"; |
|
| 94 |
|
|
| 95 |
// copy data to ximea api: |
|
| 96 |
if (description->type == "double") {
|
|
| 97 |
dynamicReconfigureFloat(description->name.c_str(), |
|
| 98 |
static_cast<float>(boost::any_cast<double>(val))); |
|
| 99 |
} else if (description->type == "bool") {
|
|
| 100 |
dynamicReconfigureInt(description->name.c_str(), (boost::any_cast<bool>(val))?1:0); |
|
| 101 |
} else if (description->type == "int") {
|
|
| 102 |
dynamicReconfigureInt(description->name.c_str(), boost::any_cast<int>(val)); |
|
| 103 |
} else {
|
|
| 104 |
std::cerr << "ERROR: unsupported config type " << description->type << "\n"; |
|
| 105 |
} |
|
| 106 |
}*/ |
|
| 79 |
// fetch config |
|
| 80 |
humotion::server::Config *config = controller_->get_config(); |
|
| 81 |
|
|
| 82 |
// saccade detection thresholds |
|
| 83 |
config->threshold_velocity_eye_saccade = dyn_config.threshold_velocity_eye_saccade; |
|
| 84 |
config->threshold_angle_neck_saccade = dyn_config.threshold_angle_neck_saccade; |
|
| 85 |
config->threshold_angle_omr_limit = dyn_config.threshold_angle_omr_limit; |
|
| 86 |
|
|
| 87 |
|
|
| 88 |
// neck motion generation configuration |
|
| 89 |
config->scale_velocity_neck = dyn_config.scale_velocity_neck; |
|
| 90 |
config->scale_acceleration_neck = dyn_config.scale_acceleration_neck; |
|
| 91 |
config->limit_velocity_neck = dyn_config.limit_velocity_neck; |
|
| 92 |
config->limit_acceleration_neck = dyn_config.limit_acceleration_neck; |
|
| 93 |
|
|
| 94 |
// eye motion generation configuration |
|
| 95 |
config->scale_velocity_eye = dyn_config.scale_velocity_eye; |
|
| 96 |
config->scale_acceleration_eye = dyn_config.scale_acceleration_eye; |
|
| 97 |
config->limit_velocity_eye = dyn_config.limit_velocity_eye; |
|
| 98 |
config->limit_acceleration_eye = dyn_config.limit_acceleration_eye; |
|
| 99 |
|
|
| 100 |
// parameters fo the breathing pattern |
|
| 101 |
config->breath_period = dyn_config.breath_period; |
|
| 102 |
config->breath_amplitude = dyn_config.breath_amplitude; |
|
| 103 |
|
|
| 104 |
// parameters for eye blinking |
|
| 105 |
config->eyeblink_duration = dyn_config.eyeblink_duration; |
|
| 106 |
config->eyeblink_periodic_distribution_lower = dyn_config.eyeblink_periodic_distribution_lower; |
|
| 107 |
config->eyeblink_periodic_distribution_upper = dyn_config.eyeblink_periodic_distribution_upper; |
|
| 108 |
config->eyeblink_probability_after_saccade = dyn_config.eyeblink_probability_after_saccade; |
|
| 109 |
config->eyeblink_blocked_time = dyn_config.eyeblink_blocked_time; |
|
| 107 | 110 |
} |
| 108 | 111 |
|
| 109 | 112 |
//! attach to dynamic reconfigure server |
| 110 | 113 |
void MiddlewareROS::attach_to_reconfiguration_server(ros::NodeHandle priv_nodehandle) {
|
| 111 |
ROS_DEBUG("connecting to dynamic reconfiguration server");
|
|
| 114 |
ROS_DEBUG_STREAM_NAMED("MiddlewareROS", "connecting to dynamic reconfiguration server");
|
|
| 115 |
|
|
| 112 | 116 |
ros::NodeHandle reconf_node(priv_nodehandle, "humotion/configuration"); |
| 113 | 117 |
|
| 114 | 118 |
reconf_server_ = new dynamic_reconfigure::Server<humotion::humotionConfig>(reconf_node); |
| src/server/neck_motion_generator.cpp | ||
|---|---|---|
| 73 | 73 |
} |
| 74 | 74 |
|
| 75 | 75 |
// fetch next time |
| 76 |
breath_time_ += 1000.0/Server::MOTION_UPDATERATE;
|
|
| 76 |
breath_time_ += 1.0/Server::MOTION_UPDATERATE; |
|
| 77 | 77 |
|
| 78 | 78 |
if (breath_time_ >= config->breath_period) {
|
| 79 | 79 |
breath_time_ -= config->breath_period; |
Also available in: Unified diff