WIP: Fix Sonar Publishing

p2os_driver/include/p2os_driver/sip.hpp

@@ -25,6 +25,7 @@
 
 #include <climits>
 #include <cstdint>
+#include <memory>
 #include <string>
 
 typedef struct ArmJoint
@@ -66,7 +67,7 @@ class SIP
   uint16_t ptu, compass, timer, rawxpos;
   uint16_t rawypos, frontbumpers, rearbumpers;
   int16_t angle, lvel, rvel, control;
-  uint16_t * sonars;
+  std::unique_ptr<double[]> sonars;
   int xpos, ypos;
   int x_offset, y_offset, angle_offset;
   std::string odom_frame_id;
@@ -115,12 +116,12 @@ class SIP
   // void FillArm(player_p2os_data_t* data);
 
   explicit SIP(int idx)
-  : param_idx(idx), sonarreadings(0), sonars(NULL),
+  : param_idx(idx), sonarreadings(0), sonars(nullptr),
     xpos(0), ypos(0), x_offset(0), y_offset(0), angle_offset(0),
     blobmx(0), blobmy(0), blobx1(0), blobx2(0), bloby1(0), bloby2(0),
     blobarea(0), blobconf(0), blobcolor(0),
-    armPowerOn(false), armConnected(false), armVersionString(NULL),
-    armNumJoints(0), armJoints(NULL),
+    armPowerOn(false), armConnected(false), armVersionString(nullptr),
+    armNumJoints(0), armJoints(nullptr),
     lastLiftPos(0.0f)
   {
     for (int i = 0; i < 6; ++i) {
@@ -131,10 +132,7 @@ class SIP
     }
   }
 
-  ~SIP()
-  {
-    delete[] sonars;
-  }
+  ~SIP() {}
 };
 
 #endif  // P2OS_DRIVER__SIP_HPP_

p2os_driver/src/p2os.cpp

@@ -721,7 +721,7 @@ int P2OSNode::SendReceive(P2OSPacket * pkt, bool publish_data)
       if (ptz_.isOn()) {
         int len = packet.packet[2] - 3;
         if (ptz_.cb_.gotPacket()) {
-          ROS_ERROR("PTZ got a message, but alread has the complete packet.");
+          ROS_ERROR("PTZ got a message, but already has the complete packet.");
         } else {
           for (int i = 4; i < 4 + len; ++i) {
             ptz_.cb_.putOnBuf(packet.packet[i]);

p2os_driver/src/sip.cpp

@@ -20,6 +20,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <p2os_driver/sip.hpp>
 
 #include <stdio.h>
 #include <limits.h>
@@ -28,10 +29,12 @@
 #include <stdlib.h> /* for abs() */
 #include <unistd.h>
 
-#include <p2os_driver/sip.hpp>
 #include <tf/tf.h>
 #include <tf/transform_datatypes.h>
+
+#include <memory>
 #include <sstream>
+#include <utility>
 
 void SIP::FillStandard(ros_p2os_data_t * data)
 {
@@ -241,8 +244,6 @@ int SIP::PositionChange(uint16_t from, uint16_t to)
 
 void SIP::Print()
 {
-  int i;
-
   ROS_DEBUG("lwstall:%d rwstall:%d\n", lwstall, rwstall);
 
   std::stringstream front_bumper_info;
@@ -260,19 +261,19 @@ void SIP::Print()
 
   ROS_DEBUG("status: 0x%x analog: %d param_id: %d ", status, analog, param_idx);
   std::stringstream status_info;
-  for (i = 0; i < 11; i++) {
+  for (int i = 0; i < 11; i++) {
     status_info << " " <<
       static_cast<int>((status >> (7 - i) ) & 0x01);
   }
   ROS_DEBUG("status:%s", status_info.str().c_str());
   std::stringstream digin_info;
-  for (i = 0; i < 8; i++) {
+  for (int i = 0; i < 8; i++) {
     digin_info << " " <<
       static_cast<int>((digin >> (7 - i) ) & 0x01);
   }
   ROS_DEBUG("digin:%s", digin_info.str().c_str());
   std::stringstream digout_info;
-  for (i = 0; i < 8; i++) {
+  for (int i = 0; i < 8; i++) {
     digout_info << " " <<
       static_cast<int>((digout >> (7 - i) ) & 0x01);
   }
@@ -291,13 +292,14 @@ void SIP::Print()
 void SIP::PrintSonars()
 {
   if (sonarreadings <= 0) {
+    ROS_WARN("sonarreadings <= 0");
     return;
   }
   std::stringstream sonar_info;
   for (int i = 0; i < sonarreadings; i++) {
-    sonar_info << " " << static_cast<int>(sonars[i]);
+    sonar_info << " " << sonars[i];
   }
-  ROS_DEBUG("Sonars: %s", sonar_info.str().c_str());
+  ROS_INFO("Sonars: %s", sonar_info.str().c_str());
 }
 
 void SIP::PrintArm()
@@ -425,23 +427,24 @@ void SIP::ParseStandard(unsigned char * buffer)
     unsigned char maxSonars = sonarreadings;
     for (unsigned char i = 0; i < numSonars; i++) {
       unsigned char sonarIndex = buffer[cnt + i * (sizeof(unsigned char) + sizeof(uint16_t))];
-      if ((sonarIndex + 1) > maxSonars) {maxSonars = sonarIndex + 1;}
+      if ((sonarIndex + 1) > maxSonars) {
+        maxSonars = sonarIndex + 1;
+      }
     }
 
     // if necessary make more space in the array and preserve existing readings
     if (maxSonars > sonarreadings) {
-      uint16_t * newSonars = new uint16_t[maxSonars];
+      auto newSonars = std::make_unique<double[]>(maxSonars);
       for (unsigned char i = 0; i < sonarreadings; i++) {
         newSonars[i] = sonars[i];
       }
-      if (sonars != NULL) {delete[] sonars;}
-      sonars = newSonars;
+      sonars = std::move(newSonars);
       sonarreadings = maxSonars;
     }
 
     // update the sonar readings array with the new readings
     for (unsigned char i = 0; i < numSonars; i++) {
-      sonars[buffer[cnt]] = static_cast<uint16_t>(
+      sonars[buffer[cnt]] = static_cast<double>(
         rint((buffer[cnt + 1] | (buffer[cnt + 2] << 8)) *
         PlayerRobotParams[param_idx].RangeConvFactor));
       cnt += sizeof(unsigned char) + sizeof(uint16_t);
@@ -460,8 +463,8 @@ void SIP::ParseStandard(unsigned char * buffer)
   digout = buffer[cnt];
   cnt += sizeof(unsigned char);
   // for debugging:
-  Print();
-  // PrintSonars();
+  // Print();
+  PrintSonars();
 }
 
 /** Parse a SERAUX SIP packet.  For a CMUcam, this will have blob