Skip to content

Commit

Permalink
Allow setting a global namespace prefix for ROS Comm (Issue #178) (#180)
Browse files Browse the repository at this point in the history 
* Fix: python print command

* Fix: enable comm in worldComm plugin

* Fix: only update volumes from graphics loop

* Fix: reset didn't always work on all bodies.

* Adding capability to reset world and bodies from world comm obj

* Adding the ability to reset the world or the bodies

* FIX: Resetting the GLFW_VISIBLE flag to show subsequent visible windows

* Issue #163. Setting activeControllerType to Force.

* Fix: For rigid body's Position command, use the COM Transform.

* Reset sensors communication message

* Should address #178
  • Loading branch information
@adnanmunawar
adnanmunawar committed Jul 18, 2022
1 parent 54eb94f commit cd35139
Show file tree
Hide file tree
Showing 4 changed files with 36 additions and 49 deletions.
52 changes: 20 additions & 32 deletions ambf_framework/afFramework.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ int afCamera::s_windowIdx = 0;
bool afComm::s_globalOverride = false;
int afComm::s_maxFreq = 1000;
int afComm::s_minFreq = 50;
string afComm::s_global_namespace_prefix = "";

btGhostPairCallback* afGhostObject::m_bulletGhostPairCallback = nullptr;
//------------------------------------------------------------------------------
Expand Down Expand Up @@ -628,6 +629,18 @@ void afComm::overrideMinPublishingFrequency(int freq)
}


///
/// \brief afComm::setGlobalNamespacePrefix
/// \param a_global_namespace_prefix
///
void afComm::setGlobalNamespacePrefix(string a_global_namespace_prefix){
s_global_namespace_prefix = a_global_namespace_prefix;
if (!s_global_namespace_prefix.empty()){
cerr << " INFO! FORCE PREPENDING GLOBAL NAMESPACE PREFIX \"" << s_global_namespace_prefix << "\" \n" ;
}
}



///
/// \brief afCartesianController::afCartesianController
Expand Down Expand Up @@ -898,15 +911,15 @@ string afIdentification::getTypeAsStr(){

string afIdentification::getName(){return m_name;}

string afIdentification::getNamespace(){return m_namespace;}
string afIdentification::getNamespace(){return afComm::getGlobalNamespacePrefix() + m_namespace;}

string afIdentification::getQualifiedName(){return m_namespace + m_name;}
string afIdentification::getQualifiedName(){return getNamespace() + m_name;}

void afIdentification::setName(string a_name){m_name = a_name;}

void afIdentification::setNamespace(string a_namespace){m_namespace = a_namespace; }

string afIdentification::getQualifiedIdentifier(){return m_namespace + m_identifier;}
string afIdentification::getQualifiedIdentifier(){return getNamespace() + m_identifier;}


///
Expand Down Expand Up @@ -3829,10 +3842,10 @@ bool afJoint::createFromAttribs(afJointAttributes *a_attribs)
m_damping = attribs.m_damping; // Initialize damping to 0

// First we should search in the local Model space and if we don't find the body.
// On then we find the world space
// Only then we find the world space

string body1Name = m_namespace + m_parentName;
string body2Name = m_namespace + m_childName;
string body1Name = getNamespace() + m_parentName;
string body2Name = getNamespace() + m_childName;

m_afParentBody = m_modelPtr->getRigidBody(body1Name, true);
m_afChildBody = m_modelPtr->getRigidBody(body2Name, true);
Expand Down Expand Up @@ -5645,7 +5658,7 @@ void afModelManager::addChildsSceneObjectsToWorld(afBaseObjectPtr a_object)
/// \brief afWorld::afWorld
/// \param a_global_namespace
///
afWorld::afWorld(string a_global_namespace): afIdentification(afType::WORLD), afModelManager(this){
afWorld::afWorld(): afIdentification(afType::WORLD), afModelManager(this){
m_maxIterations = 10;

// reset simulation time
Expand Down Expand Up @@ -5714,7 +5727,6 @@ afWorld::afWorld(string a_global_namespace): afIdentification(afType::WORLD), af
m_pickColor.setOrangeTomato();
m_pickColor.setTransparencyLevel(0.3);
m_namespace = "";
setGlobalNamespace(a_global_namespace);
}

afWorld::~afWorld()
Expand Down Expand Up @@ -5822,30 +5834,6 @@ bool afWorld::loadCommunicationPlugin(afWorldPtr a_worldPtr, afWorldAttribsPtr a
}


///
/// \brief afWorld::getFullyQualifiedName
/// \param a_name
/// \return
///
string afWorld::resolveGlobalNamespace(string a_name){
string fully_qualified_name = getGlobalNamespace() + a_name;
fully_qualified_name = afUtils::removeAdjacentBackSlashes(fully_qualified_name);
return fully_qualified_name;
}


///
/// \brief afWorld::setGlobalNamespace
/// \param a_global_namespace
///
void afWorld::setGlobalNamespace(string a_global_namespace){
m_global_namespace = a_global_namespace;
if (!m_global_namespace.empty()){
cerr << " INFO! FORCE PREPENDING GLOBAL NAMESPACE \"" << m_global_namespace << "\" \n" ;
}
}


///
/// \brief afWorld::resetCameras
///
Expand Down
18 changes: 7 additions & 11 deletions ambf_framework/afFramework.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,10 @@ class afComm{
// Override the Min Freq
static void overrideMinPublishingFrequency(int freq);

static string getGlobalNamespacePrefix(){return s_global_namespace_prefix;}

static void setGlobalNamespacePrefix(string a_namespace_prefix);

private:
// Min publishing frequency
uint m_minPubFreq=50;
Expand All @@ -294,6 +298,8 @@ class afComm{
static int s_maxFreq;
static int s_minFreq;

static string s_global_namespace_prefix;

double m_timeStamp = 0.0;
};

Expand Down Expand Up @@ -2105,7 +2111,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{
friend class afModel;

public:
afWorld(string a_global_namespace);
afWorld();

virtual ~afWorld();

Expand Down Expand Up @@ -2206,12 +2212,6 @@ class afWorld: public afIdentification, public afComm, public afModelManager{
// defined in the bodies
void buildCollisionGroups();

string resolveGlobalNamespace(string a_name);

string getGlobalNamespace(){return m_global_namespace;}

void setGlobalNamespace(string a_namespace);

bool pickBody(const cVector3d& rayFromWorld, const cVector3d& rayToWorld);

bool movePickedBody(const cVector3d& rayFromWorld, const cVector3d& rayToWorld);
Expand Down Expand Up @@ -2340,10 +2340,6 @@ class afWorld: public afIdentification, public afComm, public afModelManager{

protected:

// If this string is set, it will force itself to preeced all nampespaces
// regardless of whether any namespace starts with a '/' or not.
string m_global_namespace;

// Current time of simulation.
double m_simulationTime;

Expand Down
2 changes: 1 addition & 1 deletion ambf_ros_modules/ambf_server/include/ambf_server/RosComBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class afROSNode{
if (s_initialized == false){
int argc = 0;
char **argv = 0;
ros::init(argc, argv, "ambf_comm_node");
ros::init(argc, argv, "ambf_comm_node", ros::init_options::AnonymousName);
s_nodePtr = new ros::NodeHandle;
s_initialized = true;
std::cerr << "INFO! INITIALIZING ROS NODE HANDLE\n";
Expand Down
13 changes: 8 additions & 5 deletions ambf_simulator/src/ambf_simulator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ struct CommandLineOptions{
// Control whether to run headless or not
bool showGUI; //
// Override the default world namespace
std::string prepend_namespace;
std::string namespace_prefix;
// The running speed of the simulation. 1.0 indicates a stepping of one second.
double simulation_speed;

Expand Down Expand Up @@ -247,7 +247,7 @@ int main(int argc, char* argv[])
("override_max_comm_freq", p_opt::value<int>(), "Override the maximum publishing frequency for all afObjects (default: 1000 Hz)")
("override_min_comm_freq", p_opt::value<int>(), "Override the minimum publishing frequency for all afObjects (default: 50 Hz)")
("show_gui,g", p_opt::value<bool>()->default_value(true), "Show GUI")
("ns", p_opt::value<std::string>()->default_value(""), "Override the default (or specified in ADF) world namespace")
("ns", p_opt::value<std::string>()->default_value(""), "Global namespace prefix for ROS Communication")
("sim_speed_factor,s", p_opt::value<double>()->default_value(1.0), "Override the speed of \"NON REAL-TIME\" simulation by a specified factor (Default 1.0)")
("plugins,", p_opt::value<std::string>()->default_value(""), "Simulator plugins to load, .e.g. "
"--plugins <plugin1_filepath>, <plugin2_filepath> loads plugin1 and plugin2 simualtor plugin")
Expand All @@ -272,7 +272,7 @@ int main(int argc, char* argv[])
g_cmdOpts.useFixedHtxTimeStep = var_map["fixed_htx_timestep"].as<bool>();
g_cmdOpts.enableForceFeedback = var_map["enableforces"].as<bool>();
g_cmdOpts.showGUI = var_map["show_gui"].as<bool>();
g_cmdOpts.prepend_namespace = var_map["ns"].as<std::string>();
g_cmdOpts.namespace_prefix = var_map["ns"].as<std::string>();
g_cmdOpts.simulation_speed = var_map["sim_speed_factor"].as<double>();
g_cmdOpts.simulator_plugins = var_map["plugins"].as<std::string>();

Expand Down Expand Up @@ -340,7 +340,7 @@ int main(int argc, char* argv[])
glfwSetErrorCallback(errorCallback);

// set OpenGL version
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);

// set active stereo mode
Expand All @@ -354,6 +354,8 @@ int main(int argc, char* argv[])
}
}

cerr << "GLFW VERSION: " << glfwGetVersionString() << endl;


//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
Expand Down Expand Up @@ -395,7 +397,8 @@ int main(int argc, char* argv[])
g_adfLoaderPtr->loadTeleRoboticUnitsAttribs(launchAttribs.m_inputDevicesFilepath.c_str(), &tuAttribs, devIndexes);

// create a dynamic world.
g_afWorld = new afWorld(g_cmdOpts.prepend_namespace);
afComm::setGlobalNamespacePrefix(g_cmdOpts.namespace_prefix);
g_afWorld = new afWorld();
g_afWorld->m_physicsFrequency = g_cmdOpts.phxFrequency;
g_afWorld->m_hapticsFrequency = g_cmdOpts.htxFrequency;
g_afWorld->m_updateCounterLimit = g_cmdOpts.phxFrequency * 2;
Expand Down

0 comments on commit cd35139

Please sign in to comment.