Node.cpp
Go to the documentation of this file.
00001 
00012 // worldlib
00013 #include "worldlib/remote/Node.h"
00014 
00015 // ROS
00016 #include <ros/package.h>
00017 
00018 using namespace std;
00019 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00020 using namespace rail::spatial_temporal_learning::worldlib::remote;
00021 
00022 Node::Node() : private_node_("~"), tf_listener_(tfs_)
00023 {
00024   okay_ = true;
00025 }
00026 
00027 bool Node::loadWorldYamlFile(const bool verbose)
00028 {
00029   // location of the world config file
00030   string world_config(ros::package::getPath("worldlib") + "/config/world.yaml");
00031   private_node_.getParam("world_config", world_config);
00032   if (verbose)
00033   {
00034     ROS_INFO("World Configutation YAML: %s", world_config.c_str());
00035   }
00036   // load the config
00037   return world_.loadFromYaml(world_config);
00038 }
00039 
00040 InteractiveWorldModelClient *Node::createInteractiveWorldModelClient(const bool verbose) const
00041 {
00042   // set connection defaults
00043   int port = InteractiveWorldModelClient::DEFAULT_PORT;
00044   string host("robotsfor.me");
00045 
00046   // grab any parameters we need
00047   node_.getParam("/worldlib/interactive_world_model_client/host", host);
00048   node_.getParam("/worldlib/interactive_world_model_client/port", port);
00049 
00050   // create the new client
00051   InteractiveWorldModelClient *client = new InteractiveWorldModelClient(host, port);
00052 
00053   // check verbosity
00054   if (verbose)
00055   {
00056     ROS_INFO("Interactive World Model Server: http://%s:%hu/", client->getHost().c_str(), client->getPort());
00057   }
00058 
00059   return client;
00060 }
00061 
00062 SpatialWorldClient *Node::createSpatialWorldClient(const bool verbose) const
00063 {
00064   // set connection defaults
00065   int port = SpatialWorldClient::DEFAULT_PORT;
00066   string host("localhost");
00067   string user("ros");
00068   string password("");
00069   string database("rms");
00070 
00071   // grab any parameters we need
00072   node_.getParam("/worldlib/spatial_world_client/host", host);
00073   node_.getParam("/worldlib/spatial_world_client/port", port);
00074   node_.getParam("/worldlib/spatial_world_client/user", user);
00075   node_.getParam("/worldlib/spatial_world_client/password", password);
00076   node_.getParam("/worldlib/spatial_world_client/database", database);
00077 
00078   // create the new client
00079   SpatialWorldClient *client = new SpatialWorldClient(host, port, user, password, database);
00080 
00081   // check verbosity
00082   if (verbose)
00083   {
00084     ROS_INFO("Spatial World Server: mysql://%s@%s:%hu/%s (Using Password: %s)", client->getUser().c_str(),
00085              client->getHost().c_str(), client->getPort(), client->getDatabase().c_str(),
00086              (client->getPassword().empty()) ? "NO" : "YES");
00087   }
00088 
00089   return client;
00090 }
00091 
00092 bool Node::okay() const
00093 {
00094   return okay_;
00095 }
00096 
00097 Pose Node::transformToWorld(const Pose &pose, const string &pose_frame_id) const
00098 {
00099   // get the transform from the frame origin to the world frame
00100   const worldlib::geometry::Pose p_frame_world(tfs_.lookupTransform(world_.getFixedFrameID(), pose_frame_id,
00101                                                                     ros::Time(0)).transform);
00102   // multiply to get the pose in the world frame
00103   const tf2::Transform t_pose_frame = pose.toTF2Transform();
00104   const tf2::Transform t_frame_world = p_frame_world.toTF2Transform();
00105   const worldlib::geometry::Pose p_pose_world(t_frame_world * t_pose_frame);
00106   return p_pose_world;
00107 }


worldlib
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:36