22 #include "clips_ros_thread.h" 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> 42 :
Thread(
"ClipsROSThread",
Thread::OPMODE_WAITFORWAKEUP),
70 envs_[env_name] = clips;
72 clips->add_function(
"ros-get-nodes",
75 sigc::mem_fun(*
this, &ClipsROSThread::clips_ros_get_nodes),
80 clips->add_function(
"ros-get-topics",
83 sigc::mem_fun(*
this, &ClipsROSThread::clips_ros_get_topics),
88 clips->add_function(
"ros-get-topic-connections",
91 sigc::mem_fun(*
this, &ClipsROSThread::clips_ros_get_topic_connections),
96 clips->batch_evaluate(SRCDIR
"/clips/ros.clp");
102 envs_.erase(env_name);
112 ClipsROSThread::clips_ros_get_nodes(std::string env_name)
114 if (envs_.find(env_name) == envs_.end()) {
116 "Cannot get ROS nodes for environment %s " 117 "(not defined)", env_name.c_str());
123 XmlRpc::XmlRpcValue args, result, payload;
124 args[0] = ros::this_node::getName();
126 if (!ros::master::execute(
"getSystemState", args, result, payload,
true)) {
128 "Failed to retrieve system state from ROS master");
132 std::map<std::string, RosNodeInfo> nodes;
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);
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);
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);
165 CLIPS::Template::pointer temp = clips->get_template(
"ros-node");
167 for (
auto n : nodes) {
168 CLIPS::Values published, subscribed, services;
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);
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);
180 CLIPS::Fact::pointer new_fact = clips->assert_fact(fact);
183 "Failed to assert ros-node fact for %s", n.first.c_str());
188 "Could not get deftemplate 'ros-node'");
194 ClipsROSThread::clips_ros_get_topics(std::string env_name)
196 if (envs_.find(env_name) == envs_.end()) {
198 "Cannot get ROS nodes for environment %s " 199 "(not defined)", env_name.c_str());
206 ros::master::V_TopicInfo topics;
207 if (! ros::master::getTopics(topics)) {
209 "Failed to retrieve topics ROS master");
214 CLIPS::Template::pointer temp = clips->get_template(
"ros-topic");
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);
224 "Could not get deftemplate 'ros-topic'");
230 ClipsROSThread::clips_ros_get_topic_connections(std::string env_name)
232 if (envs_.find(env_name) == envs_.end()) {
234 "Cannot get bus info for environment %s " 235 "(not defined)", env_name.c_str());
242 CLIPS::Template::pointer temp = clips->get_template(
"ros-topic-connection");
245 "Could not get deftemplate 'ros-topic-connection'");
250 if (! ros::master::getNodes(nodes)) {
252 "Failed to get nodes from ROS master");
256 std::map<std::string, std::string> uri_to_node;
257 std::map<std::string, XmlRpc::XmlRpcClient *> xmlrpc_clients;
259 for (
size_t i = 0; i < nodes.size(); ++i) {
260 XmlRpc::XmlRpcValue args, result, payload;
261 args[0] = ros::this_node::getName();
263 if (ros::master::execute(
"lookupNode", args, result, payload,
false)) {
264 std::string uri = (std::string)payload;
265 uri_to_node[uri] = nodes[i];
268 if (ros::network::splitURI(uri, host, port)) {
269 xmlrpc_clients[nodes[i]] =
new XmlRpc::XmlRpcClient(host.c_str(), port,
"/");
274 std::vector<std::tuple<std::string, std::string, std::string> > connections;
276 ros::XMLRPCManagerPtr xm = ros::XMLRPCManager::instance();
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)) {
283 if (! xm->validateXmlrpcResponse(
"getBusInfo", result, payload)) {
285 "%s returned no valid response on getBusInfo", n.first.c_str());
289 for (
int i = 0; i < payload.size(); ++i) {
296 (payload[i].size() >= 6) ? (
bool)payload[i][5] :
true;
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];
304 if (std::string(payload[i][2]) ==
"i") {
313 connections.push_back(make_tuple(topic, from, to));
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());
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);
Thread aspect to provide a feature to CLIPS environments.
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.
Thread class encapsulation of pthreads.
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.
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.
ClipsROSThread()
Constructor.
CLIPS feature maintainer.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void finalize()
Finalize the thread.