1. OpenSourceRoboticsFoundation
  2. Simulation
  3. gazebo

Commits

Nathan Koenig  committed 41d283c

testing hz_rate

  • Participants
  • Parent commits 20d881b
  • Branches hz_test

Comments (0)

Files changed (5)

File gazebo/sensors/RaySensor.cc

View file
 //////////////////////////////////////////////////
 void RaySensor::UpdateImpl(bool /*_force*/)
 {
+  std::cout << "Ray[" << this->GetName() << "] SimHz[" << 1.0 / (this->world->GetSimTime() - this->lastMeasurementTime).Double() << "] Wallhz[" << 1.0 / (common::Time::GetWallTime() - this->lastWallTime).Double() << "]\n";
+
   // do the collision checks
   // this eventually call OnNewScans, so move mutex lock behind it in case
   // need to move mutex lock after this? or make the OnNewLaserScan connection
   // call somewhere else?
   this->laserShape->Update();
   this->lastMeasurementTime = this->world->GetSimTime();
+  this->lastWallTime = common::Time::GetWallTime();
 
   // moving this behind laserShape update
   boost::mutex::scoped_lock lock(this->mutex);

File gazebo/sensors/RaySensor.hh

View file
       private: transport::PublisherPtr scanPub;
       private: boost::mutex mutex;
       private: msgs::LaserScanStamped laserMsg;
+
+      private: common::Time lastWallTime;
     };
     /// \}
   }

File gazebo/transport/ConnectionManager.cc

View file
   while (!this->stop)
   {
     this->RunUpdate();
-    common::Time::MSleep(30);
+    // common::Time::MSleep(1);
   }
   this->RunUpdate();
 

File sdf/worlds/empty.world

View file
     <include>
       <uri>model://ground_plane</uri>
     </include>
+
+    <!-- Camera 1 -->
+    <include>
+      <uri>model://camera</uri>
+      <name>Camera 1</name>
+    </include>
+
+    <!-- Camera 2 -->
+    <include>
+      <uri>model://camera</uri>
+      <name>Camera 2</name>
+    </include>
+
+    <!-- Camera 3 -->
+    <include>
+      <uri>model://camera</uri>
+      <name>Camera 3</name>
+    </include>
+
+    <!-- Camera 4 -->
+    <include>
+      <uri>model://camera</uri>
+      <name>Camera 4</name>
+    </include>
+
+    <!-- PR2 -->
+    <include>
+      <uri>model://pr2</uri>
+      <pose>5 0 1 0 0 0</pose>
+    </include>
+
+
+
+    <model name="my_hokuyo1">
+      <pose>0 0 1 0 0 0</pose>
+      <link name="link">
+        <visual name="visual">
+          <geometry>
+            <mesh>
+              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <mesh>
+              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
+            </mesh>
+          </geometry>
+        </collision>
+
+        <sensor name="my_laser" type="ray">
+          <pose>0.01 0 0.03 0 -0 0</pose>
+          <always_on>1</always_on>
+          <update_rate>60</update_rate>
+          <visualize>true</visualize>
+          <!--
+          <plugin name="laser" filename="libRayPlugin.so" />
+-->
+
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.26889</min_angle>
+                <max_angle>2.268899</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+        </sensor>
+      </link>
+    </model>
+
+    <!--
+    <model name="hokuyo2">
+      <pose>2 0 1 0 0 0</pose>
+      <link name="link">
+        <visual name="visual">
+          <geometry>
+            <mesh>
+              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <mesh>
+              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
+            </mesh>
+          </geometry>
+        </collision>
+
+        <sensor name="laser" type="ray">
+          <pose>0.01 0 0.03 0 -0 0</pose>
+          <always_on>1</always_on>
+          <update_rate>60</update_rate>
+          <visualize>true</visualize>
+          <plugin name="laser" filename="libRayPlugin.so" />
+
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.26889</min_angle>
+                <max_angle>2.268899</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+        </sensor>
+      </link>
+    </model>
+-->
+
   </world>
 </sdf>

File tools/gztopic.cc

View file
  *
 */
 
+#include <vector>
+#include <list>
+
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/thread/mutex.hpp>
+
 #include <google/protobuf/message.h>
 
 #include <gazebo/gui/qt.h>
 
 #include <gazebo/gazebo_config.h>
 
-#include <boost/algorithm/string.hpp>
-#include <boost/lexical_cast.hpp>
-#include <boost/thread/mutex.hpp>
-
 using namespace gazebo;
 
 // transport::ConnectionPtr connection(new transport::Connection());
 std::vector<std::string> params;
 
-common::Time hz_prev_time;
+common::Time hzPrevTime;
+std::list<common::Time> hzTime;
+boost::condition_variable g_condition;
+
 common::Time bw_prev_time;
-
 std::vector<int> bwBytes;
 std::vector<common::Time> bwTime;
 
 }
 
 /////////////////////////////////////////////////
-void hz_cb(ConstGzStringPtr &/*_data*/)
+void hz_cb(const std::string &/*_data*/)
 {
-  common::Time cur_time = common::Time::GetWallTime();
+  common::Time currTime = common::Time::GetWallTime();
 
-  if (hz_prev_time != common::Time(0, 0))
-    printf("Hz: %6.2f\n", 1.0 / (cur_time - hz_prev_time).Double());
+  boost::mutex::scoped_lock lock(mutex);
 
-  hz_prev_time = cur_time;
+  if (hzPrevTime != common::Time(0, 0))
+  {
+    hzTime.push_back(currTime - hzPrevTime);
+    while(hzTime.size() > 100)
+      hzTime.pop_front();
+  }
+  std::cout << "Hz[" << 1.0 / (currTime - hzPrevTime).Double() << "]\n";
+
+  hzPrevTime = currTime;
 }
 
 /////////////////////////////////////////////////
 }
 
 /////////////////////////////////////////////////
+void runHz()
+{
+  common::Time avg;
+  while (true)
+  {
+    boost::mutex::scoped_lock lock(mutex);
+    if (hzTime.size() >= 100)
+    {
+      avg.Set(0, 0);
+      for (std::list<common::Time>::iterator iter = hzTime.begin();
+          iter != hzTime.end(); ++iter)
+      {
+        avg += *iter;
+      }
+
+      avg /= hzTime.size();
+
+      printf("Hz: %6.2f\n", 1.0 / avg.Double());
+    }
+  }
+
+  g_condition.notify_one();
+}
+
+/////////////////////////////////////////////////
 void hz()
 {
   if (params[1].empty())
   node->Init();
 
   std::string topic = params[1];
-  topic +=  "/__dbg";
 
   transport::SubscriberPtr sub = node->Subscribe(topic, hz_cb);
 
   // Run the transport loop: starts a new thread
   transport::run();
 
-  while (true)
-    common::Time::MSleep(10);
+  boost::mutex myMutex;
+  boost::mutex::scoped_lock lock(myMutex);
+
+  //boost::thread *thread = new boost::thread(boost::bind(&runHz));
+
+  g_condition.wait(lock);
 
   transport::fini();
 }