1. OpenSourceRoboticsFoundation
  2. Untitled project
  3. gazebo

Commits

"Nat...@gmail.com>"  committed 235b9b4

New gz command line tool, part 1

  • Participants
  • Parent commits d46ac06
  • Branches gz_tool_part1

Comments (0)

Files changed (11)

File gazebo/transport/ConnectionManager.cc

View file
 
 //////////////////////////////////////////////////
 bool ConnectionManager::Init(const std::string &_masterHost,
-                             unsigned int master_port,
+    unsigned int _masterPort)
+{
+  return this->Init(_masterHost, _masterPort, 30);
+}
+
+//////////////////////////////////////////////////
+bool ConnectionManager::Init(const std::string &_masterHost,
+                             unsigned int _masterPort,
                              uint32_t _timeoutIterations)
 {
   this->stop = false;
   uint32_t timeoutCount = 0;
   uint32_t waitDurationMS = 1000;
 
-  while (!this->masterConn->Connect(_masterHost, master_port) &&
+  while (!this->masterConn->Connect(_masterHost, _masterPort) &&
       this->IsRunning() && timeoutCount < _timeoutIterations)
   {
     if (!common::Console::Instance()->GetQuiet())

File gazebo/transport/ConnectionManager.hh

View file
       /// \brief Destructor
       private: virtual ~ConnectionManager();
 
+      // \todo Deprecation: This function should be removed in Gazebo 3.0.
+      // Default the _timeoutIterations to 30 in the other Init function. 
+      /// \brief Initialize the connection manager. This will call
+      /// ConnectionManager::Init with  _timeoutIterations==30.
+      /// \param[in] _masterHost Host where the master is running
+      /// \param[in] _masterPort Port where the master is running
+      /// \return true if initialization succeeded, false otherwise
+      public: bool Init(const std::string &_masterHost,
+                        unsigned int _masterPort);
+
       /// \brief Initialize the connection manager
       /// \param[in] _masterHost Host where the master is running
       /// \param[in] _masterPort Port where the master is running
       /// \return true if initialization succeeded, false otherwise
       public: bool Init(const std::string &_masterHost,
                         unsigned int _masterPort,
-                        uint32_t _timeoutIterations = 30);
+                        uint32_t _timeoutIterations);
 
       /// \brief Run the connection manager loop.  Does not return until
       /// stopped.

File gazebo/transport/Publication.cc

View file
 
   {
     boost::mutex::scoped_lock lock(this->nodeMutex);
-
     endIter = this->nodes.end();
     iter = std::find(this->nodes.begin(), this->nodes.end(), _node);
   }
 
     boost::mutex::scoped_lock lock(this->callbackMutex);
 
+    // Send latched messages to the subscription.
     for (std::map<uint32_t, MessagePtr>::iterator pubIter =
         this->prevMsgs.begin(); pubIter != this->prevMsgs.end(); ++pubIter)
     {
 
     if (_callback->GetLatching())
     {
+      // Send latached messages to the subscription.
       for (std::map<uint32_t, MessagePtr>::iterator pubIter =
           this->prevMsgs.begin(); pubIter != this->prevMsgs.end(); ++pubIter)
       {
 }
 
 //////////////////////////////////////////////////
-int Publication::Publish(MessagePtr _msg, boost::function<void(uint32_t)> _cb,
+void Publication::Publish(MessagePtr _msg, boost::function<void(uint32_t)> _cb,
+    uint32_t _id)
+{
+  this->Publish_(_msg, _cb, _id);
+}
+
+//////////////////////////////////////////////////
+int Publication::Publish_(MessagePtr _msg, boost::function<void(uint32_t)> _cb,
     uint32_t _id)
 {
   int result = 0;
     }
   }
 }
+
+//////////////////////////////////////////////////
+MessagePtr Publication::GetPrevMsg(uint32_t _pubId)
+{
+  if (this->prevMsgs.find(_pubId) != this->prevMsgs.end())
+    return this->prevMsgs[_pubId];
+  else
+    return MessagePtr();
+}

File gazebo/transport/Publication.hh

View file
       /// \param[in] _msg Message to be published
       /// \param[in] _cb Callback to be invoked after publishing
       /// is completed
+      // \todo Deprecation: This function should be removed in Gazebo 3.0,
+      // copy Publish_ to Publish, and then deprecate Publish_.
+      public: void Publish(MessagePtr _msg,
+                  boost::function<void(uint32_t)> _cb,
+                  uint32_t _id);
+
+      /// \brief Publish data to remote subscribers
+      /// \param[in] _msg Message to be published
+      /// \param[in] _cb Callback to be invoked after publishing
+      /// is completed
       /// \return Number of remote subscribers that will receive the
       /// message.
-      public: int Publish(MessagePtr _msg,
+      public: int Publish_(MessagePtr _msg,
                   boost::function<void(uint32_t)> _cb,
                   uint32_t _id);
 
       /// \param[in,out] _pub Pointer to publisher object to be added
       public: void AddPublisher(PublisherPtr _pub);
 
-      /// \brief Remove a publisher
-      /// \param[in] _pub Pointer to publisher object to remove
+      /// \brief Remove a publisher.
+      /// \param[in] _pub Pointer to publisher object to remove.
       public: void RemovePublisher(PublisherPtr _pub);
 
       /// \brief Set the previous message for a publisher.
       /// \param[in] _msg The previous message.
       public: void SetPrevMsg(uint32_t _pubId, MessagePtr _msg);
 
+      /// \brief Get a previous message for a publisher.
+      /// \param[in] _pubId ID of the publisher.
+      /// \return Pointer to the previous message. NULL if there is no
+      /// previous message.
+      public: MessagePtr GetPrevMsg(uint32_t _pubId);
+
       /// \brief Remove nodes that have been marked for removal
       private: void RemoveNodes();
 

File gazebo/transport/Publisher.cc

View file
         iter != localBuffer.end(); ++iter, ++pubIter)
     {
       // Send the latest message.
-      this->pubIds[*pubIter] = this->publication->Publish(*iter,
+      this->pubIds[*pubIter] = this->publication->Publish_(*iter,
           boost::bind(&Publisher::OnPublishComplete, this, _1), *pubIter);
 
       if (this->pubIds[*pubIter] <= 0)
 
   this->node.reset();
 }
+
+//////////////////////////////////////////////////
+std::string Publisher::GetPrevMsg() const
+{
+  std::string result;
+  if (this->publication)
+  {
+    MessagePtr msg = this->publication->GetPrevMsg(this->id);
+    if (msg)
+      msg->SerializeToString(&result);
+  }
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+MessagePtr Publisher::GetPrevMsgPtr() const
+{
+  if (this->publication)
+    return this->publication->GetPrevMsg(this->id);
+  else
+    return MessagePtr();
+}

File gazebo/transport/Publisher.hh

View file
       public: void Fini();
 
       /// \brief Implementation of Publish.
-      /// \param[in] _message Message to be published
+      /// \param[in] _message Message to be published.
       /// \param[in] _block Whether to block until the message is actually
-      /// written out
+      /// written out.
       private: void PublishImpl(const google::protobuf::Message &_message,
                                 bool _block);
 
       /// \brief Current id of the sent message.
       private: uint32_t pubId;
 
-      /// \brief Current publication ids
+      /// \brief Current publication ids.
       private: std::map<uint32_t, int> pubIds;
 
       /// \brief Unique ID for this publisher.

File gazebo/transport/TopicManager.cc

View file
 {
   GZ_ASSERT(_pub, "Unadvertising a NULL Publisher");
 
-  PublicationPtr publication = this->FindPublication(_pub->GetTopic());
-  if (publication)
-    publication->RemovePublisher(_pub);
+  if (_pub)
+  {
+    PublicationPtr publication = this->FindPublication(_pub->GetTopic());
+    if (publication)
+      publication->RemovePublisher(_pub);
 
-  this->Unadvertise(_pub->GetTopic());
+    this->Unadvertise(_pub->GetTopic());
+  }
 }
 
 //////////////////////////////////////////////////

File gazebo/transport/TopicManager.hh

View file
       /// \param[in] _topic The topic to be unadvertised
       public: void Unadvertise(const std::string &_topic);
 
-      /// \brief Unadvertise a publisher
-      /// \param[in] _pub Publisher to unadvertise
+      /// \brief Unadvertise a publisher.
+      /// \param[in] _pub Publisher to unadvertise.
       public: void Unadvertise(PublisherPtr _pub);
 
       /// \brief Send a message. Use a Publisher instead of calling this

File gazebo/transport/TransportIface.cc

View file
 
   return true;
 }
+/////////////////////////////////////////////////
+bool transport::init(const std::string &_masterHost, unsigned int _masterPort)
+{
+  return init(_masterHost, _masterPort, 30);
+}
 
 /////////////////////////////////////////////////
 bool transport::init(const std::string &_masterHost, unsigned int _masterPort,

File gazebo/transport/TransportIface.hh

View file
     bool get_master_uri(std::string &_master_host, unsigned int &_master_port);
 
     /// \brief Initialize the transport system
-    /// \param[in] _master_host The hostname or IP of the master. Leave empty to
-    ///            use pull address from the GAZEBO_MASTER_URI env var.
-    /// \param[in] _master_port The port  of the master. Leave empty to
-    ///            use pull address from the GAZEBO_MASTER_URI env var.
+    /// \param[in] _masterHost The hostname or IP of the master. Leave empty to
+    /// use pull address from the GAZEBO_MASTER_URI env var.
+    /// \param[in] _masterPort The port  of the master. Leave empty to
+    /// use pull address from the GAZEBO_MASTER_URI env var.
     /// \param[in] _timeoutIterations Number of times to wait for
-    ///            a connection to master.
+    /// a connection to master.
     /// \return true if initialization succeeded; false otherwise
-    bool init(const std::string &_master_host ="",
-              unsigned int _master_port = 0,
-              uint32_t _timeoutIterations = 30);
+    // \todo Deprecation: Remove this function in Gazebo 3.0. Set the
+    // defaults of the other init function to "", 0, 30.
+    bool init(const std::string &_masterHost = "",
+              unsigned int _masterPort = 0);
+
+    /// \brief Initialize the transport system
+    /// \param[in] _masterHost The hostname or IP of the master. Leave empty to
+    /// use pull address from the GAZEBO_MASTER_URI env var.
+    /// \param[in] _masterPort The port  of the master. Leave empty to
+    /// use pull address from the GAZEBO_MASTER_URI env var.
+    /// \param[in] _timeoutIterations Number of times to wait for
+    /// a connection to master.
+    /// \return true if initialization succeeded; false otherwise
+    bool init(const std::string &_masterHost,
+              unsigned int _masterPort,
+              uint32_t _timeoutIterations);
 
     /// \brief Run the transport component. Creates a thread to handle
     /// message passing. This call will block until the master can

File test/integration/transport.cc

View file
 bool g_sceneMsg = false;
 bool g_worldStatsMsg = false;
 bool g_worldStatsDebugMsg = false;
+bool g_stringMsg = false;
+bool g_stringMsg2 = false;
+bool g_stringMsg3 = false;
+bool g_stringMsg4 = false;
 
 void ReceiveStringMsg(ConstGzStringPtr &/*_msg*/)
 {
+  printf("1\n");
+  g_stringMsg = true;
+}
+
+void ReceiveStringMsg2(ConstGzStringPtr &/*_msg*/)
+{
+  printf("2\n");
+  g_stringMsg2 = true;
+}
+
+void ReceiveStringMsg3(ConstGzStringPtr &/*_msg*/)
+{
+  printf("3\n");
+  g_stringMsg3 = true;
+}
+
+void ReceiveStringMsg4(ConstGzStringPtr &/*_msg*/)
+{
+  printf("4\n");
+  g_stringMsg4 = true;
 }
 
 void ReceiveSceneMsg(ConstScenePtr &/*_msg*/)
   g_worldStatsDebugMsg = true;
 }
 
-
+/*
+/////////////////////////////////////////////////
 TEST_F(TransportTest, Load)
 {
   for (unsigned int i = 0; i < 2; ++i)
   }
 }
 
+/////////////////////////////////////////////////
+// Standard pub/sub
 TEST_F(TransportTest, PubSub)
 {
   Load("worlds/empty.world");
   subs.clear();
 }
 
+/////////////////////////////////////////////////
+// Test error cases
 TEST_F(TransportTest, Errors)
 {
   Load("worlds/empty.world");
   testNode.reset();
 }
 
+void SinglePub()
+{
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
+
+  msgs::GzString msg;
+  msg.set_data("Child process sending message.");
+  pub->Publish(msg);
+}
+
+
+
+/////////////////////////////////////////////////
 // This test creates a child process to test interprocess communication
-// TODO: This test needs to be fixed
-/*TEST_F(TransportTest, Processes)
+TEST_F(TransportTest, ThreadedSinglePubSub)
 {
+  g_stringMsg = false;
+
+  Load("worlds/empty.world");
+
+
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  transport::SubscriberPtr sub =
+    node->Subscribe("~/test", &ReceiveStringMsg, true);
+
+  EXPECT_STREQ("gazebo.msgs.GzString",
+      node->GetMsgType("/gazebo/default/test").c_str());
+
+  boost::thread *thread = new boost::thread(boost::bind(&SinglePub));
+
+  for (int i = 0; i < 10 && !g_stringMsg; ++i)
+    common::Time::MSleep(100);
+
+  EXPECT_TRUE(g_stringMsg);
+
+  thread->join();
+}
+
+/////////////////////////////////////////////////
+TEST_F(TransportTest, ThreadedMultiSubSinglePub)
+{
+  g_stringMsg = false;
+  g_stringMsg2 = false;
+
+  Load("worlds/empty.world");
+
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  transport::SubscriberPtr sub =
+    node->Subscribe("~/test", &ReceiveStringMsg, true);
+
+  transport::SubscriberPtr sub2 =
+    node->Subscribe("~/test", &ReceiveStringMsg2, true);
+
+  EXPECT_STREQ("gazebo.msgs.GzString",
+      node->GetMsgType("/gazebo/default/test").c_str());
+
+  boost::thread *thread = new boost::thread(boost::bind(&SinglePub));
+
+  for (int i = 0; i < 10 && !g_stringMsg && !g_stringMsg2; ++i)
+    common::Time::MSleep(100);
+
+  EXPECT_TRUE(g_stringMsg);
+
+  thread->join();
+}
+
+/////////////////////////////////////////////////
+void MultiPub()
+{
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
+  transport::PublisherPtr pub2 = node->Advertise<msgs::GzString>("~/test");
+
+  msgs::GzString msg;
+  msg.set_data("Child process sending message.");
+  pub->Publish(msg);
+  pub2->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+TEST_F(TransportTest, ThreadedMultiPubSub)
+{
+  g_stringMsg = false;
+  g_stringMsg2 = false;
+
+  Load("worlds/empty.world");
+
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  transport::SubscriberPtr sub =
+    node->Subscribe("~/test", &ReceiveStringMsg, true);
+
+  transport::SubscriberPtr sub2 =
+    node->Subscribe("~/test", &ReceiveStringMsg2, true);
+
+  EXPECT_STREQ("gazebo.msgs.GzString",
+      node->GetMsgType("/gazebo/default/test").c_str());
+
+  boost::thread *thread = new boost::thread(boost::bind(&MultiPub));
+
+  for (int i = 0; i < 10 && !g_stringMsg && !g_stringMsg2; ++i)
+    common::Time::MSleep(100);
+
+  EXPECT_TRUE(g_stringMsg);
+
+  thread->join();
+}
+*/
+
+/////////////////////////////////////////////////
+void MultiPubSub()
+{
+  transport::NodePtr node(new transport::Node());
+  node->Init("default");
+
+  //transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
+  //transport::PublisherPtr pub2 = node->Advertise<msgs::GzString>("~/test");
+
+  transport::SubscriberPtr sub =
+    node->Subscribe("~/testO", &ReceiveStringMsg3, true);
+
+  //transport::SubscriberPtr sub2 =
+  //  node->Subscribe("~/testO", &ReceiveStringMsg4, true);
+
+  //EXPECT_STREQ("gazebo.msgs.GzString",
+  //    node->GetMsgType("/gazebo/default/testO").c_str());
+
+  //msgs::GzString msg;
+  //msg.set_data("Child process sending message.");
+  //pub->Publish(msg);
+  //pub2->Publish(msg);
+
+  for (int i = 0; i < 10 && !g_stringMsg3; ++i)// && !g_stringMsg4; ++i)
+    common::Time::MSleep(100);
+
+  EXPECT_TRUE(g_stringMsg3);
+  //EXPECT_TRUE(g_stringMsg4);
+}
+
+/////////////////////////////////////////////////
+TEST_F(TransportTest, ThreadedMultiPubSubBidirectional)
+{
+  g_stringMsg = false;
+  g_stringMsg2 = false;
+  g_stringMsg3 = false;
+  g_stringMsg4 = false;
+
+  Load("worlds/empty.world");
+
+  transport::NodePtr node(new transport::Node());
+  node->Init("default");
+
+  // transport::SubscriberPtr sub =
+  //   node->Subscribe("~/test", &ReceiveStringMsg, true);
+
+  // transport::SubscriberPtr sub2 =
+  //   node->Subscribe("~/test", &ReceiveStringMsg2, true);
+
+
+  transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/testO");
+  //transport::PublisherPtr pub2 = node->Advertise<msgs::GzString>("~/testO");
+  //pub->WaitForConnection();
+  //pub2->WaitForConnection();
+
+  // EXPECT_STREQ("gazebo.msgs.GzString",
+  //     node->GetMsgType("/gazebo/default/test").c_str());
+
+  msgs::GzString msg;
+  msg.set_data("Parent send message");
+  pub->Publish(msg);
+  //pub2->Publish(msg);
+
+  boost::thread *thread = new boost::thread(boost::bind(&MultiPubSub));
+
+  // for (int i = 0; i < 10 && !g_stringMsg && !g_stringMsg2; ++i)
+  //   common::Time::MSleep(100);
+
+  // EXPECT_TRUE(g_stringMsg);
+  // EXPECT_TRUE(g_stringMsg2);
+
+  thread->join();
+}
+
+/////////////////////////////////////////////////
+// Test the case of a programe that sends a messages and then immediatly
+// stops.
+/*TEST_F(TransportTest, ImmediateEnd)
+{
+  g_stringMsg = false;
+
+  transport::init();
+  transport::run();
+
+  transport::NodePtr node(new transport::Node());
+  node->Init();
+
+  // Subscribe to the 'test' topic
+  transport::SubscriberPtr sub =
+    node->Subscribe("~/test", &ReceiveStringMsg, true);
+
   pid_t pid = fork();
   if (pid == 0)
   {
-    common::Time::MSleep(1);
-    transport::init();
-    transport::run();
+    // Child process advertises on the 'test' topic
+    transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
+    msgs::GzString msg;
 
-    transport::NodePtr node(new transport::Node());
-    node->Init();
-
-    transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
-
-    transport::SubscriberPtr sub =
-      node->Subscribe("~/world_stats", &ReceiveWorldStatsMsg2);
-    transport::SubscriberPtr sub2 =
-      node->Subscribe("~/test", &ReceiveStringMsg, true);
-
-    transport::PublisherPtr pub2 = node->Advertise<msgs::GzString>("~/test");
-
-    EXPECT_STREQ("gazebo.msgs.WorldStatistics",
-                 node->GetMsgType("/gazebo/default/world_stats").c_str());
-
-    msgs::GzString msg;
-    msg.set_data("Waiting for message");
+    // Send a message
+    msg.set_data("Child process sent message.");
     pub->Publish(msg);
-    pub2->Publish(msg);
-
-    int i = 0;
-    while (!g_worldStatsMsg2 && i < 20)
+  }
+  else
+  {
+    // Prarent processe waits for the message to arrive
+    for (int i = 0; i < 10 && !g_stringMsg; ++i)
     {
       common::Time::MSleep(100);
-      ++i;
     }
-    EXPECT_LT(i, 20);
-
-    pub.reset();
-    sub.reset();
-    node.reset();
-    transport::fini();
-    common::Time::MSleep(5);
-  }
-  else if (pid < 0)
-    printf("Fork failed\n");
-  else
-  {
-    Load("worlds/empty.world");
-
-    transport::NodePtr node(new transport::Node());
-    node->Init();
-
-    transport::PublisherPtr pub = node->Advertise<msgs::GzString>("~/test");
-    transport::SubscriberPtr sub =
-      node->Subscribe("~/test", &ReceiveStringMsg, true);
-
-    transport::PublisherPtr pub2 = node->Advertise<msgs::GzString>("~/test");
-    transport::SubscriberPtr sub2 =
-      node->Subscribe("~/test", &ReceiveStringMsg, true);
-
-    EXPECT_STREQ("gazebo.msgs.String",
-                 node->GetMsgType("/gazebo/default/test").c_str());
-
-    msgs::GzString msg;
-    msg.set_data("Waiting for message");
-    pub->Publish(msg);
-    pub2->Publish(msg);
-
-    for (int i = 0; i < 5; ++i)
-      common::Time::MSleep(100);
-
-    sub.reset();
-    sub2.reset();
-    kill(pid, SIGKILL);
+    EXPECT_TRUE(g_stringMsg);
   }
 }*/
 
+
+/////////////////////////////////////////////////
+// Main
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);