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
37using 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"),
49{
50}
51
52/** Destructor. */
54{
55}
56
57void
59{
60}
61
62void
64{
65}
66
67void
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
90void
91ClipsROSThread::clips_context_destroyed(const std::string &env_name)
92{
93 envs_.erase(env_name);
94}
95
96void
98{
99}
100
101void
102ClipsROSThread::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
186void
187ClipsROSThread::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
219void
220ClipsROSThread::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}
virtual ~ClipsROSThread()
Destructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
ClipsROSThread()
Constructor.
virtual void clips_context_destroyed(const std::string &env_name)
Notification that a CLIPS environment has been destroyed.
virtual void loop()
Code to execute in the thread.
virtual void clips_context_init(const std::string &env_name, fawkes::LockPtr< CLIPS::Environment > &clips)
Initialize a CLIPS context to use the provided feature.
Thread aspect to provide a feature to CLIPS environments.
Definition: clips_feature.h:58
CLIPS feature maintainer.
Definition: clips_feature.h:42
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.