humotion / src / server / middleware_ros.cpp @ 6d13138a
History | View | Annotate | Download (6.779 KB)
1 | 8c6c1163 | Simon Schulz | /*
|
---|---|---|---|
2 | * This file is part of humotion
|
||
3 | *
|
||
4 | * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
|
||
5 | * http://opensource.cit-ec.de/projects/humotion
|
||
6 | *
|
||
7 | * This file may be licensed under the terms of the
|
||
8 | * GNU Lesser General Public License Version 3 (the ``LGPL''),
|
||
9 | * or (at your option) any later version.
|
||
10 | *
|
||
11 | * Software distributed under the License is distributed
|
||
12 | * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
|
||
13 | * express or implied. See the LGPL for the specific language
|
||
14 | * governing rights and limitations.
|
||
15 | *
|
||
16 | * You should have received a copy of the LGPL along with this
|
||
17 | * program. If not, go to http://www.gnu.org/licenses/lgpl.html
|
||
18 | * or write to the Free Software Foundation, Inc.,
|
||
19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||
20 | *
|
||
21 | * The development of this software was supported by the
|
||
22 | * Excellence Cluster EXC 277 Cognitive Interaction Technology.
|
||
23 | * The Excellence Cluster EXC 277 is a grant of the Deutsche
|
||
24 | * Forschungsgemeinschaft (DFG) in the context of the German
|
||
25 | * Excellence Initiative.
|
||
26 | */
|
||
27 | |||
28 | #include <boost/algorithm/string/classification.hpp> |
||
29 | 0c8d22a5 | sschulz | |
30 | #include <string> |
||
31 | |||
32 | #include "humotion/server/middleware_ros.h" |
||
33 | |||
34 | using humotion::server::MiddlewareROS;
|
||
35 | 8c6c1163 | Simon Schulz | |
36 | //! constructor
|
||
37 | 0c8d22a5 | sschulz | MiddlewareROS::MiddlewareROS(std::string scope, Controller *c)
|
38 | : Middleware(scope, c) { |
||
39 | 8c6c1163 | Simon Schulz | |
40 | 5d7ba19c | Simon Schulz | printf("> using ROS middleware\n");
|
41 | 708960ff | Simon Schulz | |
42 | 0c8d22a5 | sschulz | // start ros core?
|
43 | if (ros::isInitialized()) {
|
||
44 | // oh another process is doing ros comm, fine, we do not need to call it
|
||
45 | ea068cf1 | sschulz | tick_necessary_ = false;
|
46 | 0c8d22a5 | sschulz | } 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; |
||
51 | 708960ff | Simon Schulz | node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());
|
52 | 0c8d22a5 | sschulz | printf("> registering on ROS as node %s\n", node_name.c_str());
|
53 | 8c6c1163 | Simon Schulz | ros::M_string no_remapping; |
54 | ros::init(no_remapping, node_name); |
||
55 | ea068cf1 | sschulz | tick_necessary_ = true;
|
56 | 8c6c1163 | Simon Schulz | } |
57 | |||
58 | 0c8d22a5 | sschulz | // create node handle
|
59 | 8c6c1163 | Simon Schulz | ros::NodeHandle n; |
60 | ea068cf1 | sschulz | ros::NodeHandle pnh("~");
|
61 | 8c6c1163 | Simon Schulz | |
62 | 0c8d22a5 | sschulz | // set up subscribers
|
63 | ea068cf1 | sschulz | mouth_target_subscriber_ = n.subscribe(scope + "/humotion/mouth/target", 150, |
64 | 0c8d22a5 | sschulz | &MiddlewareROS::incoming_mouth_target , |
65 | this, ros::TransportHints().unreliable());
|
||
66 | ea068cf1 | sschulz | gaze_target_subscriber_ = n.subscribe(scope + "/humotion/gaze/target", 150, |
67 | 0c8d22a5 | sschulz | &MiddlewareROS::incoming_gaze_target, |
68 | this, ros::TransportHints().unreliable());
|
||
69 | ea068cf1 | sschulz | |
70 | // set up dynamic reconfiguration service
|
||
71 | attach_to_reconfiguration_server(pnh); |
||
72 | } |
||
73 | |||
74 | //! callback for incoming dynamic reconfigure requests:
|
||
75 | void MiddlewareROS::dynamic_reconfigure_callback(const humotion::humotionConfig &config, |
||
76 | 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 | }*/
|
||
107 | } |
||
108 | |||
109 | //! attach to dynamic reconfigure server
|
||
110 | void MiddlewareROS::attach_to_reconfiguration_server(ros::NodeHandle priv_nodehandle) {
|
||
111 | ROS_DEBUG("connecting to dynamic reconfiguration server");
|
||
112 | 6d13138a | sschulz | ros::NodeHandle reconf_node(priv_nodehandle, "humotion/configuration");
|
113 | ea068cf1 | sschulz | |
114 | reconf_server_ = new dynamic_reconfigure::Server<humotion::humotionConfig>(reconf_node);
|
||
115 | reconf_server_->setCallback(boost::bind( |
||
116 | &MiddlewareROS::dynamic_reconfigure_callback, this, _1, _2));
|
||
117 | 8c6c1163 | Simon Schulz | } |
118 | |||
119 | //! destructor
|
||
120 | 0c8d22a5 | sschulz | MiddlewareROS::~MiddlewareROS() { |
121 | 8c6c1163 | Simon Schulz | } |
122 | |||
123 | //! connection ok?
|
||
124 | //! \return true if conn is alive
|
||
125 | 0c8d22a5 | sschulz | bool MiddlewareROS::ok() {
|
126 | 8c6c1163 | Simon Schulz | return ros::ok();
|
127 | } |
||
128 | |||
129 | //! do a single tick
|
||
130 | 0c8d22a5 | sschulz | void MiddlewareROS::tick() {
|
131 | ea068cf1 | sschulz | if (tick_necessary_) {
|
132 | 8c6c1163 | Simon Schulz | ros::spinOnce(); |
133 | } |
||
134 | } |
||
135 | |||
136 | //! callback to handle incoming mouth target
|
||
137 | 0c8d22a5 | sschulz | void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg) { |
138 | // printf("> incoming mouth_target\n");
|
||
139 | 8c6c1163 | Simon Schulz | MouthState mouth_state; |
140 | |||
141 | mouth_state.position_left = msg->position.left; |
||
142 | mouth_state.position_center = msg->position.center; |
||
143 | mouth_state.position_right = msg->position.right; |
||
144 | |||
145 | mouth_state.opening_left = msg->opening.left; |
||
146 | mouth_state.opening_center = msg->opening.center; |
||
147 | mouth_state.opening_right = msg->opening.right; |
||
148 | |||
149 | ea068cf1 | sschulz | controller_->set_mouth_target(mouth_state); |
150 | 8c6c1163 | Simon Schulz | } |
151 | |||
152 | |||
153 | //! callback to handle incoming gaze target
|
||
154 | 0c8d22a5 | sschulz | void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg) { |
155 | 8c6c1163 | Simon Schulz | GazeState gaze_state; |
156 | 0c8d22a5 | sschulz | // printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f] %d = %s\n",
|
157 | // msg->pan, msg->tilt, msg->roll, msg->type,
|
||
158 | // msg->type==humotion::gaze::GAZETYPE_ABSOLUTE?"ABSOLUTE":"RELATIVE");
|
||
159 | 8c6c1163 | Simon Schulz | |
160 | gaze_state.pan = msg->pan; |
||
161 | gaze_state.tilt = msg->tilt; |
||
162 | gaze_state.roll = msg->roll; |
||
163 | gaze_state.vergence = msg->vergence; |
||
164 | |||
165 | gaze_state.pan_offset = msg->pan_offset; |
||
166 | gaze_state.tilt_offset = msg->tilt_offset; |
||
167 | gaze_state.roll_offset = msg->roll_offset; |
||
168 | |||
169 | gaze_state.eyelid_opening_upper = msg->eyelid_opening_upper; |
||
170 | gaze_state.eyelid_opening_lower = msg->eyelid_opening_lower; |
||
171 | |||
172 | gaze_state.eyebrow_left = msg->eyebrow_left; |
||
173 | gaze_state.eyebrow_right = msg->eyebrow_right; |
||
174 | |||
175 | gaze_state.eyeblink_request_left = msg->eyeblink_request_left; |
||
176 | gaze_state.eyeblink_request_right = msg->eyeblink_request_right; |
||
177 | |||
178 | 0c8d22a5 | sschulz | if (msg->gaze_type == humotion::gaze::GAZETYPE_ABSOLUTE) {
|
179 | 1c758459 | Simon Schulz | gaze_state.gaze_type = GazeState::GAZETYPE_ABSOLUTE; |
180 | 0c8d22a5 | sschulz | } else {
|
181 | 1c758459 | Simon Schulz | gaze_state.gaze_type = GazeState::GAZETYPE_RELATIVE; |
182 | } |
||
183 | 32327f15 | Simon Schulz | |
184 | gaze_state.timestamp.set(msg->gaze_timestamp.sec, msg->gaze_timestamp.nsec); |
||
185 | 8c6c1163 | Simon Schulz | |
186 | ea068cf1 | sschulz | controller_->set_gaze_target(gaze_state); |
187 | 8c6c1163 | Simon Schulz | } |