diff --git a/.gitignore b/.gitignore index d50a09f..c214a9e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .env __pycache__/ +venv* \ No newline at end of file diff --git a/BBoxProcessor.py b/BBoxProcessor.py index 1739d5b..a21961f 100644 --- a/BBoxProcessor.py +++ b/BBoxProcessor.py @@ -16,6 +16,7 @@ class BBoxProcessor: closest_box = self.get_box_closest_to_center(new_boxes) if closest_box is None: return [0, 0] + print("Closest box: ", closest_box, "All: ", new_boxes) return self.get_normalized_box_position(closest_box) def bbox_distance(self, box1: list, box2: list) -> float: @@ -82,4 +83,4 @@ class BBoxProcessor: """ Returns the normalized position of a bounding box, shringking its range from 0 to 1000 to 0 to 1 """ - return [(box[0] + box[2] / 2) / 500.0 - 1, (box[1] + box[3] / 2) / 500 - 1] + return [round((box[0] + box[2] / 2) / 500.0 - 1, 3), round((box[1] + box[3] / 2) / 500 - 1, 3)] diff --git a/URSentry.py b/URSentry.py index 0158d53..864798c 100644 --- a/URSentry.py +++ b/URSentry.py @@ -81,6 +81,7 @@ class URSentry: ticks_to_wait = 10 if self.await_stop: + print("Awaiting stop --------------------") joint_speeds = self.robot.get_joint_speeds() if all([speed == 0 for speed in joint_speeds]): self.await_stop_ticks += 1 @@ -90,6 +91,7 @@ class URSentry: return if self.send_to_zero_on_stop: + print("Sending to zero --------------------") self.await_stop = True self.send_to_zero_on_stop = False self.forward_position_to_base_angle_degrees(0) @@ -111,8 +113,8 @@ class URSentry: vertical_dead_zone_radius = 0.1 # Speeds for the base and neck joints - base_max_speed = 0.8 - base_min_speed = 0.3 + base_max_speed = 1.1 + base_min_speed = 0.2 base_acceleration = 1.5 @@ -154,7 +156,7 @@ class URSentry: ) self.smooth_stop_delayed_call.start() - # print("Robot speed: ", self.robot_speed) + print("Robot speed: ", self.robot_speed) self.robot.speedj(self.robot_speed, base_acceleration, 1) diff --git a/main.py b/main.py index afbd532..f5b4cdb 100644 --- a/main.py +++ b/main.py @@ -26,7 +26,7 @@ class CameraJoystick(threading.Thread): def run(self): while self.keep_running: bb = b.get_joystick_position_from_new_set_of_bboxes(q.get()) - print(bb) + #print(bb) self.joystick_queue.put(bb) def stop(self):