minor changes

Author: NoeZC

IROS_experiments/person_controller_webots/src/person.py

@@ -41,20 +41,20 @@ def __init__(self, person_number):
 
     def set_speed(self, data):
         ########## In case the receibed speed is in the person frame ##########
-        if time.time() - self.last_time_sent_velocity > self.time_between_velocity_commands:
-            self.last_time_sent_velocity = time.time()
-            person_orientation = self.person_node.getOrientation()
-            orientation = math.atan2(person_orientation[0], person_orientation[1]) - math.pi / 2
-            rotation_matrix = np.array(
-                [[math.cos(orientation), -math.sin(orientation)], [math.sin(orientation), math.cos(orientation)]])
-            lin_speed = np.array([data.axes[1].value * 2, 0])
-            # if data.axes[1] > 0.1 or data.axes[1] < -0.1:
-            #     self.moving = True
-            # else:
-            #     self.moving = False
-            converted_speed = np.matmul(rotation_matrix, lin_speed)
-            self.person_node.setVelocity(
-                [converted_speed[0] / 2.5, converted_speed[1] / 2.5, 0, 0, 0, -data.axes[0].value * math.pi / 2])
+        # if time.time() - self.last_time_sent_velocity > self.time_between_velocity_commands:
+        #     self.last_time_sent_velocity = time.time()
+        person_orientation = self.person_node.getOrientation()
+        orientation = math.atan2(person_orientation[0], person_orientation[1]) - math.pi / 2
+        rotation_matrix = np.array(
+            [[math.cos(orientation), -math.sin(orientation)], [math.sin(orientation), math.cos(orientation)]])
+        lin_speed = np.array([data.axes[1].value * 2, 0])
+        # if data.axes[1] > 0.1 or data.axes[1] < -0.1:
+        #     self.moving = True
+        # else:
+        #     self.moving = False
+        converted_speed = np.matmul(rotation_matrix, lin_speed)
+        self.person_node.setVelocity(
+            [converted_speed[0] / 2.5, converted_speed[1] / 2.5, 0, 0, 0, -data.axes[0].value * math.pi / 2])
 
     def set_initial_pose(self, data):
         if data.buttons[3]:
@@ -65,6 +65,6 @@ def get_camera_image(self):
         color = self.person_camera.getImage()
         color_image = cv2.cvtColor(cv2.cvtColor(
             np.frombuffer(color, np.uint8).reshape(self.person_camera.getHeight(), self.person_camera.getWidth(), 4),
-            cv2.COLOR_RGBA2RGB), cv2.COLOR_BGR2RGB)
+            cv2.COLOR_BGR2RGB), cv2.COLOR_BGR2RGB)
         cv2.imshow("color", color_image)
         cv2.waitKey(1)

IROS_experiments/person_controller_webots/src/specificworker.py

@@ -18,6 +18,7 @@
 #    You should have received a copy of the GNU General Public License
 #    along with RoboComp.  If not, see <http://www.gnu.org/licenses/>.
 #
+import time
 
 from PySide2.QtCore import QTimer
 from PySide2.QtWidgets import QApplication
@@ -43,14 +44,14 @@
 class SpecificWorker(GenericWorker):
     def __init__(self, proxy_map, startup_check=False):
         super(SpecificWorker, self).__init__(proxy_map)
-        self.Period = 100
+        self.Period = 16
         if startup_check:
             self.startup_check()
         else:
             os.environ["WEBOTS_CONTROLLER_URL"] = "ipc://1234/HUMAN_" + str(1)
             self.person = Person(1)
 
-            self.speed_queue = deque(maxlen=10)
+            self.speed_queue = deque(maxlen=1)
 
             self.timer.timeout.connect(self.compute)
             self.timer.start(self.Period)
@@ -69,13 +70,16 @@ def setParams(self, params):
 
     @QtCore.Slot()
     def compute(self):
-        print(self.person.step(self.person.timeStep), self.person.timeStep)
-        while self.person.step(self.person.timeStep) != -1:
-            self.person.bvh_animation_functions.motion_step()
+        # print(self.person.step(self.person.timeStep), self.person.timeStep)
+        if self.person.step(self.person.timeStep) != -1:
+            t1 = time.time()
+            # self.person.bvh_animation_functions.motion_step()
             self.person.get_camera_image()
             if self.speed_queue:
-                self.person.set_speed(self.speed_queue.pop())
-            #
+                self.person.set_speed(self.speed_queue.copy()[0])
+                print("Speed set")
+            print("TIME",time.time() - t1)
+
 
     def startup_check(self):
         print(f"Testing RoboCompJoystickAdapter.AxisParams from ifaces.RoboCompJoystickAdapter")