From 081246faa71266f93145c1a44558cdd640eee620 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 13 Apr 2020 13:52:42 -0400 Subject: [PATCH 1/2] Minor fixes to light sensor: wrong angle bug. --- .../simulator/kheperaiv_light_rotzonly_sensor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp b/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp index 401389f..ecc6cbd 100644 --- a/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp +++ b/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp @@ -19,6 +19,7 @@ namespace argos { static CRange SENSOR_RANGE(0.0f, 1.0f); static CRadians SENSOR_SPACING = CRadians(ARGOS_PI / 12.0f); + static CRadians SENSOR_HALF_SPACING = SENSOR_SPACING * 0.5; /****************************************/ /****************************************/ @@ -157,16 +158,16 @@ namespace argos { * Division says how many sensor spacings there are between first sensor and point at which ray hits kheperaiv body * Increase magnitude of result of division to ensure correct rounding */ - Real fIdx = cAngleLightWrtRobot / SENSOR_SPACING; + Real fIdx = (cAngleLightWrtRobot - SENSOR_HALF_SPACING) / SENSOR_SPACING; SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f; /* Set the actual readings */ Real fReading = cRobotToLight.Length(); /* - * Take 2 readings before closest sensor and 2 readings after - thus we + * Take 6 readings before closest sensor and 6 readings after - thus we * process sensors that are with 180 degrees of intersection of light * ray with robot body */ - for(SInt32 nIndexOffset = -2; nIndexOffset < 3; ++nIndexOffset) { + for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) { UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 8); CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtRobot - m_tReadings[unIdx].Angle).SignedNormalize()); /* From 51e70d24472c0d63065dbff6f694be5b0250e9d0 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 13 Apr 2020 19:11:23 -0400 Subject: [PATCH 2/2] Light sensor fixes. --- .../simulator/kheperaiv_light_rotzonly_sensor.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp b/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp index ecc6cbd..a3abadc 100644 --- a/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp +++ b/src/plugins/robots/kheperaiv/simulator/kheperaiv_light_rotzonly_sensor.cpp @@ -158,8 +158,9 @@ namespace argos { * Division says how many sensor spacings there are between first sensor and point at which ray hits kheperaiv body * Increase magnitude of result of division to ensure correct rounding */ - Real fIdx = (cAngleLightWrtRobot - SENSOR_HALF_SPACING) / SENSOR_SPACING; + Real fIdx = (cAngleLightWrtRobot) / SENSOR_SPACING; SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f; + /* Set the actual readings */ Real fReading = cRobotToLight.Length(); /* @@ -167,7 +168,7 @@ namespace argos { * process sensors that are with 180 degrees of intersection of light * ray with robot body */ - for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) { + for(SInt32 nIndexOffset = -4; nIndexOffset < 5; ++nIndexOffset) { UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 8); CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtRobot - m_tReadings[unIdx].Angle).SignedNormalize()); /* @@ -176,7 +177,8 @@ namespace argos { * 0) to 0 (dist PI/2) */ m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint); - } + } + } else { /* The ray is occluded */