22 #include "clips_ros_thread.h"
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>
46 :
Thread(
"ClipsROSThread",
Thread::OPMODE_WAITFORWAKEUP),
70 envs_[env_name] = clips;
72 clips->add_function(
"ros-get-nodes",
74 sigc::bind<0>(sigc::mem_fun(*
this, &ClipsROSThread::clips_ros_get_nodes),
77 clips->add_function(
"ros-get-topics",
79 sigc::bind<0>(sigc::mem_fun(*
this, &ClipsROSThread::clips_ros_get_topics),
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),
87 clips->batch_evaluate(SRCDIR
"/clips/ros.clp");
93 envs_.erase(env_name);
102 ClipsROSThread::clips_ros_get_nodes(std::string env_name)
104 if (envs_.find(env_name) == envs_.end()) {
106 "Cannot get ROS nodes for environment %s "
114 XmlRpc::XmlRpcValue args, result, payload;
115 args[0] = ros::this_node::getName();
117 if (!ros::master::execute(
"getSystemState", args, result, payload,
true)) {
119 "Failed to retrieve system state from ROS master");
123 std::map<std::string, RosNodeInfo> nodes;
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);
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);
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);
156 CLIPS::Template::pointer temp = clips->get_template(
"ros-node");
158 for (
auto n : nodes) {
159 CLIPS::Values published, subscribed, services;
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);
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);
174 CLIPS::Fact::pointer new_fact = clips->assert_fact(fact);
177 "Failed to assert ros-node fact for %s",
182 logger->
log_warn((
"CLIPS-ROS|" + env_name).c_str(),
"Could not get deftemplate 'ros-node'");
187 ClipsROSThread::clips_ros_get_topics(std::string env_name)
189 if (envs_.find(env_name) == envs_.end()) {
191 "Cannot get ROS nodes for environment %s "
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");
206 CLIPS::Template::pointer temp = clips->get_template(
"ros-topic");
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);
215 logger->
log_warn((
"CLIPS-ROS|" + env_name).c_str(),
"Could not get deftemplate 'ros-topic'");
220 ClipsROSThread::clips_ros_get_topic_connections(std::string env_name)
222 if (envs_.find(env_name) == envs_.end()) {
224 "Cannot get bus info for environment %s "
233 CLIPS::Template::pointer temp = clips->get_template(
"ros-topic-connection");
236 "Could not get deftemplate 'ros-topic-connection'");
241 if (!ros::master::getNodes(nodes)) {
242 logger->
log_warn((
"CLIPS-ROS|" + env_name).c_str(),
"Failed to get nodes from ROS master");
246 std::map<std::string, std::string> uri_to_node;
247 std::map<std::string, XmlRpc::XmlRpcClient *> xmlrpc_clients;
249 for (
size_t i = 0; i < nodes.size(); ++i) {
250 XmlRpc::XmlRpcValue args, result, payload;
251 args[0] = ros::this_node::getName();
253 if (ros::master::execute(
"lookupNode", args, result, payload,
false)) {
254 std::string uri = (std::string)payload;
255 uri_to_node[uri] = nodes[i];
258 if (ros::network::splitURI(uri, host, port)) {
259 xmlrpc_clients[nodes[i]] =
new XmlRpc::XmlRpcClient(host.c_str(), port,
"/");
264 std::vector<std::tuple<std::string, std::string, std::string>> connections;
266 ros::XMLRPCManagerPtr xm = ros::XMLRPCManager::instance();
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)) {
274 "%s returned no valid response on getBusInfo",
279 for (
int i = 0; i < payload.size(); ++i) {
285 bool connected = (payload[i].size() >= 6) ? (
bool)payload[i][5] :
true;
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];
293 if (std::string(payload[i][2]) ==
"i") {
302 connections.push_back(make_tuple(topic, from, to));
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());
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);
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.
CLIPS feature maintainer.
Mutex * objmutex_ptr() const
Get object mutex.
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.
Thread class encapsulation of pthreads.
Fawkes library namespace.