diff --git a/droidlet/lowlevel/hello_robot/keyboard_teleop.py b/droidlet/lowlevel/hello_robot/keyboard_teleop.py new file mode 100644 index 0000000000..ee66da57f1 --- /dev/null +++ b/droidlet/lowlevel/hello_robot/keyboard_teleop.py @@ -0,0 +1,99 @@ +import sys +import time + +from pynput import keyboard +import numpy as np + + +UP = keyboard.Key.up +DOWN = keyboard.Key.down +LEFT = keyboard.Key.left +RIGHT = keyboard.Key.right +ESC = keyboard.Key.esc + +HZ_DEFAULT = 15 + +# 6 * v_max + w_max <= 1.8 (computed from max wheel vel & vel diff required for w) +VEL_MAX_DEFAULT = 0.20 +RVEL_MAX_DEFAULT = 0.45 + + +class RobotController: + def __init__( + self, + mover, + vel_max=None, + rvel_max=None, + hz=HZ_DEFAULT, + ): + # Params + self.dt = 1.0 / hz + self.vel_max = vel_max or VEL_MAX_DEFAULT + self.rvel_max = rvel_max or RVEL_MAX_DEFAULT + + # Robot + print("Connecting to robot...") + self.robot = mover.bot + print("Connected.") + + # Keyboard + self.key_states = {key: 0 for key in [UP, DOWN, LEFT, RIGHT]} + + # Controller states + self.alive = True + self.vel = 0 + self.rvel = 0 + + def on_press(self, key): + if key in self.key_states: + self.key_states[key] = 1 + elif key == ESC: + self.alive = False + return False # returning False from a callback stops listener + + def on_release(self, key): + if key in self.key_states: + self.key_states[key] = 0 + + def run(self): + print( + "(+[__]o) Teleoperation started. Use arrow keys to control robot, press ESC to exit. ^o^" + ) + while self.alive: + # Map keystrokes + vert_sign = self.key_states[UP] - self.key_states[DOWN] + hori_sign = self.key_states[LEFT] - self.key_states[RIGHT] + + # Compute velocity commands + self.vel = self.vel_max * vert_sign + self.rvel = self.rvel_max * hori_sign + + # Command robot + self.robot.set_velocity(self.vel, self.rvel) + + # Spin + time.sleep(self.dt) + + +def run_teleop(mover, vel=None, rvel=None): + robot_controller = RobotController(mover, vel, rvel) + listener = keyboard.Listener( + on_press=robot_controller.on_press, + on_release=robot_controller.on_release, + suppress=True, # suppress terminal outputs + ) + + # Start teleop + listener.start() + robot_controller.run() + + # Cleanup + listener.join() + print("(+[__]o) Teleoperation ended. =_=") + + +if __name__ == "__main__": + from droidlet.lowlevel.hello_robot.hello_robot_mover import HelloRobotMover + + mover = HelloRobotMover(ip=sys.argv[1]) + run_teleop(mover) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py new file mode 100644 index 0000000000..f9f1024c7b --- /dev/null +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -0,0 +1,180 @@ +from typing import List, Optional +import time +import threading + +import numpy as np +import rospy + +from utils import transform_global_to_base, transform_base_to_global + +V_MAX_DEFAULT = 0.2 # base.params["motion"]["default"]["vel_m"] +W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m +ACC_LIN = 0.4 # base.params["motion"]["max"]["accel_m"] +ACC_ANG = 1.2 # (accel_m_max - accel_m_default) / wheel_separation_m +MAX_HEADING_ANG = np.pi / 4 + + +class DiffDriveVelocityControl: + """ + Control logic for differential drive robot velocity control + """ + + def __init__(self, hz): + self.v_max = V_MAX_DEFAULT + self.w_max = W_MAX_DEFAULT + self.lin_error_tol = self.v_max / hz + self.ang_error_tol = self.w_max / hz + + @staticmethod + def _velocity_feedback_control(x_err, a, v_max): + """ + Computes velocity based on distance from target (trapezoidal velocity profile). + Used for both linear and angular motion. + """ + t = np.sqrt(2.0 * abs(x_err) / a) # x_err = (1/2) * a * t^2 + v = min(a * t, v_max) + return v * np.sign(x_err) + + @staticmethod + def _turn_rate_limit(lin_err, heading_diff, w_max, tol=0.0): + """ + Compute velocity limit that prevents path from overshooting goal + + heading error decrease rate > linear error decrease rate + (w - v * np.sin(phi) / D) / phi > v * np.cos(phi) / D + v < (w / phi) / (np.sin(phi) / D / phi + np.cos(phi) / D) + v < w * D / (np.sin(phi) + phi * np.cos(phi)) + + (D = linear error, phi = angular error) + """ + assert lin_err >= 0.0 + assert heading_diff >= 0.0 + + if heading_diff > MAX_HEADING_ANG: + return 0.0 + else: + return ( + w_max + * lin_err + / (np.sin(heading_diff) + heading_diff * np.cos(heading_diff) + 1e-5) + ) + + def __call__(self, xyt_err): + v_cmd = w_cmd = 0 + + lin_err_abs = np.linalg.norm(xyt_err[0:2]) + ang_err = xyt_err[2] + + # Go to goal XY position if not there yet + if lin_err_abs > self.lin_error_tol: + heading_err = np.arctan2(xyt_err[1], xyt_err[0]) + heading_err_abs = abs(heading_err) + + # Compute linear velocity + v_raw = self._velocity_feedback_control(lin_err_abs, ACC_LIN, self.v_max) + v_limit = self._turn_rate_limit( + lin_err_abs, + heading_err_abs, + self.w_max / 2.0, + tol=self.lin_error_tol, + ) + v_cmd = np.clip(v_raw, 0.0, v_limit) + + # Compute angular velocity + w_cmd = self._velocity_feedback_control(heading_err, ACC_ANG, self.w_max) + + # Rotate to correct yaw if XY position is at goal + elif abs(ang_err) > self.ang_error_tol: + # Compute angular velocity + w_cmd = self._velocity_feedback_control(ang_err, ACC_ANG, self.w_max) + + return v_cmd, w_cmd + + +class GotoVelocityController: + """ + Self-contained controller module for moving a diff drive robot to a target goal. + Target goal is update-able at any given instant. + """ + + def __init__( + self, + robot, + hz: float, + ): + self.robot = robot + self.hz = hz + self.dt = 1.0 / self.hz + + # Control module + self.control = DiffDriveVelocityControl(hz) + + # Initialize + self.loop_thr = None + self.control_lock = threading.Lock() + self.active = False + + self.xyt_loc = self.robot.get_estimator_pose() + self.xyt_goal = self.xyt_loc + self.track_yaw = True + + def _compute_error_pose(self): + """ + Updates error based on robot localization + """ + xyt_loc_new = self.robot.get_estimator_pose() + + xyt_err = transform_global_to_base(self.xyt_goal, xyt_loc_new) + if not self.track_yaw: + xyt_err[2] = 0.0 + + return xyt_err + + def _run(self): + rate = rospy.Rate(self.hz) + + while True: + # Get state estimation + xyt_err = self._compute_error_pose() + + # Compute control + v_cmd, w_cmd = self.control(xyt_err) + + # Command robot + with self.control_lock: + if self.active: + self.robot.set_velocity(v_cmd, w_cmd) + + # Spin + rate.sleep() + + def check_at_goal(self) -> bool: + xyt_err = self._compute_error_pose() + + xy_fulfilled = np.linalg.norm(xyt_err[0:2]) <= self.lin_error_tol + + t_fulfilled = True + if self.track_yaw: + t_fulfilled = abs(xyt_err[2]) <= self.ang_error_tol + + return xy_fulfilled and t_fulfilled + + def set_goal( + self, + xyt_position: List[float], + ): + self.xyt_goal = xyt_position + + def enable_yaw_tracking(self, value: bool = True): + self.track_yaw = value + + def start(self): + if self.loop_thr is None: + self.loop_thr = threading.Thread(target=self._run) + self.loop_thr.start() + self.active = True + + def pause(self): + self.active = False + with self.control_lock: + self.robot.set_velocity(0.0, 0.0) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py index e6090159d1..3b38ab0e68 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py @@ -9,9 +9,6 @@ import copy import math from math import * -from droidlet.lowlevel.locobot.remote.segmentation.detectron2_segmentation import ( - Detectron2Segmentation, -) import pyrealsense2 as rs import Pyro4 diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py index e29a988beb..34119522ae 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py @@ -63,7 +63,7 @@ def __init__(self, ip): self._robot.startup() if not self._robot.is_calibrated(): self._robot.home() - self._robot.stow() + # self._robot.stow() # HACK: not working currently, robot runs fine without this line self._done = True self.cam = None # Read battery maintenance guide https://docs.hello-robot.com/battery_maintenance_guide/ @@ -248,6 +248,10 @@ def obstacle_fn(): self._done = True return status + def set_velocity(self, v_m, w_r): + self._robot.base.set_velocity(v_m, w_r) + self._robot.push_command() + def is_base_moving(self): robot = self._robot left_wheel_moving = ( diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py index 37ff41869e..ccadb09584 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py @@ -13,13 +13,15 @@ from rich import print import Pyro4 import numpy as np + from droidlet.lowlevel.hello_robot.remote.utils import ( goto_trackback, transform_global_to_base, goto, ) -from stretch_ros_move_api import MoveNode as Robot from droidlet.lowlevel.pyro_utils import safe_call +from stretch_ros_move_api import MoveNode as Robot +from goto_controller import GotoVelocityController import traceback @@ -27,6 +29,7 @@ Pyro4.config.SERIALIZERS_ACCEPTED.add("pickle") Pyro4.config.ITER_STREAMING = True +VEL_CONTROL_HZ = 15 # ##################################################### @Pyro4.expose @@ -43,6 +46,8 @@ def __init__(self, ip): self._load_urdf() self.tilt_correction = 0.0 + self._goto_controller = GotoVelocityController(robot=self._robot, hz=VEL_CONTROL_HZ) + def _load_urdf(self): import os @@ -97,11 +102,13 @@ def get_camera_transform(self): return camera_transform def get_base_state(self): - # Best (from SLAM) - return self._robot.get_slam_pose() - # second Best (from lidar scanning) + # SLAM + wheel encoder (#1) + return self._robot.get_estimator_pose() + # SLAM (#2) + # return self._robot.get_slam_pose() + # lidar scanning (#3) # return self._robot.get_scan_matched_pose() - # Worst, from wheel encoder only + # Wheel encoder only (#4, but low latency & noise) # return self._robot.get_odom() def get_slam_pose(self): @@ -260,6 +267,55 @@ def obstacle_fn(): raise e return status + def set_velocity(self, v_m, w_r): + """Directly sets the forward and yaw velocity of the robot.""" + self._robot.set_velocity(v_m, w_r) + + def set_position_goal(self, xy_position, relative=False): + """Moves the robot base to the given goal position. + The robot does not have a yaw goal and will simply turn & move towards + the desired position. + + :param xy_position: The goal position of the form (x,y) + """ + assert ( + len(xy_position) == 2 + ), f"Input goal should be of length 2 (xy), got {len(xy_position)} instead." + + # Convert abs to rel + xyt_position = list(xy_position) + [0.0] + if not relative: + base_state = self.get_base_state() + xyt_position = transform_global_to_base(xyt_position, base_state) + xyt_position[2] = 0.0 + + # Set motion goal + self._goto_controller.start() + self._goto_controller.enable_yaw_tracking(False) + self._goto_controller.set_goal(xyt_position) + + def set_goal(self, xyt_position, relative=False): + """Moves the robot base to the given goal state + + :param xyt_position: The goal state of the form (x,y,yaw) + """ + assert ( + len(xyt_position) == 3 + ), f"Input goal should be of length 3 (xyt), got {len(xyt_position)} instead." + + # Convert abs to rel + if not relative: + base_state = self.get_base_state() + xyt_position = transform_global_to_base(xyt_position, base_state) + + # Set motion goal + self._goto_controller.start() + self._goto_controller.enable_yaw_tracking(True) + self._goto_controller.set_goal(xyt_position) + + def stop_continuous_control(self): + self._goto_controller.pause() + def is_moving(self): return not self._done diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 5dbf21a8d2..ccbdd43760 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -6,12 +6,47 @@ from sensor_msgs.msg import JointState from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint -from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, Pose2D, PoseWithCovarianceStamped, Twist from nav_msgs.msg import Odometry import hello_helpers.hello_misc as hm from tf.transformations import euler_from_quaternion import collections import numpy as np +import sophus as sp +from scipy.spatial.transform import Rotation as R + +import rospy +from geometry_msgs.msg import Twist + + +SLAM_CUTOFF_HZ = 0.2 + + +def pose_ros2sp(pose): + r_mat = R.from_quat( + (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) + ).as_matrix() + t_vec = np.array([pose.position.x, pose.position.y, pose.position.z]) + return sp.SE3(r_mat, t_vec) + + +def pose_sp2ros(pose_se3): + quat = R.from_matrix(pose_se3.so3().matrix()).as_quat() + + pose = Pose() + pose.position.x = pose_se3.translation()[0] + pose.position.y = pose_se3.translation()[1] + pose.position.z = pose_se3.translation()[2] + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + return pose + + +def cutoff_angle(duration, cutoff_freq): + return 2 * np.pi * duration * cutoff_freq class MoveNode(hm.HelloNode): @@ -27,19 +62,40 @@ def __init__(self): self._scan_matched_pose = None self._lock = threading.Lock() + self._filtered_pose = sp.SE3() + self._slam_pose_sp = sp.SE3() + self._t_odom_prev = time.time() + self._pose_odom_prev = sp.SE3() + + self._nav_mode = rospy.ServiceProxy("/switch_to_navigation_mode", Trigger) + s_request = TriggerRequest() + self._nav_mode(s_request) + + self._vel_command_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) + self._estimator_pub = rospy.Publisher( + "/state_estimator/pose_filtered", PoseStamped, queue_size=1 + ) + def _joint_states_callback(self, joint_state): with self._lock: self._joint_state = joint_state def _slam_pose_callback(self, pose): + t_curr = time.time() + ros_time = rospy.Time.now() with self._lock: self._slam_pose = pose + # Update slam pose for filtering + self._slam_pose_sp = pose_ros2sp(pose.pose.pose) + def _scan_matched_pose_callback(self, pose): with self._lock: self._scan_matched_pose = (pose.x, pose.y, pose.theta) def _odom_callback(self, pose): + t_curr = time.time() + ros_time = rospy.Time.now() with self._lock: self._odom = pose self._linear_movement.append( @@ -47,6 +103,35 @@ def _odom_callback(self, pose): ) self._angular_movement.append(abs(pose.twist.twist.angular.z)) + # Compute injected signals into filtered pose + pose_odom = pose_ros2sp(pose.pose.pose) + pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom + pose_diff_slam = self._filtered_pose.inverse() * self._slam_pose_sp + + # Update filtered pose + w = cutoff_angle(t_curr - self._t_odom_prev, SLAM_CUTOFF_HZ) + coeff = 1 / (w + 1) + + pose_diff_log = coeff * pose_diff_odom.log() + (1 - coeff) * pose_diff_slam.log() + self._filtered_pose = self._filtered_pose * sp.SE3.exp(pose_diff_log) + self._publish_filtered_state(ros_time) + + # Update variables + self._pose_odom_prev = pose_odom + self._t_odom_prev = t_curr + + def _publish_filtered_state(self, timestamp): + pose_out = PoseStamped() + pose_out.header.stamp = timestamp + pose_out.pose = pose_sp2ros(self._filtered_pose) + self._estimator_pub.publish(pose_out) + + def set_velocity(self, v_m, w_r): + cmd = Twist() + cmd.linear.x = v_m + cmd.angular.z = w_r + self._vel_command_pub.publish(cmd) + def is_moving(self): with self._lock: lm, am = self._linear_movement, self._angular_movement @@ -91,6 +176,14 @@ def get_odom(self): euler = euler_from_quaternion(quat) return (pose.pose.position.x, pose.pose.position.y, euler[2]) + def get_estimator_pose(self): + with self._lock: + pose = self._filtered_pose.copy() + + t_vec = pose.translation() + r_vec = pose.so3().log() + return (t_vec[0], t_vec[1], r_vec[2]) + def get_joint_state(self, name=None): with self._lock: joint_state = self._joint_state diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 4af1a07ed8..3f69bf9f44 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -2,8 +2,11 @@ import sys import random import math -from threading import local +import threading import time +from dataclasses import dataclass +from typing import Optional + import torch import numpy as np import Pyro4 @@ -11,12 +14,17 @@ from droidlet.lowlevel.pyro_utils import safe_call import skimage.morphology import cv2 +import rospy +from geometry_msgs.msg import Twist from slam_pkg.utils import depth_util as du from visualization.ogn_vis import ObjectGoalNavigationVisualization from policy.goal_policy import GoalPolicy from segmentation.constants import coco_categories +from geometry_msgs.msg import Pose +from tf.transformations import euler_from_quaternion, quaternion_from_euler + random.seed(0) torch.manual_seed(0) np.random.seed(0) @@ -36,6 +44,20 @@ def draw_line(start, end, mat, steps=25, w=1): return mat +def xyt2pose(xyt): + quat = quaternion_from_euler(0.0, 0.0, xyt[2]) + + pose = Pose() + pose.position.x = xyt[0] + pose.position.y = xyt[1] + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + return pose + + class Trackback(object): def __init__(self, planner): self.locs = set() @@ -72,6 +94,26 @@ def get_loc(self, cur_loc): return ans +@dataclass +class GoalParams: + is_multi_goal: bool + goal: Optional[np.ndarray] + goal_map: Optional[np.ndarray] + distance_threshold: Optional[np.ndarray] + angle_threshold: Optional[np.ndarray] + steps: float + visualize: bool + + +@dataclass +class NavigationStatus: + path_found: bool + goal_reached: bool + goal_data: Optional[GoalParams] + goal_update: threading.Event + goal_lock: threading.Lock + + @Pyro4.expose class Navigation(object): def __init__(self, planner, slam, robot): @@ -82,6 +124,19 @@ def __init__(self, planner, slam, robot): num_sem_categories = len(coco_categories) self.map_size, self.local_map_size = self.slam.get_map_sizes() + + # ROS publishers for monitoring + self._loc_pub = rospy.Publisher("nav/robot_pose", Pose, queue_size=1) + self._stg_pub = rospy.Publisher("nav/st_goal_pose", Pose, queue_size=1) + self._goal_pub = rospy.Publisher("nav/goal_pose", Pose, queue_size=1) + rospy.init_node("navigation_service") + + # Async navigator + self.nav_status = NavigationStatus(False, False, None, threading.Event(), threading.Lock()) + self.goto_thr = threading.Thread(target=self._navigation_loop) + self.goto_thr.start() + + # ObjectNav policy self.goal_policy = GoalPolicy( map_features_shape=(num_sem_categories + 8, self.local_map_size, self.local_map_size), num_outputs=2, @@ -92,7 +147,6 @@ def __init__(self, planner, slam, robot): self.goal_policy.load_state_dict(state_dict, strict=False) self._busy = False - self._stop = True self._done_exploring = False self.vis = ObjectGoalNavigationVisualization() @@ -198,37 +252,72 @@ def go_to_absolute( steps=100000000, visualize=True, ): - print("[navigation] Starting a go_to_absolute") - # specify exactly one of goal or goal_map + # Specify exactly one of goal or goal_map assert (goal is not None and goal_map is None) or (goal is None and goal_map is not None) + is_multi_goal = goal is None + goal_data = GoalParams( + is_multi_goal, goal, goal_map, distance_threshold, angle_threshold, steps, visualize + ) + + # Reset params + with self.nav_status.goal_lock: + self.nav_status.goal_data = goal_data + self.nav_status.goal_update.set() - self._busy = True + # Log + if goal is not None: + self._goal_pub.publish(xyt2pose(goal)) + + def _navigation_loop(self): self._stop = False - robot_loc = self.robot.get_base_state() - initial_robot_loc = robot_loc - goal_reached = False - path_found = True - while not goal_reached and steps > 0 and self._stop is False: - stg = self.planner.get_short_term_goal( - robot_loc, - goal=goal, - goal_map=goal_map, - vis_path=f"{self.vis.path}/planner/step{self.vis.snapshot_idx}.png", - ) - if stg == False: - # no path to end-goal - print( - "Could not find a path to the end goal {} from current robot location {}, aborting move".format( - goal, robot_loc - ) + while self._stop is False: + if self.nav_status.goal_reached or not self.nav_status.path_found: + self.nav_status.goal_update.clear() + self.nav_status.goal_update.wait() + + with self.nav_status.goal_lock: + # Load goal data + goal = self.nav_status.goal_data.goal + goal_map = self.nav_status.goal_data.goal_map + distance_threshold = self.nav_status.goal_data.distance_threshold + angle_threshold = self.nav_status.goal_data.angle_threshold + visualize = self.nav_status.goal_data.visualize + + # Plan + self.nav_status.path_found = True + + robot_loc = self.robot.get_base_state() + self._loc_pub.publish(xyt2pose(robot_loc)) + + stg = self.planner.get_short_term_goal( + robot_loc, + goal=goal, + goal_map=goal_map, + vis_path=f"{self.vis.path}/planner/step{self.vis.snapshot_idx}.png", ) - path_found = False - break - robot_loc = self.robot.get_base_state() - status, action = self.robot.go_to_absolute(stg) - robot_loc = self.robot.get_base_state() + if stg == False: + # no path to end-goal + print( + "Could not find a path to the end goal {} from current robot location {}, aborting move".format( + goal, robot_loc + ) + ) + self.nav_status.path_found = False + break + + # Log + if goal is not None: + self._stg_pub.publish(xyt2pose(stg)) + # Execute plan + # status, action = self.robot.go_to_absolute(stg) + self.robot.set_goal(stg) + status = "SUCCEEDED" + action = "move_to" + + robot_loc = self.robot.get_base_state() + """ print("[navigation] Finished a go_to_absolute") print( " Initial location: {} Final goal: {}".format( @@ -237,8 +326,9 @@ def go_to_absolute( ) print(" Short-term goal: {}, Reached Location: {}".format(stg, robot_loc)) print(" Robot Status: {}".format(status)) + """ if status == "SUCCEEDED": - goal_reached = self.planner.goal_within_threshold( + self.nav_status.goal_reached = self.planner.goal_within_threshold( robot_loc, goal=goal, goal_map=goal_map, @@ -284,8 +374,6 @@ def go_to_absolute( # TODO: if the trackback fails, we're screwed. Handle this robustly. - steps = steps - 1 - if visualize: self.vis.set_action_and_collision( {"action": action, "collision": status != "SUCCEEDED"} @@ -293,9 +381,6 @@ def go_to_absolute( self.vis.update_last_position_vis_info(self.slam.get_last_position_vis_info()) self.vis.snapshot() - self._busy = False - return path_found, goal_reached - def go_to_object( self, object_goal: str, @@ -357,13 +442,14 @@ def go_to_object( if visualize: self.vis.set_location_goal(goal_map) - _, goal_reached = self.go_to_absolute( + self.go_to_absolute( goal_map=goal_map, distance_threshold=0.5, angle_threshold=30, steps=1, visualize=visualize, ) + goal_reached = self.nav_status.goal_reached continue elif (cat_frame == 1).sum() > 0: @@ -559,8 +645,8 @@ def explore(self, far_away_goal): self._done_exploring = False if not self._done_exploring: print("exploring 1 step") - path_found, _ = self.go_to_absolute(far_away_goal, steps=1) - if path_found == False: + self.go_to_absolute(far_away_goal, steps=1) + if self.nav_status.path_found == False: # couldn't reach far_away_goal # and don't seem to have any unexplored # paths to attempt to get there @@ -578,6 +664,8 @@ def reset_explore(self): def stop(self): self._stop = True + if self.goto_thr is not None: + self.goto_thr.join() robot_ip = os.getenv("LOCOBOT_IP")