From 438476621e4574fdb78a2149ffbdceab46db15ca Mon Sep 17 00:00:00 2001
From: Peter LoVerso <biotinker@gmail.com>
Date: Mon, 30 Sep 2024 11:54:47 -0700
Subject: [PATCH] Fixes to prevent nil panics

---
 referenceframe/transformable.go | 4 ++++
 robot/server/server.go          | 5 ++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go
index d06aa8a1fb3..0c548c21d14 100644
--- 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
index 3e6f33b892c..42c460e4263 100644
--- 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.