From 9404576a4f50a5ef02cf807d3734c98b224482ac Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Mon, 27 Apr 2020 00:47:10 -0400
Subject: [PATCH] Small Fixes

- Add "/" to front of topic name in cv_model
- Change default spawn point to be closer to depth camera
- Fix desired grasp pose offset
---
 src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml | 5 -----
 src/rrrobot_ws/src/rrrobot/src/cv_model.py             | 2 +-
 src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp | 2 +-
 src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp        | 2 +-
 4 files changed, 3 insertions(+), 8 deletions(-)

diff --git a/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
index dbd3574..ca58aa4 100644
--- a/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
+++ b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
@@ -1,9 +1,4 @@
 sensors:
-    logical_camera_1:
-      type: logical_camera
-      pose:
-        xyz: [1.2, 3.5, 1.5]
-        rpy: [0, 1.5707, 0]
     depth_camera_1:
       type: depth_camera
       pose:
diff --git a/src/rrrobot_ws/src/rrrobot/src/cv_model.py b/src/rrrobot_ws/src/rrrobot/src/cv_model.py
index e9fd9ef..93d2d72 100644
--- a/src/rrrobot_ws/src/rrrobot/src/cv_model.py
+++ b/src/rrrobot_ws/src/rrrobot/src/cv_model.py
@@ -50,7 +50,7 @@ def call_back(filename):
     print('type: ', type_dict[predicted_label])
 
     # publish a message, name of this node is 'cv_model'
-    pub = rospy.Publisher('cv_model', String, queue_size=10)
+    pub = rospy.Publisher('/cv_model', String, queue_size=10)
     # rospy.init_node('talker', anonymous=True)
     rate = rospy.Rate(10) # 10hz
 
diff --git a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp
index 0b3a6fb..fd3c18b 100644
--- a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp
+++ b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp
@@ -20,7 +20,7 @@ class ObjectSpawner
 {
 public:
     ObjectSpawner(const std::string &model_file = "/app/rrrobot_ws/src/gazebo_models/model_mappings.txt")
-        : DEFAULT_SPAWN_POINT(1.21825, 5.474367, 0.937978),
+        : DEFAULT_SPAWN_POINT(1.21825, 4.0, 0.937978),
           CONVEYOR_WIDTH(0.391404)
     {
         // DEFAULT_SPAWN_POINT.x = 0.2516105;
diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
index fc8f275..4d5d38b 100644
--- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
+++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
@@ -127,7 +127,7 @@ public:
 
 		desired_grasp_pose = grasp_pose;
 		// TODO: Tune z offset so end effector doesn't hit object
-		desired_grasp_pose.position.z -= 0.025;
+		desired_grasp_pose.position.z += 0.01;
 
 		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
 		{