AIRO-392 Fixing list index exception when planner fails

When the MoveIt trajectory planner fails to find a valid path, it
returns an empty list of path points. Our mover.py script assumes there
is always something in the list, so we get an exception when there is
not (which breaks our listener node and mandates a re-launch).

Added out of bounds checks and moved the response population to the
bottom to ensure we only send a path back when it is complete, and
otherwise send an empty response (for which the srv will print an adequate
error to console).

Tested by running DemoScene with out of bounds locations for target
placement.

tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/mover.py

 
     # Pre grasp - position gripper directly above target object
     pre_grasp_pose = plan_trajectory(move_group, req.pick_pose, current_robot_joint_configuration)
+    
+    # If the trajectory has no points, planning has failed and we return an empty response
+    if not pre_grasp_pose.joint_trajectory.points:
+        return response
-    response.trajectories.append(pre_grasp_pose)
+    
+    if not grasp_pose.joint_trajectory.points:
+        return response
-    response.trajectories.append(grasp_pose)
+    
+    if not pick_up_pose.joint_trajectory.points:
+        return response
-    response.trajectories.append(pick_up_pose)
-    previous_ending_joint_angles = place_pose.joint_trajectory.points[-1].positions
+    if not place_pose.joint_trajectory.points:
+        return response
+
+    # If trajectory planning worked for all pick and place stages, add plan to response
+    response.trajectories.append(pre_grasp_pose)
+    response.trajectories.append(grasp_pose)
+    response.trajectories.append(pick_up_pose)
     response.trajectories.append(place_pose)
 
     move_group.clear_pose_targets()
 
 
 if __name__ == "__main__":
-    moveit_server()
+    moveit_server()