Fawkes API  Fawkes Development Version
clips_ros_thread.cpp
1 
2 /***************************************************************************
3  * clips_ros_thread.cpp - ROS integration for CLIPS
4  *
5  * Created: Tue Oct 22 18:14:41 2013
6  * Copyright 2006-2013 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "clips_ros_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <ros/this_node.h>
26 #include <ros/master.h>
27 #include <ros/network.h>
28 #include <ros/xmlrpc_manager.h>
29 #include <XmlRpc.h>
30 
31 #include <tuple>
32 
33 using namespace fawkes;
34 
35 /** @class ClipsROSThread "clips_ros_thread.h"
36  * ROS integration for CLIPS.
37  * @author Tim Niemueller
38  */
39 
40 /** Constructor. */
42  : Thread("ClipsROSThread", Thread::OPMODE_WAITFORWAKEUP),
43  CLIPSFeature("ros"), CLIPSFeatureAspect(this)
44 {
45 }
46 
47 
48 /** Destructor. */
50 {
51 }
52 
53 
54 void
56 {
57 }
58 
59 
60 void
62 {
63 }
64 
65 
66 void
67 ClipsROSThread::clips_context_init(const std::string &env_name,
69 {
70  envs_[env_name] = clips;
71 
72  clips->add_function("ros-get-nodes",
73  sigc::slot<void>(
74  sigc::bind<0>(
75  sigc::mem_fun(*this, &ClipsROSThread::clips_ros_get_nodes),
76  env_name)
77  )
78  );
79 
80  clips->add_function("ros-get-topics",
81  sigc::slot<void>(
82  sigc::bind<0>(
83  sigc::mem_fun(*this, &ClipsROSThread::clips_ros_get_topics),
84  env_name)
85  )
86  );
87 
88  clips->add_function("ros-get-topic-connections",
89  sigc::slot<void>(
90  sigc::bind<0>(
91  sigc::mem_fun(*this, &ClipsROSThread::clips_ros_get_topic_connections),
92  env_name)
93  )
94  );
95 
96  clips->batch_evaluate(SRCDIR"/clips/ros.clp");
97 }
98 
99 void
100 ClipsROSThread::clips_context_destroyed(const std::string &env_name)
101 {
102  envs_.erase(env_name);
103 }
104 
105 void
107 {
108 }
109 
110 
111 void
112 ClipsROSThread::clips_ros_get_nodes(std::string env_name)
113 {
114  if (envs_.find(env_name) == envs_.end()) {
115  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
116  "Cannot get ROS nodes for environment %s "
117  "(not defined)", env_name.c_str());
118  return;
119  }
120 
121  LockPtr<CLIPS::Environment> &clips = envs_[env_name];
122 
123  XmlRpc::XmlRpcValue args, result, payload;
124  args[0] = ros::this_node::getName();
125 
126  if (!ros::master::execute("getSystemState", args, result, payload, true)) {
127  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
128  "Failed to retrieve system state from ROS master");
129  return;
130  }
131 
132  std::map<std::string, RosNodeInfo> nodes;
133 
134  // publishers
135  for (int j = 0; j < payload[0].size(); ++j) {
136  std::string topic = payload[0][j][0];
137  XmlRpc::XmlRpcValue val = payload[0][j][1];
138  for (int k = 0; k < val.size(); ++k) {
139  std::string node_name = payload[0][j][1][k];
140  nodes[node_name].published.push_back(topic);
141  }
142  }
143 
144  // subscribers
145  for (int j = 0; j < payload[1].size(); ++j) {
146  std::string topic = payload[1][j][0];
147  XmlRpc::XmlRpcValue val = payload[1][j][1];
148  for (int k = 0; k < val.size(); ++k) {
149  std::string node_name = payload[1][j][1][k];
150  nodes[node_name].subscribed.push_back(topic);
151  }
152  }
153 
154  // services
155  for (int j = 0; j < payload[2].size(); ++j) {
156  std::string service = payload[2][j][0];
157  XmlRpc::XmlRpcValue val = payload[2][j][1];
158  for (int k = 0; k < val.size(); ++k) {
159  std::string node_name = payload[2][j][1][k];
160  nodes[node_name].services.push_back(service);
161  }
162  }
163 
164  fawkes::MutexLocker lock(clips.objmutex_ptr());
165  CLIPS::Template::pointer temp = clips->get_template("ros-node");
166  if (temp) {
167  for (auto n : nodes) {
168  CLIPS::Values published, subscribed, services;
169 
170  for (auto t : n.second.published) published.push_back(t);
171  for (auto t : n.second.subscribed) subscribed.push_back(t);
172  for (auto t : n.second.services) services.push_back(t);
173 
174  CLIPS::Fact::pointer fact = CLIPS::Fact::create(**clips, temp);
175  fact->set_slot("name", n.first);
176  fact->set_slot("published", published);
177  fact->set_slot("subscribed", subscribed);
178  fact->set_slot("services", services);
179 
180  CLIPS::Fact::pointer new_fact = clips->assert_fact(fact);
181  if (! new_fact) {
182  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
183  "Failed to assert ros-node fact for %s", n.first.c_str());
184  }
185  }
186  } else {
187  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
188  "Could not get deftemplate 'ros-node'");
189  }
190 }
191 
192 
193 void
194 ClipsROSThread::clips_ros_get_topics(std::string env_name)
195 {
196  if (envs_.find(env_name) == envs_.end()) {
197  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
198  "Cannot get ROS nodes for environment %s "
199  "(not defined)", env_name.c_str());
200  return;
201  }
202 
203 
204  LockPtr<CLIPS::Environment> &clips = envs_[env_name];
205 
206  ros::master::V_TopicInfo topics;
207  if (! ros::master::getTopics(topics)) {
208  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
209  "Failed to retrieve topics ROS master");
210  return;
211  }
212 
213  fawkes::MutexLocker lock(clips.objmutex_ptr());
214  CLIPS::Template::pointer temp = clips->get_template("ros-topic");
215  if (temp) {
216  for (auto t : topics) {
217  CLIPS::Fact::pointer fact = CLIPS::Fact::create(**clips, temp);
218  fact->set_slot("name", t.name);
219  fact->set_slot("type", t.datatype);
220  clips->assert_fact(fact);
221  }
222  } else {
223  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
224  "Could not get deftemplate 'ros-topic'");
225  }
226 }
227 
228 
229 void
230 ClipsROSThread::clips_ros_get_topic_connections(std::string env_name)
231 {
232  if (envs_.find(env_name) == envs_.end()) {
233  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
234  "Cannot get bus info for environment %s "
235  "(not defined)", env_name.c_str());
236  return;
237  }
238 
239  LockPtr<CLIPS::Environment> &clips = envs_[env_name];
240  fawkes::MutexLocker lock(clips.objmutex_ptr());
241 
242  CLIPS::Template::pointer temp = clips->get_template("ros-topic-connection");
243  if (! temp) {
244  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
245  "Could not get deftemplate 'ros-topic-connection'");
246  return;
247  }
248 
249  ros::V_string nodes;
250  if (! ros::master::getNodes(nodes)) {
251  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
252  "Failed to get nodes from ROS master");
253  return;
254  }
255 
256  std::map<std::string, std::string> uri_to_node;
257  std::map<std::string, XmlRpc::XmlRpcClient *> xmlrpc_clients;
258 
259  for (size_t i = 0; i < nodes.size(); ++i) {
260  XmlRpc::XmlRpcValue args, result, payload;
261  args[0] = ros::this_node::getName();
262  args[1] = nodes[i];
263  if (ros::master::execute("lookupNode", args, result, payload, false)) {
264  std::string uri = (std::string)payload;
265  uri_to_node[uri] = nodes[i];
266  std::string host;
267  uint32_t port;
268  if (ros::network::splitURI(uri, host, port)) {
269  xmlrpc_clients[nodes[i]] = new XmlRpc::XmlRpcClient(host.c_str(), port, "/");
270  }
271  }
272  }
273 
274  std::vector<std::tuple<std::string, std::string, std::string> > connections;
275 
276  ros::XMLRPCManagerPtr xm = ros::XMLRPCManager::instance();
277 
278  for (auto n : xmlrpc_clients) {
279  XmlRpc::XmlRpcValue args, result, payload;
280  args[0] = ros::this_node::getName();
281  if (n.second->execute("getBusInfo", args, result)) {
282 
283  if (! xm->validateXmlrpcResponse("getBusInfo", result, payload)) {
284  logger->log_warn(("CLIPS-ROS|" + env_name).c_str(),
285  "%s returned no valid response on getBusInfo", n.first.c_str());
286  continue;
287  }
288 
289  for (int i = 0; i < payload.size(); ++i) {
290  // Array format:
291  // ID, destination, direction, transport, topic, connected
292 
293  // roscpp does not provide the connected flag, hence regard
294  // connections which do not provide it as alive
295  bool connected =
296  (payload[i].size() >= 6) ? (bool)payload[i][5] : true;
297 
298  std::string topic = payload[i][4];
299  std::string from, to;
300  std::string nodename = payload[i][1];
301  if (uri_to_node.find(nodename) != uri_to_node.end())
302  nodename = uri_to_node[nodename];
303 
304  if (std::string(payload[i][2]) == "i") {
305  from = nodename;
306  to = n.first;
307  } else {
308  from = n.first;
309  to = nodename;
310  }
311 
312  if (connected) {
313  connections.push_back(make_tuple(topic, from, to));
314  }
315  }
316 
317  } else {
318  // node unreachable
319  //clips->assert_fact_f("(ros-node-unreachable (name %s))", n.first.c_str());
320  }
321 
322  delete n.second;
323  }
324 
325  std::vector<std::tuple<std::string, std::string, std::string> > ::iterator c;
326  std::sort(connections.begin(), connections.end());
327  c = std::unique(connections.begin(), connections.end());
328  connections.resize(c - connections.begin());
329 
330  for (auto c : connections) {
331  CLIPS::Fact::pointer fact = CLIPS::Fact::create(**clips, temp);
332  fact->set_slot("topic", std::get<0>(c));
333  fact->set_slot("from", std::get<1>(c));
334  fact->set_slot("to", std::get<2>(c));
335  clips->assert_fact(fact);
336  }
337 }
Thread aspect to provide a feature to CLIPS environments.
Definition: clips_feature.h:57
Fawkes library namespace.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
Mutex locking helper.
Definition: mutex_locker.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
virtual ~ClipsROSThread()
Destructor.
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:262
ClipsROSThread()
Constructor.
CLIPS feature maintainer.
Definition: clips_feature.h:41
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void finalize()
Finalize the thread.