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

[<tf...@us...>]

Revision: 4771
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4771&view=rev
Author:   tfoote
Date:     2008-09-29 18:37:43 +0000 (Mon, 29 Sep 2008)

Log Message:
-----------
adding stl compatibility 2dnav-stage stack.  I have tested this code with both vectors branch and trunk for 2dnav-stage.  it works for both.

Modified Paths:
--------------
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
    pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
    pkg/trunk/util/rosTF/src/rosTF.cpp
    pkg/trunk/util/tf/include/tf/transform_listener.h
    pkg/trunk/util/tf/test/tf_unittest.cpp

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-09-29 18:37:43 UTC (rev 4771)
@@ -245,7 +245,7 @@
     assert(mc_->model_.joints_.size() == mechanism_state_.get_joint_states_size());
     for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
     {
-      mechanism_control::JointState *out = mechanism_state_.joint_states + i;
+      mechanism_control::JointState *out = &mechanism_state_.joint_states[i];
       mechanism::JointState *in = &mc_->state_->joint_states_[i];
       out->name = mc_->model_.joints_[i]->name_;
       out->position = in->position_;
@@ -256,7 +256,7 @@
 
     for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
     {
-      mechanism_control::ActuatorState *out = mechanism_state_.actuator_states + i;
+      mechanism_control::ActuatorState *out = &mechanism_state_.actuator_states[i];
       ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
       out->name = mc_->hw_->actuators_[i]->name_;
       out->encoder_count = in->encoder_count_;

Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-09-29 18:37:43 UTC (rev 4771)
@@ -35,13 +35,13 @@
   
   void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative)
   {
-    NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+    NEWMAT::Matrix ranges(2, scan_in.get_ranges_size());
     double * matPointer = ranges.Store();
     // Fill the ranges matrix
-    for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+    for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
       {
         matPointer[index] = (double) scan_in.ranges[index];
-        matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+        matPointer[index+scan_in.get_ranges_size()] = (double) scan_in.ranges[index];
       }
     
 
@@ -51,12 +51,12 @@
 
     //Stuff the output cloud
     cloud_out.header = scan_in.header;
-    cloud_out.set_pts_size(scan_in.ranges_size);
-    if (scan_in.intensities_size > 0)
+    cloud_out.set_pts_size(scan_in.get_ranges_size());
+    if (scan_in.get_intensities_size() > 0)
       {
         cloud_out.set_chan_size(1);
         cloud_out.chan[0].name ="intensities";
-        cloud_out.chan[0].set_vals_size(scan_in.intensities_size);
+        cloud_out.chan[0].set_vals_size(scan_in.get_intensities_size());
       }
 
     double* outputMat = output.Store();
@@ -67,25 +67,25 @@
       range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
     
     unsigned int count = 0;
-    for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+    for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
     {
       if (!preservative){ //Default behaviour will throw out invalid data
         if ((matPointer[index] < range_cutoff) &&
             (matPointer[index] > scan_in.range_min)) //only valid
         {
           cloud_out.pts[count].x = outputMat[index];
-          cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+          cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
           cloud_out.pts[count].z = 0.0;
-          if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+          if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
             cloud_out.chan[0].vals[count] = scan_in.intensities[index];
           count++;
         }
       }
       else { //Keep all points
         cloud_out.pts[count].x = outputMat[index];
-        cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+        cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
         cloud_out.pts[count].z = 0.0;
-        if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+        if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
           cloud_out.chan[0].vals[count] = scan_in.intensities[index];
         count++;
       }
@@ -99,13 +99,13 @@
   };
     void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff, bool preservative)
   {
-    NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+    NEWMAT::Matrix ranges(2, scan_in.get_ranges_size());
     double * matPointer = ranges.Store();
     // Fill the ranges matrix
-    for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+    for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
       {
         matPointer[index] = (double) scan_in.ranges[index];
-        matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+        matPointer[index+scan_in.get_ranges_size()] = (double) scan_in.ranges[index];
       }
     
 
@@ -115,12 +115,12 @@
 
     //Stuff the output cloud
     cloud_out.header = scan_in.header;
-    cloud_out.set_pts_size(scan_in.ranges_size);
-    if (scan_in.intensities_size > 0)
+    cloud_out.set_pts_size(scan_in.get_ranges_size());
+    if (scan_in.get_intensities_size() > 0)
       {
         cloud_out.set_chan_size(1);
         cloud_out.chan[0].name ="intensities";
-        cloud_out.chan[0].set_vals_size(scan_in.intensities_size);
+        cloud_out.chan[0].set_vals_size(scan_in.get_intensities_size());
       }
 
     double* outputMat = output.Store();
@@ -131,25 +131,25 @@
       range_cutoff = std::min(range_cutoff, (double)scan_in.range_max); 
     
     unsigned int count = 0;
-    for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+    for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
     {
       if (!preservative){ //Default behaviour will throw out invalid data
         if ((matPointer[index] < range_cutoff) &&
             (matPointer[index] > scan_in.range_min)) //only valid
         {
           cloud_out.pts[count].x = outputMat[index];
-          cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+          cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
           cloud_out.pts[count].z = 0.0;
-          if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+          if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
             cloud_out.chan[0].vals[count] = scan_in.intensities[index];
           count++;
         }
       }
       else { //Keep all points
         cloud_out.pts[count].x = outputMat[index];
-        cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+        cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
         cloud_out.pts[count].z = 0.0;
-        if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+        if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
           cloud_out.chan[0].vals[count] = scan_in.intensities[index];
         count++;
       }
@@ -213,7 +213,7 @@
     temp_scan_ = scan_in; //HACK to store all metadata 
 
     /** \todo check for length of intensities too */
-    unsigned int iterations = std::min(scan_in.ranges_size, num_ranges_);
+    unsigned int iterations = std::min(scan_in.get_ranges_size(), num_ranges_);
     for (unsigned int index = 0; index < iterations; index ++)
       {
         range_data_(current_packet_num_+1, index+1)= (double) scan_in.ranges[index];
@@ -238,7 +238,7 @@
     NEWMAT::ColumnVector iColumn;
 
 
-    unsigned int iterations = std::min(scan_result.ranges_size, num_ranges_);
+    unsigned int iterations = std::min(scan_result.get_ranges_size(), num_ranges_);
     /** \todo Resize output cloud/check length */
     for (unsigned int index = 0; index < iterations; index ++)
       {

Modified: pkg/trunk/util/rosTF/src/rosTF.cpp
===================================================================
--- pkg/trunk/util/rosTF/src/rosTF.cpp	2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/rosTF/src/rosTF.cpp	2008-09-29 18:37:43 UTC (rev 4771)
@@ -101,12 +101,12 @@
 {
   cloudOut.header = scanIn.header;
   cloudOut.header.frame_id = target_frame;
-  cloudOut.set_pts_size(scanIn.ranges_size);
-    if (scanIn.intensities_size > 0)
+  cloudOut.set_pts_size(scanIn.get_ranges_size());
+    if (scanIn.get_intensities_size() > 0)
       {
         cloudOut.set_chan_size(1);
         cloudOut.chan[0].name ="intensities";
-        cloudOut.chan[0].set_vals_size(scanIn.intensities_size);
+        cloudOut.chan[0].set_vals_size(scanIn.get_intensities_size());
       }
 
   libTF::TFPoint pointIn;
@@ -119,7 +119,7 @@
   projector_.projectLaser(scanIn, intermediate, -1.0, true);
   
   unsigned int count = 0;  
-  for (unsigned int i = 0; i < scanIn.ranges_size; i++)
+  for (unsigned int i = 0; i < scanIn.get_ranges_size(); i++)
   {
     if (scanIn.ranges[i] < scanIn.range_max 
         && scanIn.ranges[i] > scanIn.range_min) //only when valid
@@ -135,7 +135,7 @@
       cloudOut.pts[count].y  = pointOut.y;
       cloudOut.pts[count].z  = pointOut.z;
       
-      if (scanIn.intensities_size >= i) /// \todo optimize and catch length difference better
+      if (scanIn.get_intensities_size() >= i) /// \todo optimize and catch length difference better
 	  cloudOut.chan[0].vals[count] = scanIn.intensities[i];
       count++;
     }
@@ -151,7 +151,7 @@
 
 void rosTFClient::receiveArray()
 {
-  for (unsigned int i = 0; i < tfArrayIn.eulers_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_eulers_size(); i++)
   {
     try{
       setWithEulers(tfArrayIn.eulers[i].header.frame_id, tfArrayIn.eulers[i].parent, tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z, tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll, tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec);
@@ -163,7 +163,7 @@
     };
   }
   //std::cout << "received euler frame: " << tfArrayIn.eulers[i].header.frame_id << " with parent:" << tfArrayIn.eulers[i].parent << "time " << tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
-  for (unsigned int i = 0; i < tfArrayIn.dhparams_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_dhparams_size(); i++)
   {
     try{
     setWithDH(tfArrayIn.dhparams[i].header.frame_id, tfArrayIn.dhparams[i].parent, tfArrayIn.dhparams[i].length, tfArrayIn.dhparams[i].twist, tfArrayIn.dhparams[i].offset, tfArrayIn.dhparams[i].angle, tfArrayIn.dhparams[i].header.stamp.sec * 1000000000ULL + tfArrayIn.dhparams[i].header.stamp.nsec);
@@ -176,7 +176,7 @@
     //  std::cout << "recieved DH frame: " << tfArrayIn.dhparams[i].header.frame_id << " with parent:" << tfArrayIn.dhparams[i].parent << std::endl;
   }
   
-  for (unsigned int i = 0; i < tfArrayIn.quaternions_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_quaternions_size(); i++)
   {
     try{
     setWithQuaternion(tfArrayIn.quaternions[i].header.frame_id, tfArrayIn.quaternions[i].parent, tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt, tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w, tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec);
@@ -188,18 +188,18 @@
     };
     //  std::cout << "recieved quaternion frame: " << tfArrayIn.quaternions[i].header.frame_id << " with parent:" << tfArrayIn.quaternions[i].parent << std::endl;
   }
-  for (unsigned int i = 0; i < tfArrayIn.matrices_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_matrices_size(); i++)
   {
       
     try{
-    if (tfArrayIn.matrices[i].matrix_size != 16)
+    if (tfArrayIn.matrices[i].get_matrix_size() != 16)
     {
-      std::cerr << "recieved matrix not of size 16, it was "<< tfArrayIn.matrices[i].matrix_size;
+      std::cerr << "recieved matrix not of size 16, it was "<< tfArrayIn.matrices[i].get_matrix_size();
       return;
     }
       
     NEWMAT::Matrix tempMatrix(4,4);
-    tempMatrix << tfArrayIn.matrices[i].matrix;
+    tempMatrix << (tfArrayIn.matrices[i].matrix[0]);
     
     setWithMatrix(tfArrayIn.matrices[i].header.frame_id, tfArrayIn.matrices[i].parent, tempMatrix, tfArrayIn.matrices[i].header.stamp.sec * 1000000000ULL + tfArrayIn.matrices[i].header.stamp.nsec);
     

Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h	2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h	2008-09-29 18:37:43 UTC (rev 4771)
@@ -93,7 +93,7 @@
   ///\todo Remove : for backwards compatability only
 void receiveArray()
 {
-  for (unsigned int i = 0; i < tfArrayIn.eulers_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_eulers_size(); i++)
   {
     try{
       //      setWithEulers(tfArrayIn.eulers[i].header.frame_id, tfArrayIn.eulers[i].parent, tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z, tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll, tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec);
@@ -110,12 +110,12 @@
     };
   }
   //std::cout << "received euler frame: " << tfArrayIn.eulers[i].header.frame_id << " with parent:" << tfArrayIn.eulers[i].parent << "time " << tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
-  for (unsigned int i = 0; i < tfArrayIn.dhparams_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_dhparams_size(); i++)
   {
     std::cerr << "receiveArray: setWithDH failed No longer supported" << std::endl;
   }
   
-  for (unsigned int i = 0; i < tfArrayIn.quaternions_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_quaternions_size(); i++)
   {
     try{
       //    setWithQuaternion(tfArrayIn.quaternions[i].header.frame_id, tfArrayIn.quaternions[i].parent, tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt, tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w, tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec);
@@ -132,7 +132,7 @@
     };
     //  std::cout << "recieved quaternion frame: " << tfArrayIn.quaternions[i].header.frame_id << " with parent:" << tfArrayIn.quaternions[i].parent << std::endl;
   }
-  for (unsigned int i = 0; i < tfArrayIn.matrices_size; i++)
+  for (unsigned int i = 0; i < tfArrayIn.get_matrices_size(); i++)
   {
     std::cerr << "receiveArray: setWithMatrix failed No longer supported" << std::endl;
   }

Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp	2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp	2008-09-29 18:37:43 UTC (rev 4771)
@@ -166,6 +166,15 @@
     }
   }
   
+  Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), runs, "child");
+  Stamped<btTransform> outpose;
+  outpose.data_.setIdentity(); //to make sure things are getting mutated
+  mTR.transformStamped("child",inpose, outpose);
+  EXPECT_NEAR(outpose.data_.getOrigin().x(), 0, epsilon);
+  EXPECT_NEAR(outpose.data_.getOrigin().y(), 0, epsilon);
+  EXPECT_NEAR(outpose.data_.getOrigin().z(), 0, epsilon);
+  
+  
 }
 
 TEST(tf, TransformVector3Cartesian)


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