Feat/swarinfo 539 azimuth fix (#146)

* Remove references to second angle * Fix ros + tests * Fix build
SwarmUS · Oct 7, 2021 · 138b787 · 138b787
1 parent 8740cbf
commit 138b787
Show file tree

Hide file tree

Showing 13 changed files with 21 additions and 49 deletions.
diff --git a/cmake/propolis/common.cmake b/cmake/propolis/common.cmake
@@ -13,7 +13,7 @@ function(propolis_fetch_populate)
         ${PROJECT_NAME}_propolis
 
         GIT_REPOSITORY  https://github.com/SwarmUS/Propolis
-        GIT_TAG         78d4f10
+        GIT_TAG         3cbc543
         GIT_PROGRESS    TRUE
     )
 

diff --git a/src/bittybuzz/buzz_scripts/follow_leader.bzz b/src/bittybuzz/buzz_scripts/follow_leader.bzz
@@ -7,7 +7,7 @@ function tick(context){
         var neighbor = neighbors.get(leader);
 
         if(is_table(neighbor)){
-            var angle = neighbor.elevation;
+            var angle = neighbor.azimuth;
             var x = math.cos(angle);
             var y = math.sin(angle);
             log("Angle: ", angle, ". Moving to leader", leader, " at x:", x, " y:", y);

diff --git a/src/bittybuzz/src/BittyBuzzNeighborsManager.cpp b/src/bittybuzz/src/BittyBuzzNeighborsManager.cpp
@@ -18,9 +18,8 @@ void BittyBuzzNeighborsManager::updateNeighbors() {
                 RelativePosition& pos = posOpt.value();
                 bbzneighbors_elem_t neighbor;
                 neighbor.robot = robotId.value();
-                neighbor.azimuth = bbzfloat_fromfloat(pos.m_relativeOrientation);
+                neighbor.azimuth = bbzfloat_fromfloat(pos.m_angle);
                 neighbor.distance = bbzfloat_fromfloat(pos.m_distance);
-                neighbor.elevation = bbzfloat_fromfloat(pos.m_angle);
                 bbzneighbors_add(&neighbor);
             }
         }

diff --git a/src/bittybuzz/tests/BittyBuzzNeighborsManagerUnitTests.cpp b/src/bittybuzz/tests/BittyBuzzNeighborsManagerUnitTests.cpp
@@ -34,7 +34,7 @@ TEST_F(BittyBuzzNeighborsManagerTestFixture, BittyBuzzNeighborsManager_updateNei
     pos.m_robotId = m_robotId;
     pos.m_distance = 0;
     pos.m_isInLineOfSight = true;
-    pos.m_relativeOrientation = 0;
+    pos.m_angle = 0;
 
     const std::optional<std::reference_wrapper<const uint16_t>> robotId = m_robotId;
     const std::optional<RelativePosition> posOpt = pos;

diff --git a/src/bittybuzz/tests/vm_tests/BittyBuzzVmNeighborsDistanceTests.cpp b/src/bittybuzz/tests/vm_tests/BittyBuzzVmNeighborsDistanceTests.cpp
@@ -29,7 +29,7 @@ TEST_F(BittyBuzzVmTestFixture, BittyBuzzVm_neighborsDistance) {
     pos.m_robotId = robotId;
     pos.m_distance = 42;
     pos.m_isInLineOfSight = true;
-    pos.m_relativeOrientation = 355;
+    pos.m_angle = 355;
 
     EXPECT_CALL(messageHandlerMock, messageQueueLength).Times(1).WillOnce(testing::Return(0));
 
@@ -77,13 +77,13 @@ TEST_F(BittyBuzzVmTestFixture, BittyBuzzVm_neighborsDistance_twiceData) {
     pos1.m_robotId = robotId;
     pos1.m_distance = 40;
     pos1.m_isInLineOfSight = true;
-    pos1.m_relativeOrientation = 300;
+    pos1.m_angle = 300;
 
     RelativePosition pos2;
     pos2.m_robotId = robotId;
     pos2.m_distance = 42;
     pos2.m_isInLineOfSight = true;
-    pos2.m_relativeOrientation = 355;
+    pos2.m_angle = 355;
 
     EXPECT_CALL(messageHandlerMock, messageQueueLength).Times(1).WillOnce(testing::Return(0));
     EXPECT_CALL(messageServiceMock, queueBuzzMessages).WillOnce(testing::Return(true));

diff --git a/src/bsp/include/bsp/InterlocUpdate.h b/src/bsp/include/bsp/InterlocUpdate.h
@@ -12,11 +12,6 @@ struct InterlocUpdate {
      */
     std::optional<float> m_distance;
 
-    /**
-     * @brief Relative orientation in degrees
-     */
-    std::optional<float> m_relativeOrientation;
-
     /**
      * @brief Angle of the distant agent in the frame of the current agent (in degrees)
      */

diff --git a/src/bsp/src/posix/src/InterlocManager.cpp b/src/bsp/src/posix/src/InterlocManager.cpp
@@ -46,15 +46,6 @@ double InterlocManager::getDistance(const tf2::Transform& transform) {
                 pow(transform.getOrigin().z(), 2));
 }
 
-double InterlocManager::getRelativeOrientation(const tf2::Transform& transform) {
-    double roll;
-    double pitch;
-    double yaw;
-    tf2::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
-
-    return yaw;
-}
-
 double InterlocManager::getAngleOfArrival(const tf2::Transform& agentToAgentTransform) {
     return atan2(agentToAgentTransform.getOrigin().y(), agentToAgentTransform.getOrigin().x());
 }
@@ -125,22 +116,19 @@ void InterlocManager::gazeboUpdateCallback(const gazebo_msgs::ModelStates& msg)
         tf2::Transform relTransform = currentAgentTf.inverseTimes(distantAgentTf);
 
         double distance = getDistance(relTransform);
-        double orientation = getRelativeOrientation(relTransform) * 180 / M_PI;
         double angle = getAngleOfArrival(relTransform) * 180 / M_PI;
 
         if (m_positionUpdateCallback != nullptr) {
             InterlocUpdate update;
             update.m_robotId = agentId;
             update.m_distance = distance;
-            update.m_relativeOrientation = orientation;
             update.m_angleOfArrival = angle;
             update.m_isInLineOfSight = true; // TODO: maybe add a way to get LOS
 
             m_positionUpdateCallback(m_positionUpdateContext, update);
 
-            m_logger.log(LogLevel::Debug,
-                         "Updating position of agent %d. Dist: %f, Orientation %f, Angle %f",
-                         agentId, distance, orientation, angle);
+            m_logger.log(LogLevel::Debug, "Updating position of agent %d. Dist: %f, Angle %f",
+                         agentId, distance, angle);
         }
     }
 

diff --git a/src/interloc/include/interloc/RelativePosition.h b/src/interloc/include/interloc/RelativePosition.h
@@ -11,11 +11,6 @@ struct RelativePosition {
      */
     float m_distance;
 
-    /**
-     * @brief Relative orientation in degrees
-     */
-    float m_relativeOrientation;
-
     /**
      * @brief Angle of other agent in current agent frame (in degrees)
      */

diff --git a/src/interloc/src/Interloc.cpp b/src/interloc/src/Interloc.cpp
@@ -67,10 +67,6 @@ void Interloc::updateRobotPosition(RelativePosition& positionToUpdate, InterlocU
         positionToUpdate.m_distance = update.m_distance.value();
     }
 
-    if (update.m_relativeOrientation) {
-        positionToUpdate.m_relativeOrientation = update.m_relativeOrientation.value();
-    }
-
     if (update.m_angleOfArrival) {
         positionToUpdate.m_angle = update.m_angleOfArrival.value();
     }

diff --git a/src/interloc/tests/InterlocUnitTests.cpp b/src/interloc/tests/InterlocUnitTests.cpp
@@ -44,7 +44,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_pushData_robotInList) {
     position.m_robotId = robotId;
     position.m_isInLineOfSight = true;
     position.m_distance = 42;
-    position.m_relativeOrientation = 0;
+    position.m_angleOfArrival = 0;
 
     uint16_t queuePushValue;
     EXPECT_CALL(m_posUpdateQueue, push(testing::_))
@@ -72,7 +72,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_validData) {
     position.m_robotId = robotId;
     position.m_isInLineOfSight = isInLineOfSight;
     position.m_distance = distance;
-    position.m_relativeOrientation = angle;
+    position.m_angleOfArrival = angle;
 
     uint16_t queuePushValue;
     EXPECT_CALL(m_posUpdateQueue, push(testing::_))
@@ -86,7 +86,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_validData) {
 
     // Expect
     EXPECT_EQ(ret->m_distance, distance);
-    EXPECT_EQ(ret->m_relativeOrientation, angle);
+    EXPECT_EQ(ret->m_angle, angle);
     EXPECT_EQ(ret->m_isInLineOfSight, isInLineOfSight);
     EXPECT_EQ(queuePushValue, robotId);
 }
@@ -102,7 +102,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_updateDistance_validData) {
     position1.m_robotId = robotId;
     position1.m_isInLineOfSight = isInLineOfSight;
     position1.m_distance = distance;
-    position1.m_relativeOrientation = angle;
+    position1.m_angleOfArrival = angle;
 
     float updatedDistance = 43;
     InterlocUpdate position2;
@@ -140,12 +140,12 @@ TEST_F(InterlocFixture, Interloc_getPosition_updateOrientation_validData) {
     position1.m_robotId = robotId;
     position1.m_isInLineOfSight = isInLineOfSight;
     position1.m_distance = distance;
-    position1.m_relativeOrientation = angle;
+    position1.m_angleOfArrival = angle;
 
     float updatedAngle = 43;
     InterlocUpdate position2;
     position2.m_robotId = robotId;
-    position2.m_relativeOrientation = updatedAngle;
+    position2.m_angleOfArrival = updatedAngle;
 
     uint16_t queuePushValue1;
     uint16_t queuePushValue2;
@@ -162,7 +162,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_updateOrientation_validData) {
     auto ret = m_interloc->getRobotPosition(robotId);
 
     // Expect
-    EXPECT_EQ(ret->m_relativeOrientation, updatedAngle);
+    EXPECT_EQ(ret->m_angle, updatedAngle);
     EXPECT_EQ(queuePushValue1, robotId);
     EXPECT_EQ(queuePushValue2, robotId);
 }
@@ -216,7 +216,7 @@ TEST_F(InterlocFixture, Interloc_getPosition_updateLineOfSight_validData) {
     position1.m_robotId = robotId;
     position1.m_isInLineOfSight = isInLineOfSight;
     position1.m_distance = distance;
-    position1.m_relativeOrientation = angle;
+    position1.m_angleOfArrival = angle;
 
     bool updatedLOS = false;
     InterlocUpdate position2;

diff --git a/src/message-handler/src/HiveMindHostApiRequestHandler.cpp b/src/message-handler/src/HiveMindHostApiRequestHandler.cpp
@@ -41,8 +41,7 @@ bool HiveMindHostApiRequestHandler::handleHiveMindHostApiRequest(
         if (posOpt) {
 
             const RelativePosition& pos = posOpt.value();
-            posOptDTO = NeighborPositionDTO(pos.m_distance, pos.m_relativeOrientation,
-                                            pos.m_isInLineOfSight);
+            posOptDTO = NeighborPositionDTO(pos.m_distance, pos.m_angle, pos.m_isInLineOfSight);
         }
 
         GetNeighborResponseDTO neighborResp(id, posOptDTO);

diff --git a/src/message-handler/tests/HiveMindHostApiRequestHandlerUnitTests.cpp b/src/message-handler/tests/HiveMindHostApiRequestHandlerUnitTests.cpp
@@ -60,7 +60,7 @@ TEST_F(HiveMindHostApiRequestHandlerFixture,
     // Given
     RelativePosition pos;
     pos.m_distance = 1;
-    pos.m_relativeOrientation = 1;
+    pos.m_angle = 1;
     pos.m_robotId = 42;
     pos.m_isInLineOfSight = true;
 
@@ -109,7 +109,7 @@ TEST_F(HiveMindHostApiRequestHandlerFixture,
     PositionsTable posTable;
     posTable.m_positionsLength = 1;
     posTable.m_positions[0].m_distance = 42;
-    posTable.m_positions[0].m_relativeOrientation = 24;
+    posTable.m_positions[0].m_angle = 24;
     posTable.m_positions[0].m_robotId = 8;
     posTable.m_positions[0].m_isInLineOfSight = true;
 

diff --git a/src/message-handler/tests/utils/DTOMatchers.h b/src/message-handler/tests/utils/DTOMatchers.h
@@ -20,7 +20,7 @@ MATCHER_P(MessageGetNeighborResponseDTOMatcher,
     bool match = getNeighbor.getNeighborId() == pos.m_robotId;
     auto posOpt = getNeighbor.getNeighborPosition();
     match = match && pos.m_distance == posOpt.value().getDistance();
-    match = match && pos.m_relativeOrientation == posOpt.value().getRelativeOrientation();
+    match = match && pos.m_angle == posOpt.value().getAzimuth();
     match = match && pos.m_isInLineOfSight == posOpt.value().inLOS();
     return match;
 }