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