[Personalrobots-commit] SF.net SVN: personalrobots:[4448] pkg/trunk

[<hsu...@us...>]

Revision: 4448
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4448&view=rev
Author:   hsujohnhsu
Date:     2008-09-18 23:24:59 +0000 (Thu, 18 Sep 2008)

Log Message:
-----------
fixed bug pre track ticket 318 in personal robots.

Modified Paths:
--------------
    pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
    pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
    pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml

Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff	2008-09-18 23:24:59 UTC (rev 4448)
@@ -626,195 +626,155 @@
    /// \}
  }
  #endif
-Index: server/sensors/ray/RayBlockSensor.cc
+Index: server/sensors/ray/RaySensor.hh
 ===================================================================
---- server/sensors/ray/RayBlockSensor.cc	(revision 0)
-+++ server/sensors/ray/RayBlockSensor.cc	(revision 0)
-@@ -0,0 +1,450 @@
-+/*
-+ *  Gazebo - Outdoor Multi-Robot Simulator
-+ *  Copyright (C) 2003
-+ *     Nate Koenig & Andrew Howard
-+ *
-+ *  This program is free software; you can redistribute it and/or modify
-+ *  it under the terms of the GNU General Public License as published by
-+ *  the Free Software Foundation; either version 2 of the License, or
-+ *  (at your option) any later version.
-+ *
-+ *  This program is distributed in the hope that it will be useful,
-+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
-+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-+ *  GNU General Public License for more details.
-+ *
-+ *  You should have received a copy of the GNU General Public License
-+ *  along with this program; if not, write to the Free Software
-+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
-+ *
-+ */
-+/* Desc: Ray proximity sensor
-+ * Author: Carle Cote
-+ * Date: 23 february 2004
-+ * SVN: $Id: RayBlockSensor.cc 7000 2008-09-01 18:56:47Z natepak $
-+*/
+--- server/sensors/ray/RaySensor.hh	(revision 7023)
++++ server/sensors/ray/RaySensor.hh	(working copy)
+@@ -100,6 +100,22 @@
+   /// \return The number of ranges
+   public: int GetRangeCount() const;
+ 
++  /// \brief Get the vertical scan line count
++  /// \return The number of scan lines vertically
++  public: int GetVerticalRayCount() const;
 +
-+#include <assert.h>
-+#include <float.h>
-+#include <sstream>
++  /// \brief Get the vertical scan line count
++  /// \return The number of scan lines vertically
++  public: int GetVerticalRangeCount() const;
 +
-+#include "SensorFactory.hh"
-+#include "XMLConfig.hh"
-+#include "Global.hh"
-+#include "RayGeom.hh"
-+#include "World.hh"
-+#include "PhysicsEngine.hh"
-+#include "GazeboError.hh"
-+#include "ODEPhysics.hh"
-+#include "XMLConfig.hh"
-+#include "Controller.hh"
-+#include "RayBlockSensor.hh"
++  /// \brief Get the vertical scan bottom angle
++  /// \return The minimum angle of the scan block
++  public: Angle GetVerticalMinAngle() const;
 +
-+#include "Vector3.hh"
++  /// \brief Get the vertical scan line top angle
++  /// \return The Maximum angle of the scan block
++  public: Angle GetVerticalMaxAngle() const;
 +
-+using namespace gazebo;
+   /// \brief Set ray parameters
+   /// \param index Rayindex (from 0 to rayCount - 1).
+   /// \param a, b Ray endpoints (initial and final points).  These are
+@@ -142,6 +158,11 @@
+   /// Display rays when rendering images
+   private: ParamT<bool> *displayRaysP;
+ 
++  // For ray blocks such as Velodyne
++  private: ParamT<int> *verticalRayCountP;
++  private: ParamT<int> *verticalRangeCountP;
++  private: ParamT<Angle> *verticalMinAngleP;
++  private: ParamT<Angle> *verticalMaxAngleP;
+ };
+ /// \}
+ /// \}
+Index: server/sensors/ray/SConscript
+===================================================================
+--- server/sensors/ray/SConscript	(revision 7023)
++++ server/sensors/ray/SConscript	(working copy)
+@@ -1,7 +1,7 @@
+ #Import variable
+ Import('env sharedObjs')
+ 
+-sources = Split('RaySensor.cc')
++sources = Split('RaySensor.cc RayBlockSensor.cc')
+ 
+ #staticObjs.append(env.StaticObject(sources))
+ sharedObjs.append(env.SharedObject(sources))
+Index: server/sensors/ray/RaySensor.cc
+===================================================================
+--- server/sensors/ray/RaySensor.cc	(revision 7023)
++++ server/sensors/ray/RaySensor.cc	(working copy)
+@@ -64,6 +64,12 @@
+   this->maxRangeP = new ParamT<double>("maxRange",0,1);
+   this->originP = new ParamT<Vector3>("origin", Vector3(0,0,0), 0);
+   this->displayRaysP = new ParamT<bool>("displayRays", true, 0);
 +
-+GZ_REGISTER_STATIC_SENSOR("ray_block", RayBlockSensor);
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Constructor
-+RayBlockSensor::RayBlockSensor(Body *body)
-+    : Sensor(body)
-+{
-+  this->active = false;
-+
-+  this->typeName = "ray";
-+
-+  Param::Begin(&this->parameters);
-+  this->rayCountP = new ParamT<int>("rayCount",0,1);
-+  this->rangeCountP = new ParamT<int>("rangeCount",0,1);
-+  this->minAngleP = new ParamT<Angle>("minAngle",DTOR(-90),1);
-+  this->maxAngleP = new ParamT<Angle>("maxAngle",DTOR(-90),1);
-+  this->minRangeP = new ParamT<double>("minRange",0,1);
-+  this->maxRangeP = new ParamT<double>("maxRange",0,1);
-+  this->originP = new ParamT<Vector3>("origin", Vector3(0,0,0), 0);
-+  this->displayRaysP = new ParamT<bool>("displayRays", true, 0);
-+
 +  // for block rays, vertical setting
 +  this->verticalRayCountP = new ParamT<int>("verticalRayCount", 1, 0);
 +  this->verticalRangeCountP = new ParamT<int>("verticalRangeCount", 1, 0);
 +  this->verticalMinAngleP = new ParamT<Angle>("verticalMinAngle", DTOR(0), 0);
 +  this->verticalMaxAngleP = new ParamT<Angle>("verticalMaxAngle", DTOR(0), 0);
-+  Param::End();
-+}
+   Param::End();
+ }
+ 
+@@ -80,6 +86,11 @@
+   delete this->maxRangeP;
+   delete this->originP;
+   delete this->displayRaysP;
 +
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Destructor
-+RayBlockSensor::~RayBlockSensor()
-+{
-+  delete this->rayCountP;
-+  delete this->rangeCountP;
-+  delete this->minAngleP;
-+  delete this->maxAngleP;
-+  delete this->minRangeP;
-+  delete this->maxRangeP;
-+  delete this->originP;
-+  delete this->displayRaysP;
-+
 +  delete this->verticalRayCountP;
 +  delete this->verticalRangeCountP;
 +  delete this->verticalMinAngleP;
 +  delete this->verticalMaxAngleP;
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Load the ray using parameter from an XMLConfig node
-+void RayBlockSensor::LoadChild(XMLConfigNode *node)
-+{
-+  if (this->body == NULL)
-+  {
-+    gzthrow("Null body in the ray sensor");
-+  }
-+
-+  this->rayCountP->Load(node);
-+  this->rangeCountP->Load(node);
-+  this->minAngleP->Load(node);
-+  this->maxAngleP->Load(node);
-+  this->minRangeP->Load(node);
-+  this->maxRangeP->Load(node);
-+  this->originP->Load(node);
-+  this->displayRaysP->Load(node);
+ }
+ 
+ //////////////////////////////////////////////////////////////////////////////
+@@ -99,6 +110,10 @@
+   this->maxRangeP->Load(node);
+   this->originP->Load(node);
+   this->displayRaysP->Load(node);
 +  this->verticalRayCountP->Load(node);
 +  this->verticalRangeCountP->Load(node);
 +  this->verticalMinAngleP->Load(node);
 +  this->verticalMaxAngleP->Load(node);
-+
-+
-+  // Create a space to contain the ray space
-+  this->superSpaceId = dSimpleSpaceCreate( 0 );
-+
-+  // Create a space to contain all the rays
-+  this->raySpaceId = dSimpleSpaceCreate( this->superSpaceId );
-+
-+  // Set collision bits
-+  dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_LASER_COLLIDE);
-+  dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_LASER_COLLIDE);
-+
-+  //this->body->spaceId = this->superSpaceId;
-+  this->body->spaceId = this->raySpaceId;
-+
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Save the sensor info in XML format
-+void RayBlockSensor::SaveChild(std::string &prefix, std::ostream &stream)
-+{
-+  stream << prefix << "  " << *(this->minAngleP) << "\n";
-+  stream << prefix << "  " << *(this->maxAngleP) << "\n";
-+  stream << prefix << "  " << *(this->minRangeP) << "\n";
-+  stream << prefix << "  " << *(this->maxRangeP) << "\n";
-+  stream << prefix << "  " << *(this->originP) << "\n";
-+  stream << prefix << "  " << *(this->rayCountP) << "\n";
-+  stream << prefix << "  " << *(this->rangeCountP) << "\n";
-+  stream << prefix << "  " << *(this->displayRaysP) << "\n";
+ 
+ 
+   // Create a space to contain the ray space
+@@ -128,6 +143,10 @@
+   stream << prefix << "  " << *(this->rayCountP) << "\n";
+   stream << prefix << "  " << *(this->rangeCountP) << "\n";
+   stream << prefix << "  " << *(this->displayRaysP) << "\n";
 +  stream << prefix << "  " << *(this->verticalRayCountP) << "\n";
 +  stream << prefix << "  " << *(this->verticalRangeCountP) << "\n";
 +  stream << prefix << "  " << *(this->verticalMinAngleP) << "\n";
 +  stream << prefix << "  " << *(this->verticalMaxAngleP) << "\n";
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Init the ray
-+void RayBlockSensor::InitChild()
-+{
-+  Pose3d bodyPose;
+ }
+ 
+ //////////////////////////////////////////////////////////////////////////////
+@@ -135,35 +154,42 @@
+ void RaySensor::InitChild()
+ {
+   Pose3d bodyPose;
+-  double angle;
 +  double yawAngle, pitchAngle;
-+  Vector3 start, end, axis;
-+  RayGeom *ray;
-+
-+  bodyPose = this->body->GetPose();
-+  this->prevPose = bodyPose;
-+
+   Vector3 start, end, axis;
+   RayGeom *ray;
+ 
+   bodyPose = this->body->GetPose();
+   this->prevPose = bodyPose;
+ 
 +  double pDiff = (**(this->verticalMaxAngleP) - **(this->verticalMinAngleP)).GetAsRadian();
 +  double yDiff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian();
-+  // Create and array of ray geoms
+   // Create and array of ray geoms
+-  for (int i = 0; i < this->rayCountP->GetValue(); i++)
+-  //for (int i = this->rayCount-1; i >= 0; i--)
 +  for (int j = 0; j < this->verticalRayCountP->GetValue(); j++)
-+  {
-+
+   {
+-    double diff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian();
+ 
+-    angle = i * diff / (rayCountP->GetValue() - 1) + (**(this->minAngleP)).GetAsRadian();
 +    for (int i = 0; i < this->rayCountP->GetValue(); i++)
 +    //for (int i = this->rayCount-1; i >= 0; i--)
 +    {
-+
-+      yawAngle = i * yDiff / (rayCountP->GetValue() - 1) + (**(this->minAngleP)).GetAsRadian();
-+
-+      pitchAngle = j * pDiff / (verticalRayCountP->GetValue() - 1) + (**(this->verticalMinAngleP)).GetAsRadian();
-+
+ 
+-    axis.Set(cos(angle), sin(angle),0);
++      (rayCountP->GetValue() == 1)? 0 : yawAngle = i * yDiff / (rayCountP->GetValue() - 1) + (**(this->minAngleP)).GetAsRadian();
+ 
+-    start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue();
+-    end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue();
++      (verticalRayCountP->GetValue() == 1)? 0 :  pitchAngle = j * pDiff / (verticalRayCountP->GetValue() - 1) + (**(this->verticalMinAngleP)).GetAsRadian();
+ 
+-    ray = new RayGeom(this->body, displayRaysP->GetValue());
 +      axis.Set(cos(pitchAngle)*cos(yawAngle), sin(yawAngle),sin(pitchAngle)*cos(yawAngle));
-+
+ 
+-    ray->SetPoints(start, end);
+-//    ray->SetCategoryBits( GZ_LASER_COLLIDE );
+-    //ray->SetCollideBits( ~GZ_LASER_COLLIDE );
 +      start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue();
 +      end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue();
-+
+ 
+-    this->rays.push_back(ray);
 +      ray = new RayGeom(this->body, displayRaysP->GetValue());
-+
+ 
+-    //this->body->AttachGeom(ray);
 +      ray->SetPoints(start, end);
 +      //ray->SetCategoryBits( GZ_LASER_COLLIDE );
 +      //ray->SetCollideBits( ~GZ_LASER_COLLIDE );
@@ -823,453 +783,44 @@
 +
 +      //this->body->AttachGeom(ray);
 +    }
-+  }
-+
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Init the ray
-+void RayBlockSensor::FiniChild()
-+{
-+  std::vector<RayGeom*>::iterator iter;
-+  for (iter=this->rays.begin(); iter!=this->rays.end(); iter++)
-+  {
-+    delete *iter;
-+  }
-+  this->rays.clear();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Get the minimum angle
-+Angle RayBlockSensor::GetMinAngle() const
-+{
-+  return **(this->minAngleP);
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Get the maximum angle
-+Angle RayBlockSensor::GetMaxAngle() const
-+{
-+  return **(this->maxAngleP);
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Get the minimum range
-+double RayBlockSensor::GetMinRange() const
-+{
-+  return this->minRangeP->GetValue();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+///  Get the maximum range
-+double RayBlockSensor::GetMaxRange() const
-+{
-+  return this->maxRangeP->GetValue();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Get the ray count
-+int RayBlockSensor::GetRayCount() const
-+{
-+  return this->rayCountP->GetValue();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+/// Get the range count
-+int RayBlockSensor::GetRangeCount() const
-+{
-+  return this->rangeCountP->GetValue();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
+   }
+ 
+ }
+@@ -223,6 +249,34 @@
+ }
+ 
+ //////////////////////////////////////////////////////////////////////////////
 +/// Get the vertical scan line count
-+int RayBlockSensor::GetVerticalRayCount() const
++int RaySensor::GetVerticalRayCount() const
 +{
 +  return this->verticalRayCountP->GetValue();
 +}
 +
 +//////////////////////////////////////////////////////////////////////////////
 +/// Get the vertical scan line count
-+int RayBlockSensor::GetVerticalRangeCount() const
++int RaySensor::GetVerticalRangeCount() const
 +{
 +  return this->verticalRangeCountP->GetValue();
 +}
 +
 +//////////////////////////////////////////////////////////////////////////////
 +/// Get the vertical min angle
-+Angle RayBlockSensor::GetVerticalMinAngle() const
++Angle RaySensor::GetVerticalMinAngle() const
 +{
 +  return this->verticalMinAngleP->GetValue();
 +}
 +
 +//////////////////////////////////////////////////////////////////////////////
 +/// Get the vertical max angle
-+Angle RayBlockSensor::GetVerticalMaxAngle() const
++Angle RaySensor::GetVerticalMaxAngle() const
 +{
 +  return this->verticalMaxAngleP->GetValue();
 +}
 +
 +//////////////////////////////////////////////////////////////////////////////
-+// Get detected range for a ray
-+double RayBlockSensor::GetRange(int index)
-+{
-+  if (index < 0 || index >= (int)this->rays.size())
-+  {
-+    std::ostringstream stream;
-+    stream << "index[" << index << "] out of range[0-"
-+    << this->rays.size() << "]";
-+    gzthrow(stream.str());
-+  }
-+
-+  return this->rays[index]->GetLength();
-+}
-+
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Get detected retro (intensity) value for a ray.
-+double RayBlockSensor::GetRetro(int index)
-+{
-+  if (index < 0 || index >= (int)this->rays.size())
-+  {
-+    std::ostringstream stream;
-+    stream << "index[" << index << "] out of range[0-"
-+    << this->rays.size() << "]";
-+    gzthrow(stream.str());
-+  }
-+
-+  return this->rays[index]->GetRetro();
-+}
-+
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Get detected fiducial value for a ray.
-+int RayBlockSensor::GetFiducial(int index)
-+{
-+  if (index < 0 || index >= (int)this->rays.size())
-+  {
-+    std::ostringstream stream;
-+    stream << "index[" << index << "] out of range[0-"
-+    << this->rays.size() << "]";
-+    gzthrow(stream.str());
-+  }
-+
-+  return this->rays[index]->GetFiducial();
-+}
-+
-+//////////////////////////////////////////////////////////////////////////////
-+// Update the sensor information
-+void RayBlockSensor::UpdateChild()
-+{
-+  if (this->active)
-+  {
-+    std::vector<RayGeom*>::iterator iter;
-+    Pose3d poseDelta;
-+    Vector3 a, b;
-+
-+    // Get the pose of the sensor body (global cs)
-+    poseDelta = this->body->GetPose() - this->prevPose;
-+    this->prevPose = this->body->GetPose();
-+
-+    // Reset the ray lengths and mark the geoms as dirty (so they get
-+    // redrawn)
-+    for (iter = this->rays.begin(); iter != this->rays.end(); iter++)
-+    {
-+      (*iter)->SetLength( this->maxRangeP->GetValue() );
-+      (*iter)->SetRetro( 0.0 );
-+      (*iter)->SetFiducial( -1 );
-+
-+      // Get the global points of the line
-+      (*iter)->Update();
-+    }
-+
-+    ODEPhysics *ode = dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine());
-+
-+    if (ode == NULL)
-+    {
-+      gzthrow( "Invalid physics engine. Must use ODE." );
-+    }
-+
-+    // Do collision detection
-+    dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ),
-+                    ( dGeomID ) ( ode->GetSpaceId() ),
-+                    this, &UpdateCallback );
-+  }
-+}
-+
-+
-+
-+/////////////////////////////////////////////////////////////////////////////
-+// Callback for ray intersection test
-+void RayBlockSensor::UpdateCallback( void *data, dGeomID o1, dGeomID o2 )
-+{
-+  int n = 0;
-+  dContactGeom contact;
-+  dxGeom *geom1, *geom2 = NULL;
-+  RayGeom *rayGeom = NULL;
-+  Geom *hitGeom = NULL;
-+  RayBlockSensor *self = NULL;
-+
-+  self = (RayBlockSensor*) data;
-+
-+  // Check space
-+  if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) )
-+  {
-+    if (dGeomGetSpace(o1) == self->superSpaceId || dGeomGetSpace(o2) == self->superSpaceId)
-+    {
-+      dSpaceCollide2( o1, o2, self, &UpdateCallback );
-+    }
-+    if (dGeomGetSpace(o1) == self->raySpaceId || dGeomGetSpace(o2) == self->raySpaceId)
-+    {
-+      dSpaceCollide2( o1, o2, self, &UpdateCallback );
-+    }
-+  }
-+  else
-+  {
-+    geom1 = NULL;
-+    geom2 = NULL;
-+
-+    // Get pointers to the underlying geoms
-+    if (dGeomGetClass(o1) == dGeomTransformClass)
-+      geom1 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o1));
-+    else
-+      geom1 = (dxGeom*) dGeomGetData(o1);
-+
-+    if (dGeomGetClass(o2) == dGeomTransformClass)
-+      geom2 = (dxGeom*) dGeomGetData(dGeomTransformGetGeom(o2));
-+    else
-+      geom2 = (dxGeom*) dGeomGetData(o2);
-+
-+    assert(geom1 && geom2);
-+
-+    rayGeom = NULL;
-+    hitGeom = NULL;
-+
-+    // Figure out which one is a ray; note that this assumes
-+    // that the ODE dRayClass is used *soley* by the RayGeom.
-+    if (dGeomGetClass(o1) == dRayClass)
-+    {
-+      rayGeom = (RayGeom*) geom1;
-+      hitGeom = (Geom*) geom2;
-+      dGeomRaySetParams(o1, 0, 0);
-+      dGeomRaySetClosestHit(o1, 1);
-+    }
-+
-+    if (dGeomGetClass(o2) == dRayClass)
-+    {
-+      assert(rayGeom == NULL);
-+      rayGeom = (RayGeom*) geom2;
-+      hitGeom = (Geom* )geom1;
-+      dGeomRaySetParams(o2, 0, 0);
-+      dGeomRaySetClosestHit(o2, 1);
-+    }
-+
-+    // Check for ray/geom intersections
-+    if ( rayGeom && hitGeom )
-+    {
-+
-+      n = dCollide(o1, o2, 1, &contact, sizeof(contact));
-+
-+      if ( n > 0 )
-+      {
-+        if (contact.depth < rayGeom->GetLength())
-+        {
-+          rayGeom->SetLength( contact.depth );
-+          rayGeom->SetRetro( hitGeom->GetLaserRetro() );
-+          rayGeom->SetFiducial( hitGeom->GetLaserFiducialId() );
-+        }
-+      }
-+    }
-+  }
-+}
-Index: server/sensors/ray/SConscript
-===================================================================
---- server/sensors/ray/SConscript	(revision 7023)
-+++ server/sensors/ray/SConscript	(working copy)
-@@ -1,7 +1,7 @@
- #Import variable
- Import('env sharedObjs')
- 
--sources = Split('RaySensor.cc')
-+sources = Split('RaySensor.cc RayBlockSensor.cc')
- 
- #staticObjs.append(env.StaticObject(sources))
- sharedObjs.append(env.SharedObject(sources))
-Index: server/sensors/ray/RayBlockSensor.hh
-===================================================================
---- server/sensors/ray/RayBlockSensor.hh	(revision 0)
-+++ server/sensors/ray/RayBlockSensor.hh	(revision 0)
-@@ -0,0 +1,171 @@
-+/*
-+ *  Gazebo - Outdoor Multi-Robot Simulator
-+ *  Copyright (C) 2003  
-+ *     Nate Koenig & Andrew Howard
-+ *
-+ *  This program is free software; you can redistribute it and/or modify
-+ *  it under the terms of the GNU General Public License as published by
-+ *  the Free Software Foundation; either version 2 of the License, or
-+ *  (at your option) any later version.
-+ *
-+ *  This program is distributed in the hope that it will be useful,
-+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
-+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-+ *  GNU General Public License for more details.
-+ *
-+ *  You should have received a copy of the GNU General Public License
-+ *  along with this program; if not, write to the Free Software
-+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
-+ *
-+ */
-+/* Desc: RayBlockSensor proximity sensor
-+ * Author: Carle Cote
-+ * Date: 23 february 2004
-+ * SVN: $Id: RayBlockSensor.hh 7000 2008-09-01 18:56:47Z natepak $
-+*/
-+
-+#ifndef RAYBLOCKSENSOR_HH
-+#define RAYBLOCKSENSOR_HH
-+
-+#include <vector>
-+
-+#include "Angle.hh"
-+#include "Sensor.hh"
-+#include "Body.hh"
-+
-+namespace gazebo
-+{
-+
-+  class XMLConfigNode;
-+  class RayGeom;
-+/// \addtogroup gazebo_sensor
-+/// \brief Sensor with one or more rays.
-+/// \{
-+/// \defgroup gazebo_ray Ray
-+/// \brief Sensor with one or more rays.
-+// \{
-+
-+/// \brief Sensor with one or more rays.
-+///
-+/// This sensor cast rays into the world, tests for intersections, and
-+/// reports the range to the nearest object.  It is used by ranging
-+/// sensor models (e.g., sonars and scanning laser range finders).
-+class RayBlockSensor: public Sensor
-+{
-+  /// \brief Constructor
-+  /// \param body The underlying collision test uses an ODE geom, so
-+  ///             ray sensors must be attached to a body.
-+  public: RayBlockSensor(Body *body);
-+
-+  /// \brief Destructor
-+  public: virtual ~RayBlockSensor();
-+
-+  /// Load the ray using parameter from an XMLConfig node
-+  /// \param node The XMLConfig node
-+  protected: virtual void LoadChild(XMLConfigNode *node);
-+
-+  /// \brief Save the sensor info in XML format
-+  protected: virtual void SaveChild(std::string &prefix, std::ostream &stream);
-+
-+  /// Initialize the ray
-+  protected: virtual void InitChild();
-+
-+  ///  Update sensed values
-+  protected: virtual void UpdateChild();
-+  
-+  /// Finalize the ray
-+  protected: virtual void FiniChild();
-+
-+  /// \brief Get the minimum angle
-+  /// \return The minimum angle
-+  public: Angle GetMinAngle() const;
-+
-+  /// \brief Get the maximum angle
-+  /// \return the maximum angle
-+  public: Angle GetMaxAngle() const;
-+
-+  /// \brief Get the minimum range
-+  /// \return The minimum range
-+  public: double GetMinRange() const;
-+
-+  /// \brief Get the maximum range
-+  /// \return The maximum range
-+  public: double GetMaxRange() const;
-+
-+  /// \brief Get the ray count
-+  /// \return The number of rays
-+  public: int GetRayCount() const;
-+
-+  /// \brief Get the range count
-+  /// \return The number of ranges
-+  public: int GetRangeCount() const;
-+
-+  /// \brief Get the vertical scan line count
-+  /// \return The number of scan lines vertically
-+  public: int GetVerticalRayCount() const;
-+
-+  /// \brief Get the vertical scan line count
-+  /// \return The number of scan lines vertically
-+  public: int GetVerticalRangeCount() const;
-+
-+  /// \brief Get the vertical scan bottom angle
-+  /// \return The minimum angle of the scan block
-+  public: Angle GetVerticalMinAngle() const;
-+
-+  /// \brief Get the vertical scan line top angle
-+  /// \return The Maximum angle of the scan block
-+  public: Angle GetVerticalMaxAngle() const;
-+
-+  /// \brief Set ray parameters
-+  /// \param index Rayindex (from 0 to rayCount - 1).
-+  /// \param a, b Ray endpoints (initial and final points).  These are
-+  /// in body-relative coordiantes.
-+  public: void SetRay(int index, const Vector3 &a, const Vector3 &b);
-+
-+  /// Get ray parameters
-+  /// \param index Ray index (from 0 to rayCount -1).
-+  /// \param pos, dir Ray position and direction.
-+  public: void GetRay(int index, Vector3 &pos, Vector3 &dir);
-+
-+  /// \brief Get detected range for a ray.
-+  /// \returns Returns DBL_MAX for no detection.
-+  public: double GetRange(int index);   
-+
-+  /// \brief Get detected retro (intensity) value for a ray.
-+  public: double GetRetro(int index);   
-+
-+  /// \brief Get detected fiducial value for a ray.
-+  public: int GetFiducial(int index);   
-+
-+  /// \brief Ray-intersection callback
-+  private: static void UpdateCallback( void *data, dGeomID o1, dGeomID o2 );
-+  
-+  /// Ray space for collision detector
-+  private: dSpaceID superSpaceId;
-+  private: dSpaceID raySpaceId; 
-+
-+  /// Ray data
-+  private: std::vector<RayGeom*> rays;
-+
-+  private: ParamT<Angle> *minAngleP, *maxAngleP;
-+  private: ParamT<double> *minRangeP, *maxRangeP;
-+  private: ParamT<Vector3> *originP;
-+
-+  private: Pose3d prevPose;
-+  private: ParamT<int> *rayCountP;
-+  private: ParamT<int> *rangeCountP;
-+  
-+  /// Display rays when rendering images
-+  private: ParamT<bool> *displayRaysP;
-+
-+  // For ray blocks such as Velodyne
-+  private: ParamT<int> *verticalRayCountP;
-+  private: ParamT<int> *verticalRangeCountP;
-+  private: ParamT<Angle> *verticalMinAngleP;
-+  private: ParamT<Angle> *verticalMaxAngleP;
-+};
-+/// \}
-+/// \}
-+}
-+
-+#endif
+ // Get detected range for a ray
+ double RaySensor::GetRange(int index)
+ {
 Index: server/sensors/Sensor.cc
 ===================================================================
 --- server/sensors/Sensor.cc	(revision 7023)

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh	2008-09-18 23:24:59 UTC (rev 4448)
@@ -106,7 +106,7 @@
   private: FiducialIface *fiducialIface;
 
   /// The parent sensor
-  private: RayBlockSensor *myParent;
+  private: RaySensor *myParent;
 
   // pointer to ros node
   private: ros::node *rosnode;

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc	2008-09-18 23:24:59 UTC (rev 4448)
@@ -37,7 +37,7 @@
 #include <gazebo/gazebo.h>
 #include <gazebo/GazeboError.hh>
 #include <gazebo/ControllerFactory.hh>
-#include <RayBlockSensor.hh>
+#include <RaySensor.hh>
 #include <gazebo_plugin/Ros_Block_Laser.hh>
 
 using namespace gazebo;
@@ -49,10 +49,10 @@
 Ros_Block_Laser::Ros_Block_Laser(Entity *parent)
     : Controller(parent)
 {
-  this->myParent = dynamic_cast<RayBlockSensor*>(this->parent);
+  this->myParent = dynamic_cast<RaySensor*>(this->parent);
 
   if (!this->myParent)
-    gzthrow("Ros_Block_Laser controller requires a Ray Block Sensor as its parent");
+    gzthrow("Ros_Block_Laser controller requires a Ray Sensor as its parent");
 
   // set parent sensor to active automatically
   this->myParent->SetActive(true);

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml	2008-09-18 23:24:59 UTC (rev 4448)
@@ -209,10 +209,15 @@
   
   
   <!-- to be used by sensors -->
-  <const name="torso_tilt_laser_offset_x" value="0.10000" />  <!-- mp 20080801 -->
-  <const name="torso_tilt_laser_offset_z" value="0.19525" />  <!-- mp 20080801 -->
-  <const name="tilt_laser_center_box_center_offset_z" value="0.0" />  <!-- FIXME -->
-  
+  <const name="torso_tilt_laser_mount_offset_x" value="0.10000" />  <!-- mp 20080801 -->
+  <const name="torso_tilt_laser_mount_offset_z" value="0.19525" />  <!-- mp 20080801 -->
+  <const name="tilt_laser_mount_center_box_center_offset_z" value="0.0" />  <!-- FIXME -->
+
+  <!-- from sensor frame to mount bracket frame for tilt laser -->
+  <const name="tilt_laser_mount_tilt_laser_offset_x" value="0.00" /> <!-- FIXME -->
+  <const name="tilt_laser_mount_tilt_laser_offset_z" value="0.03" /> <!-- FIXME -->
+
+
   <const name="base_base_laser_offset_x" value="0.275" />  <!-- mp 20080801 -->
   <const name="base_base_laser_offset_z" value="0.182" />  <!-- mp 20080801 -->
   <const name="base_laser_center_box_center_offset_z" value="0.12" />  <!-- FIXME -->
@@ -800,11 +805,15 @@
   <joint name="caster_front_right_joint" type="revolute" >
     <insert_const_block name="pr2_caster_joint"/>
   </joint>
-  <joint name="tilt_laser_joint" type="revolute"> 
+  <joint name="tilt_laser_mount_joint" type="revolute"> 
     <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
     <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
     <limit min= "-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
   </joint>
+  <joint name="tilt_laser_joint" type="fixed"> 
+    <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+    <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+  </joint>
   <joint name="base_laser_joint" type="fixed"> 
     <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
     <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
@@ -1720,15 +1729,13 @@
   <!-- End head definition -->
   
   
-  <!-- Begin sensor definitions -->
-  <sensor name="tilt_laser" type="laser"> 
+  <link name="tilt_laser_mount"> 
     
-    <calibration filename="calib_filename" />
     <parent name="torso" />
-    <origin xyz=" torso_tilt_laser_offset_x 0 torso_tilt_laser_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    <origin xyz=" torso_tilt_laser_mount_offset_x 0 torso_tilt_laser_mount_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
     <!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
     
-    <joint name="tilt_laser_joint" /> 
+    <joint name="tilt_laser_mount_joint" /> 
     <inertial>
       <mass value="1.0" />
       <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
@@ -1736,19 +1743,52 @@
     </inertial> 
     
     <visual>
-      <origin xyz="0   0   tilt_laser_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <origin xyz="0   0   tilt_laser_mount_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
       <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
       <map name="gazebo_material" flag="gazebo">
         <elem key="material">Gazebo/PioneerBody</elem>
       </map>
-      <geometry name="pr2_tilt_laser_mesh_file">
+      <geometry name="pr2_tilt_laser_mount_mesh_file">
         <mesh filename="hok_tilt" /> <!-- mesh specified using an obj file --> 
       </geometry>
     </visual>
     
     <collision>
-      <origin xyz="0   0   tilt_laser_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <origin xyz="0   0   tilt_laser_mount_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
       <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+      <geometry name="tilt_laser_mount_collision_geom" ><!-- geometry specified using a simple geometric object -->
+        <box size=".001 .001 .001" /> 
+      </geometry>
+    </collision>
+    
+  </link>
+
+  <!-- Begin sensor definitions -->
+  <sensor name="tilt_laser" type="laser"> 
+   
+    <calibration filename="calib_filename" />
+    <parent name="tilt_laser_mount" />
+    <origin xyz=" tilt_laser_mount_tilt_laser_offset_x 0 tilt_laser_mount_tilt_laser_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    <!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    
+    <joint name="tilt_laser_joint" /> 
+    <inertial>
+      <mass value="0.1" />
+      <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+    </inertial> 
+    
+    <visual>
+      <origin xyz="0   0   0  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+      <geometry name="pr2_tilt_laser_visual_geom">
+        <mesh scale="0.001 0.001 0.001" /> <!-- mesh specified using an obj file --> 
+      </geometry>
+    </visual>
+    
+    <collision>
+      <origin xyz="0   0   0  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
       <geometry name="tilt_laser_collision_geom" ><!-- geometry specified using a simple geometric object -->
         <box size=".001 .001 .001" /> 
       </geometry>
@@ -1757,12 +1797,11 @@
     <map name="sensor" flag="gazebo">
       <verbatim key="sensor_ray">
         <sensor:ray name="tilt_laser">
-          <gaussianNoise>0.05</gaussianNoise>
           <rayCount>683</rayCount>
           <rangeCount>683</rangeCount>
           <laserCount>1</laserCount>
 	  
-          <origin>0.0 0.0 0.05</origin>
+          <origin>0.0 0.0 0.0</origin>
           <displayRays>false</displayRays>
 	  
           <minAngle>-45</minAngle>
@@ -1772,6 +1811,7 @@
           <maxRange>10.0</maxRange>
           <updateRate>10.0</updateRate>
           <controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
             <updateRate>15.0</updateRate>
             <topicName>tilt_scan</topicName>
@@ -1820,7 +1860,6 @@
     <map name="sensor" flag="gazebo">
       <verbatim key="sensor_ray">
         <sensor:ray name="base_laser">
-          <gaussianNoise>0.05</gaussianNoise>
           <rayCount>683</rayCount>
           <rangeCount>683</rangeCount>
           <laserCount>1</laserCount>
@@ -1834,6 +1873,7 @@
           <maxRange>10.0</maxRange>
           <updateRate>10.0</updateRate>
           <controller:ros_laser name="ros_base_laser_controller" plugin="libRos_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
 	    <updateRate>10</updateRate>
 	    <topicName>base_scan</topicName>
@@ -2342,9 +2382,8 @@
     </map>
     -->
     <map name="sensor" flag="gazebo">
-      <verbatim key="sensor_ray_block">
-        <sensor:ray_block name="stereo_laser">
-          <gaussianNoise>0.05</gaussianNoise>
+      <verbatim key="sensor_ray">
+        <sensor:ray name="stereo_laser">
           <rayCount>100</rayCount>
           <rangeCount>100</rangeCount>
           <laserCount>1</laserCount>
@@ -2365,13 +2404,14 @@
           <verticalMaxAngle>  0</verticalMaxAngle>
 
           <controller:ros_block_laser name="stereo_laser_controller" plugin="libRos_Block_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
             <updateRate>10.0</updateRate>
             <topicName>full_cloud</topicName>
             <frameName>stereo</frameName>
             <interface:laser name="stereo_laser_iface" />
           </controller:ros_block_laser>
-        </sensor:ray_block>
+        </sensor:ray>
       </verbatim>
     </map>
   

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-09-18 23:24:59 UTC (rev 4448)
@@ -198,7 +198,7 @@
   </controller>
 
   <controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
-    <joint name="tilt_laser_joint" >
+    <joint name="tilt_laser_mount_joint" >
       <pid p="0.4" d="0" i="0" iClamp="0.1" />
     </joint>
   </controller>
@@ -207,7 +207,7 @@
   <controller name="tilt_laser_controller" topic="laser_test" type="LaserScannerControllerNode">
     <velocity>
       <velocityFilter smoothing="0.2"/>
-      <joint name="tilt_laser_joint" type="velocity">
+      <joint name="tilt_laser_mount_joint" type="velocity">
         <pid p="0.56" i = "14" d = "0.000001" iClamp = "0.02" />     
       </joint>
     </velocity>

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml	2008-09-18 23:24:59 UTC (rev 4448)
@@ -116,7 +116,7 @@
       <joint name="head_tilt_joint" >
           <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
       </joint>
-      <joint name="tilt_laser_joint" >
+      <joint name="tilt_laser_mount_joint" >
           <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
       </joint>
 

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-09-18 23:24:59 UTC (rev 4448)
@@ -209,10 +209,15 @@
   
   
   <!-- to be used by sensors -->
-  <const name="torso_tilt_laser_offset_x" value="0.10000" />  <!-- mp 20080801 -->
-  <const name="torso_tilt_laser_offset_z" value="0.19525" />  <!-- mp 20080801 -->
-  <const name="tilt_laser_center_box_center_offset_z" value="0.0" />  <!-- FIXME -->
-  
+  <const name="torso_tilt_laser_mount_offset_x" value="0.10000" />  <!-- mp 20080801 -->
+  <const name="torso_tilt_laser_mount_offset_z" value="0.19525" />  <!-- mp 20080801 -->
+  <const name="tilt_laser_mount_center_box_center_offset_z" value="0.0" />  <!-- FIXME -->
+
+  <!-- from sensor frame to mount bracket frame for tilt laser -->
+  <const name="tilt_laser_mount_tilt_laser_offset_x" value="0.00" /> <!-- FIXME -->
+  <const name="tilt_laser_mount_tilt_laser_offset_z" value="0.03" /> <!-- FIXME -->
+
+
   <const name="base_base_laser_offset_x" value="0.275" />  <!-- mp 20080801 -->
   <const name="base_base_laser_offset_z" value="0.182" />  <!-- mp 20080801 -->
   <const name="base_laser_center_box_center_offset_z" value="0.12" />  <!-- FIXME -->
@@ -800,11 +805,15 @@
   <joint name="caster_front_right_joint" type="revolute" >
     <insert_const_block name="pr2_caster_joint"/>
   </joint>
-  <joint name="tilt_laser_joint" type="revolute"> 
+  <joint name="tilt_laser_mount_joint" type="revolute"> 
     <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
     <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
     <limit min= "-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
   </joint>
+  <joint name="tilt_laser_joint" type="fixed"> 
+    <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+    <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+  </joint>
   <joint name="base_laser_joint" type="fixed"> 
     <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
     <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
@@ -1720,15 +1729,13 @@
   <!-- End head definition -->
   
   
-  <!-- Begin sensor definitions -->
-  <sensor name="tilt_laser" type="laser"> 
+  <link name="tilt_laser_mount"> 
     
-    <calibration filename="calib_filename" />
     <parent name="torso" />
-    <origin xyz=" torso_tilt_laser_offset_x 0 torso_tilt_laser_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    <origin xyz=" torso_tilt_laser_mount_offset_x 0 torso_tilt_laser_mount_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
     <!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
     
-    <joint name="tilt_laser_joint" /> 
+    <joint name="tilt_laser_mount_joint" /> 
     <inertial>
       <mass value="1.0" />
       <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
@@ -1736,19 +1743,52 @@
     </inertial> 
     
     <visual>
-      <origin xyz="0   0   tilt_laser_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <origin xyz="0   0   tilt_laser_mount_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
       <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
       <map name="gazebo_material" flag="gazebo">
         <elem key="material">Gazebo/PioneerBody</elem>
       </map>
-      <geometry name="pr2_tilt_laser_mesh_file">
+      <geometry name="pr2_tilt_laser_mount_mesh_file">
         <mesh filename="hok_tilt" /> <!-- mesh specified using an obj file --> 
       </geometry>
     </visual>
     
     <collision>
-      <origin xyz="0   0   tilt_laser_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <origin xyz="0   0   tilt_laser_mount_center_box_center_offset_z  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
       <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+      <geometry name="tilt_laser_mount_collision_geom" ><!-- geometry specified using a simple geometric object -->
+        <box size=".001 .001 .001" /> 
+      </geometry>
+    </collision>
+    
+  </link>
+
+  <!-- Begin sensor definitions -->
+  <sensor name="tilt_laser" type="laser"> 
+   
+    <calibration filename="calib_filename" />
+    <parent name="tilt_laser_mount" />
+    <origin xyz=" tilt_laser_mount_tilt_laser_offset_x 0 tilt_laser_mount_tilt_laser_offset_z " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    <!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
+    
+    <joint name="tilt_laser_joint" /> 
+    <inertial>
+      <mass value="0.1" />
+      <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+    </inertial> 
+    
+    <visual>
+      <origin xyz="0   0   0  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+      <geometry name="pr2_tilt_laser_visual_geom">
+        <mesh scale="0.001 0.001 0.001" /> <!-- mesh specified using an obj file --> 
+      </geometry>
+    </visual>
+    
+    <collision>
+      <origin xyz="0   0   0  " rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in the sensor's local coordinate frame -->
+      <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
       <geometry name="tilt_laser_collision_geom" ><!-- geometry specified using a simple geometric object -->
         <box size=".001 .001 .001" /> 
       </geometry>
@@ -1757,12 +1797,11 @@
     <map name="sensor" flag="gazebo">
       <verbatim key="sensor_ray">
         <sensor:ray name="tilt_laser">
-          <gaussianNoise>0.05</gaussianNoise>
           <rayCount>683</rayCount>
           <rangeCount>683</rangeCount>
           <laserCount>1</laserCount>
 	  
-          <origin>0.0 0.0 0.05</origin>
+          <origin>0.0 0.0 0.0</origin>
           <displayRays>false</displayRays>
 	  
           <minAngle>-45</minAngle>
@@ -1772,6 +1811,7 @@
           <maxRange>10.0</maxRange>
           <updateRate>10.0</updateRate>
           <controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
             <updateRate>15.0</updateRate>
             <topicName>tilt_scan</topicName>
@@ -1820,7 +1860,6 @@
     <map name="sensor" flag="gazebo">
       <verbatim key="sensor_ray">
         <sensor:ray name="base_laser">
-          <gaussianNoise>0.05</gaussianNoise>
           <rayCount>683</rayCount>
           <rangeCount>683</rangeCount>
           <laserCount>1</laserCount>
@@ -1834,6 +1873,7 @@
           <maxRange>10.0</maxRange>
           <updateRate>10.0</updateRate>
           <controller:ros_laser name="ros_base_laser_controller" plugin="libRos_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
 	    <updateRate>10</updateRate>
 	    <topicName>base_scan</topicName>
@@ -2342,9 +2382,8 @@
     </map>
     -->
     <map name="sensor" flag="gazebo">
-      <verbatim key="sensor_ray_block">
-        <sensor:ray_block name="stereo_laser">
-          <gaussianNoise>0.05</gaussianNoise>
+      <verbatim key="sensor_ray">
+        <sensor:ray name="stereo_laser">
           <rayCount>100</rayCount>
           <rangeCount>100</rangeCount>
           <laserCount>1</laserCount>
@@ -2365,13 +2404,14 @@
           <verticalMaxAngle>  0</verticalMaxAngle>
 
           <controller:ros_block_laser name="stereo_laser_controller" plugin="libRos_Block_Laser.so">
+            <gaussianNoise>0.005</gaussianNoise>
             <alwaysOn>true</alwaysOn>
             <updateRate>10.0</updateRate>
             <topicName>full_cloud</topicName>
             <frameName>stereo</frameName>
             <interface:laser name="stereo_laser_iface" />
           </controller:ros_block_laser>
-        </sensor:ray_block>
+        </sensor:ray>
       </verbatim>
     </map>
   

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml	2008-09-18 23:20:32 UTC (rev 4447)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml	2008-09-18 23:24:59 UTC (rev 4448)
@@ -249,7 +249,7 @@
   </transmission>
   <transmission type="SimpleTransmission" name="tilt_laser_trans">
     <actuator name="tilt_laser_motor" />
-    <joint name="tilt_laser_joint" />
+    <joint name="tilt_laser_mount_joint" />
     <mechanicalReduction>6</mechanicalReduction> <!-- per hw test dl 2008 08 22 -->
     <motorTorqueConstant>-0.538</motorTorqueConstant> <!-- per hw test dl 2008 08 22 -->
     <pulsesPerRevolution>10000</pulsesPerRevolution> <!-- per hw test dl 2008 08 22 -->


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.