Anonymous avatar Anonymous committed 0bb7cd5

Fixing actor and depth camera per answers

Comments (0)

Files changed (6)

gazebo/physics/Actor.cc

 #include <limits>
 #include <algorithm>
 
-#include "common/KeyFrame.hh"
-#include "common/Animation.hh"
-#include "common/Plugin.hh"
-#include "common/Events.hh"
-#include "common/Exception.hh"
-#include "common/Console.hh"
-#include "common/CommonTypes.hh"
-#include "common/MeshManager.hh"
-#include "common/Mesh.hh"
-#include "common/Skeleton.hh"
-#include "common/SkeletonAnimation.hh"
-#include "common/BVHLoader.hh"
+#include "gazebo/common/Common.hh"
+#include "gazebo/common/KeyFrame.hh"
+#include "gazebo/common/Animation.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/common/Exception.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/common/MeshManager.hh"
+#include "gazebo/common/Mesh.hh"
+#include "gazebo/common/Skeleton.hh"
+#include "gazebo/common/SkeletonAnimation.hh"
+#include "gazebo/common/BVHLoader.hh"
 
-#include "physics/World.hh"
-#include "physics/Joint.hh"
-#include "physics/Link.hh"
-#include "physics/Model.hh"
-#include "physics/PhysicsEngine.hh"
-#include "physics/Actor.hh"
-#include "physics/Physics.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/Joint.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/Actor.hh"
+#include "gazebo/physics/Physics.hh"
 
-#include "transport/Node.hh"
+#include "gazebo/transport/Node.hh"
 
 using namespace gazebo;
 using namespace physics;
 Actor::Actor(BasePtr _parent)
   : Model(_parent)
 {
+  printf("New actor\n");
   this->AddType(ACTOR);
   this->mesh = NULL;
   this->skeleton = NULL;
 //////////////////////////////////////////////////
 void Actor::Load(sdf::ElementPtr _sdf)
 {
+  printf("load actor\n");
   sdf::ElementPtr skinSdf = _sdf->GetElement("skin");
-  this->skinFile = skinSdf->GetValueString("filename");
+
+  std::cout << "URI[" << skinSdf->GetValueString("uri") << "]\n";
+  this->skinFile = common::find_file(skinSdf->GetValueString("uri"));
+  std::cout << "SKinFile[" << this->skinFile << "]\n";
   this->skinScale = skinSdf->GetValueDouble("scale");
 
   MeshManager::Instance()->Load(this->skinFile);
   }
   else
   {
-    std::string animFile = _sdf->GetValueString("filename");
+    std::string animFile = _sdf->GetValueString("uri");
     std::string extension = animFile.substr(animFile.rfind(".") + 1,
         animFile.size());
     double scale = _sdf->GetValueDouble("scale");
   visualPoseSdf->Set(_pose);
   sdf::ElementPtr geomVisSdf = visualSdf->GetElement("geometry");
   sdf::ElementPtr meshSdf = geomVisSdf->GetElement("mesh");
-  meshSdf->GetElement("filename")->Set(this->skinFile);
+  meshSdf->GetElement("uri")->Set(this->skinFile);
   meshSdf->GetElement("scale")->Set(math::Vector3(this->skinScale,
       this->skinScale, this->skinScale));
 }

gazebo/physics/World.cc

 //////////////////////////////////////////////////
 ActorPtr World::LoadActor(sdf::ElementPtr _sdf , BasePtr _parent)
 {
+  printf("World LoadActor\n");
   ActorPtr actor(new Actor(_parent));
   actor->SetWorld(shared_from_this());
   actor->Load(_sdf);
 //////////////////////////////////////////////////
 void World::LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent)
 {
+  printf("Load Entities\n");
   if (_sdf->HasElement("light"))
   {
     sdf::ElementPtr childElem = _sdf->GetElement("light");

gazebo/sdf/1.3/actor.sdf

     <description></description>
   </attribute>
 
-  <attribute name="static" type="bool" default="false" required="0">
+  <element name="static" type="bool" default="false" required="0">
     <description></description>
-  </attribute>
+  </element>
 
   <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
     <description>Origin of the actor</description>
   <element name="skin" required="1">
     <description></description>
 
-    <element name="filename" type="string" default="__default__" required="1">
+    <element name="uri" type="string" default="__default__" required="1">
       <description></description>
     </element>
 
       <description></description>
     </attribute>
 
-    <element name="filename" type="string" default="__default__" required="1">
+    <element name="uri" type="string" default="__default__" required="1">
       <description></description>
     </element>
+
     <element name="scale" type="double" default="1.0" required="0">
       <description></description>
     </element>
+
     <element name="interpolate_x" type="bool" default="false" required="0">
       <description></description>
     </element>

gazebo/sensors/DepthCameraSensor.cc

 
 #include <sstream>
 
-#include "physics/World.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/common/Image.hh"
 
-#include "common/Events.hh"
-#include "common/Exception.hh"
+#include "gazebo/common/Events.hh"
+#include "gazebo/common/Exception.hh"
 
-#include "transport/transport.hh"
+#include "gazebo/transport/transport.hh"
 
-#include "rendering/DepthCamera.hh"
-#include "rendering/Scene.hh"
-#include "rendering/Rendering.hh"
+#include "gazebo/rendering/DepthCamera.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/Rendering.hh"
 
-#include "sensors/SensorFactory.hh"
-#include "sensors/DepthCameraSensor.hh"
+#include "gazebo/sensors/SensorFactory.hh"
+#include "gazebo/sensors/DepthCameraSensor.hh"
 
 using namespace gazebo;
 using namespace sensors;
 void DepthCameraSensor::Load(const std::string &_worldName)
 {
   Sensor::Load(_worldName);
+
+  std::string topicName = "~/" + this->parentName + "/" + this->GetName();
+  boost::replace_all(topicName, "::", "/");
+
+  std::string imageTopicName = topicName + "/image";
+  std::string depthTopicName = topicName + "/depth";
+
+  this->imagePub = this->node->Advertise<msgs::ImageStamped>(imageTopicName);
+  this->depthPub = this->node->Advertise<msgs::ImageStamped>(depthTopicName);
 }
 
 //////////////////////////////////////////////////
     this->camera->Render();
     this->camera->PostRender();
     this->lastMeasurementTime = this->world->GetSimTime();
+
+    if (this->imagePub->HasConnections())
+    {
+      msgs::ImageStamped msg;
+      msgs::Set(msg.mutable_time(), this->world->GetSimTime());
+      msg.mutable_image()->set_width(this->camera->GetImageWidth());
+      msg.mutable_image()->set_height(this->camera->GetImageHeight());
+      msg.mutable_image()->set_pixel_format(common::Image::ConvertPixelFormat(
+            this->camera->GetImageFormat()));
+
+      msg.mutable_image()->set_step(this->camera->GetImageWidth() *
+          this->camera->GetImageDepth());
+      msg.mutable_image()->set_data(this->camera->GetImageData(),
+          msg.image().width() * this->camera->GetImageDepth() *
+          msg.image().height());
+      this->imagePub->Publish(msg);
+    }
+
+    if (this->depthPub->HasConnections())
+    {
+      msgs::ImageStamped msg;
+      msgs::Set(msg.mutable_time(), this->world->GetSimTime());
+      msg.mutable_image()->set_width(this->camera->GetImageWidth());
+      msg.mutable_image()->set_height(this->camera->GetImageHeight());
+      msg.mutable_image()->set_pixel_format(common::Image::ConvertPixelFormat(
+            this->camera->GetImageFormat()));
+
+      msg.mutable_image()->set_step(this->camera->GetImageWidth() *
+          this->camera->GetImageDepth());
+      msg.mutable_image()->set_data(this->camera->GetImageData(),
+          msg.image().width() * this->camera->GetImageDepth() *
+          msg.image().height());
+      this->depthPub->Publish(msg);
+    }
   }
 }
 
 //////////////////////////////////////////////////
+unsigned int DepthCameraSensor::GetImageWidth() const
+{
+  if (this->camera)
+    return this->camera->GetImageWidth();
+  return 0;
+}
+
+//////////////////////////////////////////////////
+unsigned int DepthCameraSensor::GetImageHeight() const
+{
+  if (this->camera)
+    return this->camera->GetImageHeight();
+  return 0;
+}
+
+//////////////////////////////////////////////////
+const unsigned char *DepthCameraSensor::GetImageData()
+{
+  return this->camera->GetImageData(0);
+}
+
+//////////////////////////////////////////////////
 bool DepthCameraSensor::SaveFrame(const std::string &_filename)
 {
   this->SetActive(true);
   return this->camera->SaveFrame(_filename);
 }
 
+//////////////////////////////////////////////////
+bool DepthCameraSensor::IsActive()
+{
+  return Sensor::IsActive() || this->imagePub->HasConnections();
+}

gazebo/sensors/DepthCameraSensor.hh

       public: rendering::DepthCameraPtr GetDepthCamera() const
               {return this->camera;}
 
+      /// \brief Gets the width of the image in pixels.
+      /// \return The image width in pixels.
+      public: unsigned int GetImageWidth() const;
+
+      /// \brief Gets the height of the image in pixels.
+      /// \return The image height in pixels.
+      public: unsigned int GetImageHeight() const;
+
+      /// \brief Gets the raw image data from the sensor.
+      /// \return The pointer to the image data array.
+      public: const unsigned char *GetImageData();
+
       /// \brief Saves an image frame of depth camera sensor to file
       /// \param[in] Name of file to save as
       /// \return True if saved, false if not
       public: bool SaveFrame(const std::string &_filename);
 
+      // Documentation inherited
+      public: virtual bool IsActive();
+
+      /// \brief Pointer to the camera userd for rendering.
       private: rendering::DepthCameraPtr camera;
 
+      /// \brief Pointer to the scene.
       private: rendering::ScenePtr scene;
+
+      /// \brief Publisher of RGB image frames.
+      private: transport::PublisherPtr imagePub;
+
+      /// \brief Publisher of depth image frames.
+      private: transport::PublisherPtr depthPub;
     };
     /// \}
   }

sdf/worlds/actor.world

 <?xml version="1.0" ?>
 <sdf version="1.3">
-    <world name="default">
-        <include><uri>model://ground_plane</uri></include>
-        <light type="directional" name="sun">
-            <pose>0 0 100 0 0 0</pose>
-            <diffuse>0.5 0.5 0.5 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-            <attenuation>
-                <range>100</range>
-                <linear>0.01</linear>
-                <constant>0.8</constant>
-                <quadratic>0.0</quadratic>
-            </attenuation>
-            <direction>0.0 0.2 -8.0</direction>
-            <cast_shadows>true</cast_shadows>
-        </light>
-        <include filename="actor.model" />
-    </world>
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <include>
+      <uri>model://actor</uri>
+    </include>
+  </world>
 </sdf>
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.