Statistics
| Branch: | Tag: | Revision:

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
}