PR Feedback

Signed-off-by: Martiño Crespo <[email protected]>

src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc

@@ -328,15 +328,14 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
     }
   }
 
-  // Advertise topics for normal forces
+  // Advertise topic for normal forces
   std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces";
   this->dataPtr->normalForcesPub =
     this->dataPtr->node.Advertise<ignition::msgs::Image>(normalForcesTopic);
   if (!this->dataPtr->normalForcesPub)
   {
     ignerr << "Error advertising topic [" << normalForcesTopic << "]"
       << std::endl;
-    return;
   }
 
   // Advertise enabling service
@@ -351,7 +350,6 @@ void OpticalTactilePlugin::Configure(const Entity &_entity,
   {
     ignerr << "Error advertising service [" << enableService << "]"
       << std::endl;
-    return;
   }
 }
 
@@ -756,7 +754,7 @@ void OpticalTactilePluginPrivate::ComputeNormalForces(
       // Add force to buffer
       // Forces buffer is composed of XYZ coordinates, while _msg buffer is
       // made up of XYZRGB values
-      bufferIndex = j*(_msg.row_step()/2) + i*(_msg.point_step()/2);
+      bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2);
       normalForcesBuffer[bufferIndex] = normalForce.X();
       normalForcesBuffer[bufferIndex + sizeof(float)] = normalForce.Y();
       normalForcesBuffer[bufferIndex + 2 * sizeof(float)] = normalForce.Z();

src/systems/optical_tactile_plugin/Visualization.cc

@@ -118,11 +118,11 @@ void OpticalTactilePluginVisualization::AddContactToMarkerMsg(
 
   for (auto const &position : _contact.position())
   {
-    ignition::math::Vector3d firstPoint = ignition::msgs::Convert(position);
-    ignition::math::Vector3d secondPoint = firstPoint + contactNormal;
+    ignition::math::Vector3d startPoint = ignition::msgs::Convert(position);
+    ignition::math::Vector3d endPoint = startPoint + contactNormal;
 
-    ignition::msgs::Set(_contactMarkerMsg.add_point(), firstPoint);
-    ignition::msgs::Set(_contactMarkerMsg.add_point(), secondPoint);
+    ignition::msgs::Set(_contactMarkerMsg.add_point(), startPoint);
+    ignition::msgs::Set(_contactMarkerMsg.add_point(), endPoint);
   }
 }
 
@@ -211,40 +211,40 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs(
     (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose;
   normalForcePoseFromWorld.Correct();
 
-  // Get the first point of the normal force
-  ignition::math::Vector3f firstPointFromWorld =
+  // Get the start point of the normal force
+  ignition::math::Vector3f startPointFromWorld =
     normalForcePoseFromWorld.Pos();
 
   // Move the normal force pose a distance of forceLength along the direction
-  // of _normalForce and get the second point
+  // of _normalForce and get the end point
   normalForcePoseFromSensor.Set(normalForcePositionFromSensor +
     _normalForce * this->forceLength, normalForceOrientationFromSensor);
 
   normalForcePoseFromWorld =
     (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose;
   normalForcePoseFromWorld.Correct();
 
-  ignition::math::Vector3f secondPointFromWorld =
+  ignition::math::Vector3f endPointFromWorld =
     normalForcePoseFromWorld.Pos();
 
   // Check invalid points to avoid data transfer overhead
-  if (firstPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) ||
-      secondPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0))
+  if (startPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) ||
+      endPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0))
     return;
 
   // Add points to the messages
 
   ignition::msgs::Set(_positionMarkerMsg.add_point(),
-    ignition::math::Vector3d(firstPointFromWorld.X(),
-      firstPointFromWorld.Y(), firstPointFromWorld.Z()));
+    ignition::math::Vector3d(startPointFromWorld.X(),
+      startPointFromWorld.Y(), startPointFromWorld.Z()));
 
   ignition::msgs::Set(_forceMarkerMsg.add_point(),
-    ignition::math::Vector3d(firstPointFromWorld.X(),
-      firstPointFromWorld.Y(), firstPointFromWorld.Z()));
+    ignition::math::Vector3d(startPointFromWorld.X(),
+      startPointFromWorld.Y(), startPointFromWorld.Z()));
 
   ignition::msgs::Set(_forceMarkerMsg.add_point(),
-    ignition::math::Vector3d(secondPointFromWorld.X(),
-      secondPointFromWorld.Y(), secondPointFromWorld.Z()));
+    ignition::math::Vector3d(endPointFromWorld.X(),
+      endPointFromWorld.Y(), endPointFromWorld.Z()));
 }
 
 //////////////////////////////////////////////////

test/integration/CMakeLists.txt

@@ -46,7 +46,7 @@ set(tests
 set(tests_needing_display
   depth_camera.cc
   gpu_lidar.cc
-  optical_tactile_sensor_plugin.cc
+  optical_tactile_plugin.cc
   rgbd_camera.cc
   sensors_system.cc
 )

test/integration/optical_tactile_sensor_plugin.cc → test/integration/optical_tactile_plugin.cc

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019 Open Source Robotics Foundation
+ * Copyright (C) 2020 Open Source Robotics Foundation
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -93,7 +93,7 @@ class OpticalTactilePluginTest : public ::testing::Test
 // The test checks the normal forces on the corners of the box-shaped sensor
 TEST_F(OpticalTactilePluginTest, ForcesOnPlane)
 {
-  this->StartServer("/test/worlds/optical_tactile_sensor_plugin.sdf");
+  this->StartServer("/test/worlds/optical_tactile_plugin.sdf");
 
   math::Vector3f upperLeftNormalForce(0, 0, 0);
   math::Vector3f upperRightNormalForce(0, 0, 0);

test/worlds/optical_tactile_sensor_plugin.sdf → test/worlds/optical_tactile_plugin.sdf

@@ -44,7 +44,7 @@
 
     <model name="tactile_sensor">
       <pose>0 0 0.0030 0 1.57 -1.57</pose>
-      <link name="link">
+      <link name="contact_surface">
         <collision name="collision">
           <geometry>
             <box>
@@ -55,7 +55,7 @@
         <sensor name="depth_camera" type="depth_camera">
           <update_rate>1</update_rate>
           <topic>depth_camera</topic>
-          <pose relative_to="tactile_sensor">-0.05 0 0 0 0 0</pose>
+          <pose relative_to="contact_surface">-0.05 0 0 0 0 0</pose>
           <camera>
             <image>
               <width>640</width>
@@ -73,7 +73,6 @@
             <collision>collision</collision>
           </contact>
         </sensor>
-
       </link>
       <static>false</static>
       <plugin