RSDK-8887 Update to prevent nil panics in framesystem (#4405)

viamrobotics · Sep 30, 2024 · 59e3c5c · 59e3c5c
1 parent 388c8d3
commit 59e3c5c
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go
@@ -111,6 +111,10 @@ func (lF *LinkInFrame) ToStaticFrame(name string) (Frame, error) {
 
 // PoseInFrameToProtobuf converts a PoseInFrame struct to a PoseInFrame protobuf message.
 func PoseInFrameToProtobuf(framedPose *PoseInFrame) *commonpb.PoseInFrame {
+	if framedPose == nil {
+		return &commonpb.PoseInFrame{}
+	}
+
 	poseProto := &commonpb.Pose{}
 	if framedPose.pose != nil {
 		poseProto = spatialmath.PoseToProtobuf(framedPose.pose)

diff --git a/robot/server/server.go b/robot/server/server.go
@@ -244,7 +244,10 @@ func (s *Server) TransformPose(ctx context.Context, req *pb.TransformPoseRequest
 		return nil, err
 	}
 	transformedPose, err := s.robot.TransformPose(ctx, referenceframe.ProtobufToPoseInFrame(req.Source), req.Destination, transforms)
-	return &pb.TransformPoseResponse{Pose: referenceframe.PoseInFrameToProtobuf(transformedPose)}, err
+	if err != nil {
+		return nil, err
+	}
+	return &pb.TransformPoseResponse{Pose: referenceframe.PoseInFrameToProtobuf(transformedPose)}, nil
 }
 
 // TransformPCD will transform the pointcloud to the desired frame in the robot's frame system.