WorldServer.hh

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Author: Edward Yunkai Cui
// Description: This is a world plugin template for this simulation that
//              provides an interface to serve as a middle ware. The main 
//              functions of this plugin are managing magnetic connections,
//              loading configurations, interpreting gait commands, managing 
//              and routing communications, providing APIs to manage the
//              shared libraries, managing module configurations, etc.
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#ifndef _GAZEBO_WORLD_SERVER_HH_
#define _GAZEBO_WORLD_SERVER_HH_
#ifndef _GAZEBO_CUTOMIZED_WORLD_PLUGIN
#define _GAZEBO_CUTOMIZED_WORLD_PLUGIN WorldServer
#endif
#include <stdlib.h>

#include <string>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <fstream>
#include <sstream>
#include <iterator>
#include <boost/bind.hpp>
#include <queue>
#include <unistd.h>

#include <sdf/sdf.hh>
#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"
// Libraries for messages needed to use to communicate between plugins
#include "collision_message.pb.h"
#include "command_message.pb.h"
#include "world_status_message.pb.h"
// XML paser libraries
#include "rapidxml.hpp"
#include "rapidxml_utils.hpp"
// Libraries for connectivity representation
#include "SmoresModule.hh"
// Other useful classes for storing information
#include "CollisionInformation.hh"
#include "Condition.hh"
// Library for colored log text
#include "ColorLog.hh"
// Libraries for configuration motion feedback control
#include "LibraryTemplate.hh"

#define PI 3.141593   // 3.1411593
#define VALIDCONNECTIONDISUPPER 0.110
#define VALIDCONNECTIONDISLOWER 0.098
// TODO: tests needed to make sure it depende on current execution folder
#define MODULEPATH "SMORE.sdf"
// #define INTIALCONFIGURATION "InitialConfiguration"

using std::string;
using std::vector;

typedef const boost::shared_ptr
    <const collision_message::msgs::CollisionMessage> CollisionMessagePtr;
typedef const boost::shared_ptr
    <const command_message::msgs::CommandMessage> CommandMessagePtr;
typedef boost::shared_ptr<gazebo::Condition> ConditionPtr;
typedef const boost::shared_ptr
    <const command_message::msgs::WorldStatusMessage> WorldStatusMessagePtr;

// TODO: Considering 
namespace {
inline string Int2String(int number)
{
  stringstream ss; //create a stringstream
  ss << number;    //add number to the stream
  return ss.str(); //return a string with the contents of the stream
} // Int2String
} // namespace 
namespace gazebo
{
class WorldServer : public WorldPlugin
{
 public:
  WorldServer();
  ~WorldServer();
  virtual void ExtraInitializationInLoad(physics::WorldPtr _parent, 
      sdf::ElementPtr _sdf);
  LibraryTemplate *DynamicallyLoadedLibrary(const char* library_path, 
      void *lib_handle);
  void CloseLoadedLibrary(void **lib_handle);
  virtual void OnSystemRunningExtra(const common::UpdateInfo & _info);

  virtual void InsertModel(string name, math::Pose position, string joint_angles);

  virtual void InsertModel(string name, math::Pose position, string joint_angles, 
      string model_path);
  void AddInitialPosition(math::Pose position);
  void AddInitialJoints(string joint_angles);

  void DeleteSmoresmodulePtr(string module_name);

  void DeleteModule(string module_name);
  void DeleteAllModules(void);

  math::Vector3 GetCurrentConfigurationPose(void);
  void BuildConfigurationFromXML(string file_name);
  void BuildConfigurationFromXML(string file_name, math::Vector3 initial_pose);
  void BuildConnectionFromXML(string file_name);
  virtual void ExtraWorkWhenModelInserted(CommandMessagePtr &msg);
  double ConversionForAngleOverPi(double angle);
  void EnableAutoMagneticConnection(void);
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // These functions are used to connect or disconnect modules
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  void PassiveConnect(SmoresModulePtr module_1, SmoresModulePtr module_2, 
      int node1_ID, int node2_ID, double node_angle, double node_distance);
  void PassiveConnect(SmoresModulePtr module_1, SmoresModulePtr module_2, 
      int node1_ID, int node2_ID);
  void ActiveConnect(SmoresModulePtr module_1, SmoresModulePtr module_2, 
      int node1_ID, int node2_ID, double node_angle, double node_distance);
  void ActiveConnect(SmoresModulePtr module_1, SmoresModulePtr module_2, 
      int node1_ID, int node2_ID);
  void Disconnect(SmoresEdgePtr aEdge);
  void Disconnect(SmoresModulePtr aModule, int node_ID);
  void Disconnect(string moduleName, int node_ID);
  void Disconnect(string moduleName1, string moduleName2);

  // Theses functions are used to send 'gait table'
  // TODO: Think about how to combine these functions

  void SendGaitTable(SmoresModulePtr module, const int *flag, 
      const double *gait_value, int msg_type, unsigned int time_stamp, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, const int *flag, 
      const double *gait_value, int msg_type, unsigned int time_stamp);

  void SendGaitTable(SmoresModulePtr module, const int *flag, 
      const double *gait_value, int msg_type, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, const int *flag, 
      const double *gait_value, int msg_type);

  void SendGaitTable(SmoresModulePtr module, int joint_ID, 
      double gait_value, int msg_type, unsigned int time_stamp, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, int joint_ID, 
      double gait_value, int msg_type, unsigned int time_stamp);

  void SendGaitTable(SmoresModulePtr module, int joint_ID, 
      double gait_value, int msg_type, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, int joint_ID, 
      double gait_value, int msg_type);

  void SendGaitTable(SmoresModulePtr module, string module1, string module2, 
      int node1, int node2, int commandtype, unsigned int time_stamp, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, string module1, string module2, 
      int node1, int node2, int commandtype, unsigned int time_stamp);

  void SendGaitTable(SmoresModulePtr module, string module1, string module2, 
      int node1, int node2, int commandtype, 
      string condition_str, string dependency_str);

  void SendGaitTable(SmoresModulePtr module, string module1, string module2, 
      int node1, int node2, int commandtype);

  void SendGaitTableInstance(SmoresModulePtr module, const int *flag, 
      const double *gait_value, int msg_type);

  void SendGaitTableInstance(SmoresModulePtr module, const int *flag, 
      const double *gait_value);

  void SendPositionInstance(SmoresModulePtr module, double x, double y, 
      double orientation_angle);
  void EraseComaands(SmoresModulePtr module);
  void DynamicJointDestroy(SmoresEdgePtr aEdge);
  void ReBuildDynamicJoint(SmoresEdgePtr a_edge);

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // These functions are utility functions
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  int GetNodeIDByName(string node_name);
  bool CheckModuleExistByName(string module_name);

  SmoresModulePtr GetModulePtrByName(string module_name);
  SmoresModulePtr GetModulePtrByIDX(unsigned int idx);
  unsigned int GetModuleListSize(void);
  void EraseCommandPtrByModule(SmoresModulePtr module_ptr);
  int GetModuleIndexByName(string module_name);
  bool AlreadyConnected(SmoresModulePtr module_1, SmoresModulePtr module_2, 
      int node1_ID, int node2_ID);
  bool AlreadyConnected(SmoresModulePtr module_1, SmoresModulePtr module_2);
  bool AlreadyConnected(SmoresModulePtr module, int node_ID);
  void ReadFileAndGenerateCommands(const char* fileName);

  void FigureInterpret(const vector<string> *joints_spec, int *type_flags, 
      double *joint_values);

  void InterpretCommonGaitString(string a_command_str);

  void InterpretSpecialString(string a_command_str);

  unsigned int CountModules(SmoresModulePtr module);
  unsigned int GetInitialJointSequenceSize(void);
  unsigned int GetEdgeCounts(void);
  SmoresEdgePtr GetEdgePtrByIDX(unsigned int idx);

 private: 
  void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf);
  void AddEntityToWorld(std::string & _info);
  void OnSystemRunning(const common::UpdateInfo & /*_info*/);
  void FeedBackMessageDecoding(CommandMessagePtr &msg);
  void WorldStatusMessageDecoding(WorldStatusMessagePtr &msg);
  void AutomaticMagneticConnectionManagement(CollisionMessagePtr &msg);
  void ConnectAndDynamicJointGeneration(SmoresModulePtr module_1, 
      SmoresModulePtr module_2, int node1_ID, int node2_ID, 
      SmoresEdgePtr an_edge);
  void RotationQuaternionCalculation(math::Vector3 normal_axis,
    math::Vector3 z_axis_of_link1, math::Vector3 z_axis_of_link2, 
    math::Vector3 first_rotation, math::Vector3 second_rotation,
    math::Quaternion *first_rotation_of_link2, 
    math::Quaternion *second_rotation_of_link2);
  void NewPositionCalculation(SmoresEdgePtr an_edge,
    math::Pose old_pose_of_module1, math::Pose old_pose_of_module2, 
    int node1_ID, int node2_ID, 
    math::Pose *new_pose_of_module1, math::Pose *new_pose_of_module2);
  void CommandManager(void); 
  void CommandExecution(ModuleCommandsPtr current_command_container);
  void AddCondition(string conditionid);
  void FinishOneConditionCommand(string conditionid);
  bool CheckCondition(string conditionid);
  int StripOffTimerInCommandString(string &command_string);
  string StripOffCondition(string &command_string);
  string StripOffDependency(string &command_string);

 public:
  physics::WorldPtr currentWorld;
 private: 
  event::ConnectionPtr addEntityConnection;
  transport::PublisherPtr welcomePub;

  transport::PublisherPtr smoreWorldPub;
  transport::SubscriberPtr smoreWorldSub;
  vector<SmoresModulePtr> moduleList;
  vector<CollisionInformation> pendingRequest;
  vector<physics::JointPtr> dynamicConnections;
  event::ConnectionPtr updateConnection;
  vector<SmoresEdgePtr> connectionEdges;
  vector<string> waitingNameList;
  vector<string> initalJointValue;
  vector<math::Pose> initialPosition;
  vector<ConditionPtr> commandConditions;
  vector<ModuleCommandsPtr> moduleCommandContainer;
// private: vector<transport::PublisherPtr> WorldPublisher;
  vector<transport::SubscriberPtr> WorldColSubscriber;
  bool autoMagneticConnectionFlag;
  string configurationFile;
//+++++++++ testing ++++++++++++++++++++++++++++
};
} // namespace gazebo
#endif