From 82d266a1e1835de27c0e8ca9f4ac5844df5ffa19 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergio=20Mart=C3=ADnez=20Alonso?= <sergio.martinez3@udc.es>
Date: Thu, 4 Apr 2024 12:51:26 +0200
Subject: [PATCH] Improvements in Goal node

---
 cognitive_nodes/cognitive_nodes/goal.py | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/cognitive_nodes/cognitive_nodes/goal.py b/cognitive_nodes/cognitive_nodes/goal.py
index dbdad38..cc08135 100644
--- a/cognitive_nodes/cognitive_nodes/goal.py
+++ b/cognitive_nodes/cognitive_nodes/goal.py
@@ -15,7 +15,7 @@ class Goal(CognitiveNode):
     """
     Goal class
     """
-    def __init__(self, name='goal', data = None, class_name = 'cognitive_nodes.goal.Goal', space_class = None, space = None, **params):
+    def __init__(self, name='goal', data = None, class_name = 'cognitive_nodes.goal.Goal', space_class = None, space = None, robot_service = 'simulator/', **params):
         """
         Constructor of the Goal class
 
@@ -31,6 +31,8 @@ def __init__(self, name='goal', data = None, class_name = 'cognitive_nodes.goal.
         :type space_class: str
         :param space: The space used to define the Goal
         :type space: cognitive_nodes.space
+        :param robot_service: The ROS service prefix to connect with robot or simulator
+        :type robot_service: str
         """
         super().__init__(name, class_name, **params)
         self.register_in_LTM({})
@@ -39,7 +41,7 @@ def __init__(self, name='goal', data = None, class_name = 'cognitive_nodes.goal.
         self.start = None
         self.end = None
         self.period = None
-        self.sim_service = 'mdb_simulator'
+        self.robot_service = robot_service
         self.old_perception = []
 
         if data:
@@ -256,7 +258,7 @@ def calculate_closest_position(self, angle):
         closest_position_client.destroy_node()
         return dist_near, ang_near
     
-    def object_pickable_with_two_hands(self):
+    def object_pickable_with_two_hands_request(self):
         """
         Check of an obkect is pickable with the two hands of the robot
 
@@ -410,7 +412,7 @@ def ball_and_box_on_the_same_side(self):
                 break
         return same_side
 
-    def object_pickable_withtwohands(self):
+    def object_pickable_with_two_hands(self):
         """
         Check if an object can be hold with two hands.
 
@@ -421,7 +423,7 @@ def object_pickable_withtwohands(self):
         """
         pickable = False
         for cylinder in self.perception["cylinders"]:
-            pickable = (not Goal.object_held()) and self.object_pickable_withtwohands(
+            pickable = (not Goal.object_held()) and self.object_pickable_with_two_hands_request(
                 cylinder["distance"], cylinder["angle"]
             )
             if pickable:
@@ -624,7 +626,7 @@ def get_reward(self):
             for _, activation in zip(self.perception, self.activation):
                 if activation > self.threshold:
                     if (
-                        self.object_pickable_withtwohands()
+                        self.object_pickable_with_two_hands()
                         and (not self.object_in_close_box())
                         and (not self.object_with_robot())
                     ):
@@ -760,7 +762,7 @@ def get_reward(self):
                     elif not self.object_held_before():
                         self.reward = 0.3
                 elif not self.object_held_before():
-                    if self.object_pickable_withtwohands():
+                    if self.object_pickable_with_two_hands():
                         self.reward = 0.3
                     elif self.object_was_approximated():
                         self.reward = 0.2
@@ -785,7 +787,7 @@ def get_reward(self):
                     if not self.object_held_before():
                         self.reward = 0.6
                 elif not self.object_held_before():
-                    if self.object_pickable_withtwohands():
+                    if self.object_pickable_with_two_hands():
                         self.reward = 0.3
                     elif self.object_was_approximated():
                         self.reward = 0.2