diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 2946ad4914cb7fa1bc26f16ee6ac18375eed5ce1..3f49b46f9abfc2cd7ccfad634ef8a204f34a4cf7 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -142,6 +142,9 @@ ros::Timer m_timer_mocap_timeout_check;
// > Time out duration after which Mocap is considered unavailable
float yaml_mocap_timeout_duration = 1.0;
+// VARIABLES FOR GETTING MOTION CAPTURE DATA OF OTHER OBJECTS
+int m_otherObjectPoseDataIndex = -1;
+
// VARIABLES FOR STORING THE PARAMTER OF THE POSITION
@@ -266,7 +269,7 @@ ros::Publisher m_controllerUsedPublisher;
void viconCallback(const ViconData& viconData);
// > For extracting the pose data of an specific
// object by name
-int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose);
+int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index ,CrazyflieData& pose);
// > For converting the global frame motion capture
// data to the local frame of this agent
void coordinatesToLocal(CrazyflieData& cf);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 210946f021a8df09d9eac05a21d7bcef591b4edc..769deba2cf9936a5d1b9fe19db00a7aafa431099 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -88,11 +88,21 @@ void viconCallback(const ViconData& viconData)
// Initilise a variable for the pose data of this agent
CrazyflieData poseDataForThisAgent;
- // Extract the pose data from the full motion capture array
+ // Extract the pose data for the allocated object from the
+ // full motion capture array
// NOTE: that if the return index is a negative then this
// indicates that the pose data was not found.
m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
+ // Initilise a variable for the pose data of another agent
+ CrazyflieData poseDataForOtherAgent;
+ poseDataForOtherAgent.occluded = true;
+
+ // Extract the pose data for the other object from the
+ // full motion capture array
+ // NOTE: that if the return index is a negative then this
+ // indicates that the pose data was not found.
+ //m_otherObjectPoseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, "NameOfOtherObject" , m_otherObjectPoseDataIndex , poseDataForOtherAgent );
// Detecting time-out of the motion capture data
// > Update the flag
@@ -174,7 +184,12 @@ void viconCallback(const ViconData& viconData)
// Fill in the pose data for this agent
controllerCall.request.ownCrazyflie = poseDataForThisAgent;
-
+ // Fill in the pose data of another agaent, if the data
+ // is avaialable
+ if (m_otherObjectPoseDataIndex >= 0)
+ {
+ controllerCall.request.otherCrazyflies.push_back(poseDataForOtherAgent);
+ }
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
@@ -292,7 +307,7 @@ int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::s
)
{
// Check if the names match
- if (viconData.crazyflies[expected_index].crazyflieName == m_context.crazyflieName)
+ if (viconData.crazyflies[expected_index].crazyflieName == name)
{
object_index = expected_index;
}
@@ -305,7 +320,7 @@ int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::s
for( int i=0 ; i<length_poseData ; i++ )
{
// Check if the names match
- if(viconData.crazyflies[i].crazyflieName == m_context.crazyflieName)
+ if(viconData.crazyflies[i].crazyflieName == name)
{
object_index = i;
}
diff --git a/wiki/faq.md b/wiki/faq.md
index 04e4c9ba569d2aa82cc505c8cfdf238ffe76dd9c..b2f13834c8f7e9efbfa38f04cc374ec018e320db 100644
--- a/wiki/faq.md
+++ b/wiki/faq.md
@@ -179,7 +179,7 @@ The ``calculateControlOutput`` function of your ``StudentControllerService.cpp``
```
ros::ServiceServer service = nodeHandle.advertiseService("StudentController", calculateControlOutput);
```
-This service is called on from the ``PPSClient.cpp`` file, and it is expected to adhere to the data structures described in ``/srv/Controller.srv``:
+This service is called on from the ``FlyingAgentClient.cpp`` file, and it is expected to adhere to the data structures described in ``/srv/Controller.srv``:
```
CrazyflieData ownCrazyflie
CrazyflieData[] otherCrazyflies
@@ -188,8 +188,8 @@ ControlCommand controlOutput
```
Hence why the position and attitude information of your own Crazyflie is accessed from inside the ``calculateControlOutput`` function via ``request.ownCrazyflie.{x,y,z,roll,pitch,yaw}``.
-By default the property ``otherCrazyflies`` is left empty when the service request is constructed in the ``PPSClient.cpp``. Thus, in order to have access to the position of another object recognised by the Vicon system you can edit the ``PPSClient.cpp`` as per the following steps:
-- In the ``PPSClient.cpp`` file locate the implementation of the function ``viconCallback``, which has the full prototype:
+By default the property ``otherCrazyflies`` is left empty when the service request is constructed in the ``FlyingAgentClient.cpp``. Thus, in order to have access to the position of another object recognised by the Vicon system you can edit the ``FlyingAgentClient.cpp`` as per the following steps:
+- In the ``FlyingAgentClient.cpp`` file locate the implementation of the function ``viconCallback``, which has the full prototype:
```
void viconCallback(const ViconData& viconData)
```
@@ -211,42 +211,27 @@ float64 acquiringTime #delta t
bool occluded
```
-- At the start of the ``viconCallback`` in the ``PPSClient.cpp`` file (i.e., before the ``for`` loop), add the following varible declaration:
+- Locate the following commented out line near the start of the ``viconCallback`` in the ``FlyingAgentClient.cpp``:
```
-CrazyflieData otherObject;
+//m_otherObjectPoseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, "NameOfOtherObject" , m_otherObjectPoseDataIndex , poseDataForOtherAgent );
```
-where the plan is to fill in this variable with the data about the object of interest and then pass it as part of the service request in the ``.otherCrazyflies`` property.
-- In the ``viconCallback`` function of the ``PPSClient.cpp`` file, just after the variable declaration you added, add the following ``for`` loop:
+- Uncomment this line and change ``"NameOfOtherObject"`` to be the name of the object that zou would like the data of. For the Crazyflie, the format used is ``"CFXX"`` where you replace ``XX`` with the number of the Crazyflie you want (zero padded), i.e., ``"CF01"`` would give you the data for Crazyflie one.
+
+-Now the ``.otherCrazyflies`` property of the ``request`` variable that is passed to the ``calculateControlOutput`` function of your ``StudentControllerService.cpp`` file will contain the position of the ``NameOfOtherObject`` as the first entry in the array, i.e., you can access the data via ``request.otherCrazyflies[0].{x,y,z,roll,pitch,yaw}``.
+
+-IMPORTANT: if the other object is not visible by Vicon for some time, then the ``occluded`` property will indicate this, and the ``{x,y,z,roll,pitch,yaw}`` properties will have garbage values. Hence anywhere you use the data in your code, you should always check its validity with something like the following:
```
-for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+if (request.otherCrazyflies[0].occluded)
{
- CrazyflieData thisObject = *it;
-
- if ( thisObject.crazyflieName == "name_of_object_I_am_searching_for" )
- {
- otherObject = thisObject;
- break;
- }
+ // The data is garbage, do NOT use it
+}
+else
+{
+ // The data is valid and can be used
}
-```
-This for loop iterates over all the objects provided to the ``viconCallback`` function, and fills in the ``otherObject`` variable with the object whose name matches the string ``"name_of_object_I_am_searching_for"``.
-
-- Define a new object via the Vicon Tracker software, give it a meaningful name in the Vicon software, and replace the string ``"name_of_object_I_am_searching_for"`` with the exact string that you used in the Vicon saftware.
-- In the ``viconCallback`` function of the ``PPSClient.cpp`` file, look for the following lines of code:
-```
-Controller controllerCall;
-CrazyflieData local = global;
-coordinatesToLocal(local);
-controllerCall.request.ownCrazyflie = local;
-```
-and immediately after these existing lines of code, add the following new lines of code:
-```
-coordinatesToLocal(otherObject);
-controllerCall.request.otherCrazyflies.push_back(otherObject);
```
-Now the ``.otherCrazyflies`` property of the ``request`` variable that is passed to the ``calculateControlOutput`` function of your ``StudentControllerService.cpp`` file will contain the position of the ``otherObject`` as the first entry in the array, i.e., you can access the data via ``request.otherCrazyflies[0].{x,y,z,roll,pitch,yaw}``.