SMORES Robot Platform Simulation
Modlab at Penn, ASL at Cornell
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends
gazebo::WorldServer Class Reference
Inheritance diagram for gazebo::WorldServer:
gazebo::ConfigEditor gazebo::GaitRecorder gazebo::WorldController

Public Member Functions

virtual void ExtraInitializationInLoad (physics::WorldPtr _parent, sdf::ElementPtr _sdf)
 This function will be called in Load() to perform extra initialization.
 
LibraryTemplateDynamicallyLoadedLibrary (const char *library_path, void *lib_handle)
 Load a shared library in linux.
 
void CloseLoadedLibrary (void **lib_handle)
 Close the loaded libraries.
 
virtual void OnSystemRunningExtra (const common::UpdateInfo &_info)
 
virtual void InsertModel (string name, math::Pose position, string joint_angles)
 Insert a model to the current world. More...
 
virtual void InsertModel (string name, math::Pose position, string joint_angles, string model_path)
 Insert a model to the current world. More...
 
void AddInitialPosition (math::Pose position)
 Add a new position to set at the end of 'intialPosition' vector.
 
void AddInitialJoints (string joint_angles)
 Add new initial joint values to set at the end of 'initalJointValue' vector.
 
void DeleteSmoresmodulePtr (string module_name)
 Delete the smoresmodule object in the world. More...
 
void DeleteModule (string module_name)
 Delete a model that already in the world. More...
 
void DeleteAllModules (void)
 Delete all models that already in the world.
 
math::Vector3 GetCurrentConfigurationPose (void)
 Get the current position in world of the inserted configuration. More...
 
void BuildConfigurationFromXML (string file_name)
 Build a configuration at origin using a XML file.
 
void BuildConfigurationFromXML (string file_name, math::Vector3 initial_pose)
 Build a configuration at given initial_pose using a XML file.
 
void BuildConnectionFromXML (string file_name)
 This function is used to build connection using a XML file.
 
virtual void ExtraWorkWhenModelInserted (CommandMessagePtr &msg)
 This function will be called after set the model initial position.
 
double ConversionForAngleOverPi (double angle)
 Convert angles so that their absolutely value always smaller than Pi.
 
void EnableAutoMagneticConnection (void)
 
void PassiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID, double node_angle, double node_distance)
 Connect two modules by pointers and node_ID.
 
void PassiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID)
 With default angle and distance offset equal to 0.
 
void ActiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID, double node_angle, double node_distance)
 TODO: Need add 'active' feature to this function.
 
void ActiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID)
 With default angle and distance offset equal to 0.
 
void Disconnect (SmoresEdgePtr aEdge)
 
void Disconnect (SmoresModulePtr aModule, int node_ID)
 Disconnect two module base on one module and one node of that module.
 
void Disconnect (string moduleName, int node_ID)
 Disconnect two module base on one module and one node of that module.
 
void Disconnect (string moduleName1, string moduleName2)
 Disconnect two module base on their names.
 
void SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, unsigned int time_stamp, string condition_str, string dependency_str)
 Send time based common gait with condition and dependency. More...
 
void SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, unsigned int time_stamp)
 Send time based common gait without condition and dependency. More...
 
void SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, string condition_str, string dependency_str)
 Send common gait with condition and dependency. More...
 
void SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type)
 Send common gait without condition and dependency. More...
 
void SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, unsigned int time_stamp, string condition_str, string dependency_str)
 Send gait table to change a single joint of a module. More...
 
void SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, unsigned int time_stamp)
 Send gait table to change a single joint of a module. More...
 
void SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, string condition_str, string dependency_str)
 Send gait table to change a single joint of a module. More...
 
void SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type)
 Send gait table to change a single joint of a module. More...
 
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)
 Used in the direct driving situation, ignore execution order. More...
 
void SendGaitTableInstance (SmoresModulePtr module, const int *flag, const double *gait_value)
 Used in the direct driving situation with default type 4. More...
 
void SendPositionInstance (SmoresModulePtr module, double x, double y, double orientation_angle)
 Used in direct control, dirve a module to a specific position. More...
 
void EraseComaands (SmoresModulePtr module)
 
void DynamicJointDestroy (SmoresEdgePtr aEdge)
 
void ReBuildDynamicJoint (SmoresEdgePtr a_edge)
 Rebuild the dynamic joint using the information in the edge object.
 
int GetNodeIDByName (string node_name)
 
bool CheckModuleExistByName (string module_name)
 Get return true if the smores module with given name exist in the world.
 
SmoresModulePtr GetModulePtrByName (string module_name)
 Get SmoresModule object by specifying the name. More...
 
SmoresModulePtr GetModulePtrByIDX (unsigned int idx)
 Get SmoresModule object by specifying the index in the vector moduleList.
 
unsigned int GetModuleListSize (void)
 Get the count of the modules that are in the list.
 
void EraseCommandPtrByModule (SmoresModulePtr module_ptr)
 
int GetModuleIndexByName (string module_name)
 
bool AlreadyConnected (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID)
 Check whether two nodes are connected together.
 
bool AlreadyConnected (SmoresModulePtr module_1, SmoresModulePtr module_2)
 Check whether two modules have already connected on a node.
 
bool AlreadyConnected (SmoresModulePtr module, int node_ID)
 Check whether a node a of module has been occupied.
 
void ReadFileAndGenerateCommands (const char *fileName)
 Read a 'gait table' stored in a text file.
 
void FigureInterpret (const vector< string > *joints_spec, int *type_flags, double *joint_values)
 Used to interpret the number in gait table. More...
 
void InterpretCommonGaitString (string a_command_str)
 Interpret normal command string. More...
 
void InterpretSpecialString (string a_command_str)
 Interpret special command. More...
 
unsigned int CountModules (SmoresModulePtr module)
 Count how many modules are there in a cluster with the current module. More...
 
unsigned int GetInitialJointSequenceSize (void)
 Get the length of the initial joint value setting sequence.
 
unsigned int GetEdgeCounts (void)
 Get the total counts of the edges that stored in the program.
 
SmoresEdgePtr GetEdgePtrByIDX (unsigned int idx)
 Get SmoresEdge object by specifying the index in the vector connectionEdges.
 

Public Attributes

physics::WorldPtr currentWorld
 

Member Function Documentation

unsigned int gazebo::WorldServer::CountModules ( SmoresModulePtr  module)

Count how many modules are there in a cluster with the current module.

Parameters
moduleA pointer of SmoresModule object
void gazebo::WorldServer::DeleteModule ( string  module_name)

Delete a model that already in the world.

Parameters
module_nameModule name string
void gazebo::WorldServer::DeleteSmoresmodulePtr ( string  module_name)

Delete the smoresmodule object in the world.

Parameters
module_nameModule name string
void gazebo::WorldServer::Disconnect ( SmoresEdgePtr  aEdge)

Disconnect two modules on one edge TODO: This pointer must point to an element in the vector

void gazebo::WorldServer::DynamicJointDestroy ( SmoresEdgePtr  aEdge)

Destroyer of the connection between different modules, which is dynamic joint here

void gazebo::WorldServer::EnableAutoMagneticConnection ( void  )

Enable auto magnetic connection, all module Have to be called in Load() Default: disabled

void gazebo::WorldServer::EraseComaands ( SmoresModulePtr  module)

Erase all the existing commands of a specific module TODO: Need to be tested

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

Used to interpret the number in gait table.

Parameters
joints_specString tokens of the joint specs
type_flagsJoint control flag, see SendGaitTable (return)
joint_valuesJoint control values (return)

TODO: This is at a very low API level of SGST need to implement all the features in the future

math::Vector3 gazebo::WorldServer::GetCurrentConfigurationPose ( void  )

Get the current position in world of the inserted configuration.

Averaging the pose of all modules return

SmoresModulePtr gazebo::WorldServer::GetModulePtrByName ( string  module_name)

Get SmoresModule object by specifying the name.

Parameters
module_nameModule name string
Returns
A pointer of SmoresModule object
void gazebo::WorldServer::InsertModel ( string  name,
math::Pose  position,
string  joint_angles 
)
virtual

Insert a model to the current world.

With joint angles specified

Reimplemented in gazebo::ConfigEditor.

void gazebo::WorldServer::InsertModel ( string  name,
math::Pose  position,
string  joint_angles,
string  model_path 
)
virtual

Insert a model to the current world.

With joint angles and model path specified

Reimplemented in gazebo::ConfigEditor.

void gazebo::WorldServer::InterpretCommonGaitString ( string  a_command_str)

Interpret normal command string.

Parameters
a_command_strA command string
void gazebo::WorldServer::InterpretSpecialString ( string  a_command_str)

Interpret special command.

Parameters
a_command_strA command string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value,
int  msg_type,
unsigned int  time_stamp,
string  condition_str,
string  dependency_str 
)

Send time based common gait with condition and dependency.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
msg_typeDefined in command_message.proto, 3 for joint control
time_stampTime based gait table timer
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value,
int  msg_type,
unsigned int  time_stamp 
)

Send time based common gait without condition and dependency.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
msg_typeDefined in command_message.proto, 3 for joint control
time_stampTime based gait table timer
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value,
int  msg_type,
string  condition_str,
string  dependency_str 
)

Send common gait with condition and dependency.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
msg_typeDefined in command_message.proto, 3 for joint control
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value,
int  msg_type 
)

Send common gait without condition and dependency.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
msg_typeDefined in command_message.proto, 3 for joint control
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
int  joint_ID,
double  gait_value,
int  msg_type,
unsigned int  time_stamp,
string  condition_str,
string  dependency_str 
)

Send gait table to change a single joint of a module.

Parameters
moduleA pointer of SmoresModule object
joint_IDNumber index of a joint
gait_valueJoint value, either be position, speed or torque
msg_type0: position; 1: speed; 2: torque(Not implemented); 3: ignore;
time_stampTime based gait table timer
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
int  joint_ID,
double  gait_value,
int  msg_type,
unsigned int  time_stamp 
)

Send gait table to change a single joint of a module.

Parameters
moduleA pointer of SmoresModule object
joint_IDNumber index of a joint
gait_valueJoint value, either be position, speed or torque
msg_type0: position; 1: speed; 2: torque(Not implemented); 3: ignore;
time_stampTime based gait table timer
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
int  joint_ID,
double  gait_value,
int  msg_type,
string  condition_str,
string  dependency_str 
)

Send gait table to change a single joint of a module.

Parameters
moduleA pointer of SmoresModule object
joint_IDNumber index of a joint
gait_valueJoint value, either be position, speed or torque
msg_type0: position; 1: speed; 2: torque(Not implemented); 3: ignore;
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
int  joint_ID,
double  gait_value,
int  msg_type 
)

Send gait table to change a single joint of a module.

Parameters
moduleA pointer of SmoresModule object
joint_IDNumber index of a joint
gait_valueJoint value, either be position, speed or torque
msg_type0: position; 1: speed; 2: torque(Not implemented); 3: ignore;
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
string  module1,
string  module2,
int  node1,
int  node2,
int  commandtype,
unsigned int  time_stamp,
string  condition_str,
string  dependency_str 
)

Send connect or disconnect command, with condition or dependency and time based

Parameters
moduleA pointer of SmoresModule object of module 1
module1Name string of module 1
module2Name string of module 2
node1Number index of the joint of module 1
node2Number index of the joint of module 2
commandtypeSpecial command indicator, 1 connect; 2 disconnect
time_stampTime based gait table timer
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
string  module1,
string  module2,
int  node1,
int  node2,
int  commandtype,
unsigned int  time_stamp 
)

Send connect or disconnect command, without condition or dependency and time based

Parameters
moduleA pointer of SmoresModule object of module 1
module1Name string of module 1
module2Name string of module 2
node1Number index of the joint of module 1
node2Number index of the joint of module 2
commandtypeSpecial command indicator, 1 connect; 2 disconnect
time_stampTime based gait table timer
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
string  module1,
string  module2,
int  node1,
int  node2,
int  commandtype,
string  condition_str,
string  dependency_str 
)

Send connect or disconnect command, with condition or dependency

Parameters
moduleA pointer of SmoresModule object of module 1
module1Name string of module 1
module2Name string of module 2
node1Number index of the joint of module 1
node2Number index of the joint of module 2
commandtypeSpecial command indicator, 1 connect; 2 disconnect
condition_strCondition string
dependency_strDependency string
void gazebo::WorldServer::SendGaitTable ( SmoresModulePtr  module,
string  module1,
string  module2,
int  node1,
int  node2,
int  commandtype 
)

Send connect or disconnect command, without condition or dependency

Parameters
moduleA pointer of SmoresModule object of module 1
module1Name string of module 1
module2Name string of module 2
node1Number index of the joint of module 1
node2Number index of the joint of module 2
commandtypeSpecial command indicator, 1 connect; 2 disconnect
void gazebo::WorldServer::SendGaitTableInstance ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value,
int  msg_type 
)

Used in the direct driving situation, ignore execution order.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
msg_typeDefined in command_message.proto, 4 for direct drive
void gazebo::WorldServer::SendGaitTableInstance ( SmoresModulePtr  module,
const int *  flag,
const double *  gait_value 
)

Used in the direct driving situation with default type 4.

Parameters
moduleA pointer of SmoresModule object
flagMessage type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented)
gait_valueJoints values, either be position, speed or torque
void gazebo::WorldServer::SendPositionInstance ( SmoresModulePtr  module,
double  x,
double  y,
double  orientation_angle 
)

Used in direct control, dirve a module to a specific position.

Parameters
moduleA pointer of SmoresModule object
x2D coordinate x
y2D coordinate y
orientation_angleFinal orientation of the module in global frame

The documentation for this class was generated from the following files: