Revision: 7497
http://playerstage.svn.sourceforge.net/playerstage/?rev=7497&view=rev
Author: hsujohnhsu
Date: 2009-03-17 00:52:22 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
update RaySensor to support block of ray with sweeps in yaw and pitch.
Modified Paths:
--------------
code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.cc
code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.hh
Modified: code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.cc
2009-03-17 00:48:50 UTC (rev 7496)
+++ code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.cc
2009-03-17 00:52:22 UTC (rev 7497)
@@ -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);
+
+ // 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();
}
@@ -80,6 +86,11 @@
delete this->maxRangeP;
delete this->originP;
delete this->displayRaysP;
+
+ delete this->verticalRayCountP;
+ delete this->verticalRangeCountP;
+ delete this->verticalMinAngleP;
+ delete this->verticalMaxAngleP;
}
//////////////////////////////////////////////////////////////////////////////
@@ -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
@@ -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";
}
//////////////////////////////////////////////////////////////////////////////
@@ -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;
+ double pDiff = (**(this->verticalMaxAngleP) -
**(this->verticalMinAngleP)).GetAsRadian();
+ double yDiff = (**(this->maxAngleP) - **(this->minAngleP)).GetAsRadian();
// 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--)
+ {
- axis.Set(cos(angle), sin(angle),0);
+ yawAngle = (rayCountP->GetValue() == 1)? 0 : i * yDiff /
(rayCountP->GetValue() - 1) + (**(this->minAngleP)).GetAsRadian();
- start = (axis * this->minRangeP->GetValue()) + this->originP->GetValue();
- end = (axis * this->maxRangeP->GetValue()) + this->originP->GetValue();
+ pitchAngle = (verticalRayCountP->GetValue() == 1)? 0 : 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 );
+
+ this->rays.push_back(ray);
+
+ //this->body->AttachGeom(ray);
+ }
}
}
@@ -223,6 +249,34 @@
}
//////////////////////////////////////////////////////////////////////////////
+/// Get the vertical scan line count
+int RaySensor::GetVerticalRayCount() const
+{
+ return this->verticalRayCountP->GetValue();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Get the vertical scan line count
+int RaySensor::GetVerticalRangeCount() const
+{
+ return this->verticalRangeCountP->GetValue();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Get the vertical min angle
+Angle RaySensor::GetVerticalMinAngle() const
+{
+ return this->verticalMinAngleP->GetValue();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Get the vertical max angle
+Angle RaySensor::GetVerticalMaxAngle() const
+{
+ return this->verticalMaxAngleP->GetValue();
+}
+
+//////////////////////////////////////////////////////////////////////////////
// Get detected range for a ray
double RaySensor::GetRange(int index)
{
Modified: code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.hh
2009-03-17 00:48:50 UTC (rev 7496)
+++ code/gazebo/branches/ogre-1.4.9/server/sensors/ray/RaySensor.hh
2009-03-17 00:52:22 UTC (rev 7497)
@@ -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;
+
+ /// \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
@@ -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;
};
/// \}
/// \}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
