fix bug in frame system serialization

motionplan/armplanning/data/plan_request_sample.json

@@ -327,7 +327,10 @@
         }
       }
     },
-    "parents": null
+    "parents": {
+      "xArm6": "world",
+      "xArmVgripper": "xArm6"
+    }
   },
   "goals": [
     {
@@ -428,7 +431,7 @@
     "logging_interval": 0,
     "timeout": 300,
     "smooth_iter": 100,
-    "num_threads": 8,
+    "num_threads": 6,
     "goal_threshold": 0.1,
     "plan_iter": 1500,
     "frame_step": 0.01,

referenceframe/frame_system.go

@@ -458,9 +458,10 @@ func (sfs *FrameSystem) MarshalJSON() ([]byte, error) {
 		typedFrames[name] = frameJSON
 	}
 	serializedFS := serializableFrameSystem{
-		Name:   sfs.name,
-		World:  worldFrameJSON,
-		Frames: typedFrames,
+		Name:    sfs.name,
+		World:   worldFrameJSON,
+		Frames:  typedFrames,
+		Parents: sfs.parents,
 	}
 	return json.Marshal(serializedFS)
 }

referenceframe/frame_system_test.go

@@ -451,4 +451,26 @@ func TestSerialization(t *testing.T) {
 	frames2 := fs2.FrameNames()
 	sort.Strings(frames2)
 	test.That(t, frames1, test.ShouldResemble, frames2)
+
+	for _, frameName := range frames1 {
+		frame1 := fs.Frame(frameName)
+		frame2 := fs2.Frame(frameName)
+		frame1JsonBytes, err := json.Marshal(frame1)
+		test.That(t, err, test.ShouldBeNil)
+		frame2JsonBytes, err := json.Marshal(frame2)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, frame1JsonBytes, test.ShouldResemble, frame2JsonBytes)
+
+		parentFrame1, err := fs.Parent(frame1)
+		test.That(t, err, test.ShouldBeNil)
+		parentFrame2, err := fs2.Parent(frame2)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, parentFrame1, test.ShouldNotBeNil)
+		test.That(t, parentFrame2, test.ShouldNotBeNil)
+		parentFrame1JsonBytes, err := json.Marshal(parentFrame1)
+		test.That(t, err, test.ShouldBeNil)
+		parentFrame2JsonBytes, err := json.Marshal(parentFrame2)
+		test.That(t, err, test.ShouldBeNil)
+		test.That(t, parentFrame1JsonBytes, test.ShouldResemble, parentFrame2JsonBytes)
+	}
 }