From a332651314b153e63689b974babf1109dfd465bd Mon Sep 17 00:00:00 2001
From: mrceki <105711013+mrceki@users.noreply.github.com>
Date: Thu, 2 Mar 2023 09:35:05 +0300
Subject: [PATCH] orientation hotfix

---
 air_moveit_config/src/add_sphere.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp
index 5e4080c..e08e529 100644
--- a/air_moveit_config/src/add_sphere.cpp
+++ b/air_moveit_config/src/add_sphere.cpp
@@ -30,8 +30,11 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz)
   float cam_degree = 30; // Angle between robot and camera
   pose.position.x = z + (((radius/2) / sin(((90-cam_degree) * M_PI)/180))*2);
   pose.position.y = -x; 
-  pose.position.z = -y + radius; //
-  pose.orientation.w = 1;
+  pose.position.z = -y + radius; //hotfix for collision
+
+  // Orientation fix (to do: cam_degree)
+  pose.orientation.w = 0.9659258;
+  pose.orientation.y = -0.258819;
 
   moveit_msgs::CollisionObject collision_object;
   collision_object.header.frame_id = "camera_link";