diff --git a/README.md b/README.md index 803141dc9..830411f09 100644 --- a/README.md +++ b/README.md @@ -54,19 +54,21 @@ robot.arm_to([0.1, 0.5, 0, 0, 0, 0]) robot.move_to_nav_posture() # Move the robot back to origin -# navigate_to() is only allowed in navigation mode -robot.navigate_to([0, 0, 0]) +# move_base_to() is only allowed in navigation mode +robot.move_base_to([0, 0, 0]) # Move the robot 0.5m forward -robot.navigate_to([0.5, 0, 0], relative=True) +robot.move_base_to([0.5, 0, 0], relative=True) # Rotate the robot 90 degrees to the left -robot.navigate_to([0, 0, 3.14159/2], relative=True) +robot.move_base_to([0, 0, 3.14159/2], relative=True) # And to the right -robot.navigate_to([0, 0, -3.14159/2], relative=True) +robot.move_base_to([0, 0, -3.14159/2], relative=True) ``` +You can find this code in [examples/simple_navigation.py](examples/simple_navigation.py). + ## Apps After [installation](#installation), on the robot, run the server: diff --git a/examples/simple_motions.py b/examples/simple_motions.py new file mode 100644 index 000000000..95297f417 --- /dev/null +++ b/examples/simple_motions.py @@ -0,0 +1,64 @@ +# Copyright (c) Hello Robot, Inc. +# All rights reserved. +# +# This source code is licensed under the license found in the LICENSE file in the root directory +# of this source tree. +# +# Some code may be adapted from other open-source works with their respective licenses. Original +# license information maybe found below, if so. + +from stretch.agent import RobotClient + +print("Initialize the robot client") +print("robot = RobotClient()") +robot = RobotClient() +print("Done") +# On future connection attempts, the IP address can be left blank + +# Turn head towards robot's hand +print("Turn head towards robot's hand") +print("robot.move_to_manip_posture()") +robot.move_to_manip_posture() +print("Done") + +# Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height +print("Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height") +print("robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True)") +robot.arm_to([0.1, 0.5, 0, 0, 0, 0]) +print("Done") + +# Turn head towards robot's base and switch base to navigation mode +# In navigation mode, we can stream velocity commands to the base for smooth motions, and base +# rotations are enabled +print("Turn head towards robot's base and switch base to navigation mode") +print("robot.move_to_nav_posture()") +robot.move_to_nav_posture() +print("Done") + +# Move the robot back to origin +# move_base_to() is only allowed in navigation mode +print("Move the robot back to origin") +print("robot.move_base_to([0, 0, 0])") +robot.move_base_to([0, 0, 0]) +print("Done") + +# Move the robot 0.5m forward +print("Move the robot 0.25m forward") +print("robot.move_base_to([0.25, 0, 0], relative=True)") +robot.move_base_to([0.5, 0, 0], relative=True) +print("Done") + +# Rotate the robot 90 degrees to the left +print("Rotate the robot 90 degrees to the left") +print("robot.move_base_to([0, 0, 3.14159/2], relative=True)") +robot.move_base_to([0, 0, 3.14159 / 2], relative=True, verbose=True) +print("Done") + +# And to the right +print("Rotate the robot 90 degrees to the right") +print("robot.move_base_to([0, 0, -3.14159/2], relative=True, blocking=True)") +robot.move_base_to([0, 0, -3.14159 / 2], relative=True, verbose=True) + +print("Stop the robot") +print("robot.stop()") +robot.stop() diff --git a/scripts/set_robot_ip.sh b/scripts/set_robot_ip.sh new file mode 100755 index 000000000..b0c85e967 --- /dev/null +++ b/scripts/set_robot_ip.sh @@ -0,0 +1,29 @@ +#!/bin/bash + +## Check if an IP address is provided +if [ $# -eq 0 ]; then + echo "Error: Please provide an IP address as an argument." + echo "Usage: $0 " + exit 1 +fi + +## Validate IP address format +if ! [[ $1 =~ ^[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+$ ]]; then + echo "Error: Invalid IP address format. Please use xxx.xxx.xxx.xxx" + exit 1 +fi + +## Create directory if it doesn't exist +mkdir -p ~/.stretch + +## Write IP address to file +echo "$1" > ~/.stretch/robot_ip.txt + +## Verify file creation and content +if [ -f ~/.stretch/robot_ip.txt ]; then + echo "Success: IP address $1 has been written to ~/.stretch/robot_ip.txt" +else + echo "Error: Failed to create or write to ~/.stretch/robot_ip.txt" + exit 1 +fi + diff --git a/src/stretch/agent/operations/navigate.py b/src/stretch/agent/operations/navigate.py index f6a8a4238..3bba2f2d6 100644 --- a/src/stretch/agent/operations/navigate.py +++ b/src/stretch/agent/operations/navigate.py @@ -79,7 +79,7 @@ def run(self): theta = math.atan2( xyz[1] - self.robot.get_base_pose()[1], xyz[0] - self.robot.get_base_pose()[0] ) - self.robot.navigate_to( + self.robot.move_base_to( np.array([start_xyz[0], start_xyz[1], theta + np.pi / 2]), blocking=True, timeout=30.0, @@ -94,10 +94,10 @@ def run(self): # Orient the robot towards the object and use the end effector camera to pick it up xyt = self.plan.trajectory[-1].state - # self.robot.navigate_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0) + # self.robot.move_base_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0) if self.be_precise: self.warn("Moving again to make sure we're close enough to the goal.") - self.robot.navigate_to(xyt, blocking=True, timeout=30.0) + self.robot.move_base_to(xyt, blocking=True, timeout=30.0) def was_successful(self): """This will be successful if we got within a reasonable distance of the target object.""" diff --git a/src/stretch/agent/robot_agent.py b/src/stretch/agent/robot_agent.py index a6dae0f7b..9aba573a3 100644 --- a/src/stretch/agent/robot_agent.py +++ b/src/stretch/agent/robot_agent.py @@ -135,6 +135,9 @@ def __init__( self.reset_object_plans() + # Is this still running? + self._running = True + # Store the current scene graph computed from detected objects self.scene_graph = None @@ -164,8 +167,13 @@ def __init__( self.sub_socket = self.context.socket(zmq.SUB) self.sub_socket.connect(f"tcp://localhost:{obs_sub_port}") self.sub_socket.setsockopt_string(zmq.SUBSCRIBE, "") + else: + self._update_map_thread = None + self._get_observations_thread = None def __del__(self): + if self._update_map_thread is not None and self._update_map_thread.is_alive(): + self._update_map_thread.join() self._update_map_thread.join() def _create_voxel_map(self, parameters: Parameters) -> SparseVoxelMap: @@ -406,7 +414,7 @@ def rotate_in_place( steps += 1 while i < steps: t0 = timeit.default_timer() - self.robot.navigate_to( + self.robot.move_base_to( [x, y, theta + (i * step_size)], relative=False, blocking=True, @@ -459,7 +467,8 @@ def show_map(self): ) def get_observations_loop(self): - while True: + """Threaded function that gets observations in real-time.""" + while self.robot.running and self._running: obs = None t0 = timeit.default_timer() @@ -615,8 +624,8 @@ def update_map_with_pose_graph(self): self._obs_history_lock.release() def update_map_loop(self): - """Threaded function that updates our voxel map in real-time""" - while True: + """Threaded function that updates our voxel map in real-time.""" + while self.robot.running and self._running: with self._robot_lock: self.update_map_with_pose_graph() time.sleep(0.5) @@ -989,11 +998,11 @@ def recover_from_invalid_start(self) -> bool: if self.space.is_valid(xyt_goal_backward, verbose=True): logger.warning("Trying to move backwards...") # Compute the position forward or backward from the robot - self.robot.navigate_to(xyt_goal_backward, relative=False) + self.robot.move_base_to(xyt_goal_backward, relative=False) elif self.space.is_valid(xyt_goal_forward, verbose=True): logger.warning("Trying to move forward...") # Compute the position forward or backward from the robot - self.robot.navigate_to(xyt_goal_forward, relative=False) + self.robot.move_base_to(xyt_goal_forward, relative=False) else: logger.warning("Could not recover from invalid start state!") return False @@ -1074,15 +1083,15 @@ def move_to_any_instance( print("ROBOT IS STUCK! Move back!") r = np.random.randint(3) if r == 0: - self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True) elif r == 1: - self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True) elif r == 2: - self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True) return False time.sleep(1.0) - self.robot.navigate_to([0, 0, np.pi / 2], relative=True) + self.robot.move_base_to([0, 0, np.pi / 2], relative=True) self.robot.move_to_manip_posture() return True @@ -1445,7 +1454,7 @@ def run_exploration( if not start_is_valid: print("Start not valid. back up a bit.") print(f"robot base pose: {self.robot.get_base_pose()}") - self.robot.navigate_to([-0.1, 0, 0], relative=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True) continue # Now actually plan to the frontier @@ -1503,11 +1512,11 @@ def run_exploration( r = np.random.randint(3) if r == 0: - self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True) elif r == 1: - self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True) elif r == 2: - self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True) # Append latest observations if not self._realtime_updates: @@ -1564,7 +1573,7 @@ def move_closed_loop(self, goal: np.ndarray, max_time: float = 10.0) -> bool: t0 = timeit.default_timer() self.robot.move_to_nav_posture() while True: - self.robot.navigate_to(goal, blocking=False, timeout=30.0) + self.robot.move_base_to(goal, blocking=False, timeout=30.0) if self.robot.last_motion_failed(): return False position = self.robot.get_base_pose() diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 995c90e0d..7249a46e1 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -29,7 +29,7 @@ from stretch.core.robot import AbstractRobotClient from stretch.motion import PlanResult from stretch.motion.kinematics import HelloStretchIdx, HelloStretchKinematics -from stretch.utils.geometry import angle_difference +from stretch.utils.geometry import angle_difference, xyt_base_to_global from stretch.utils.image import Camera from stretch.utils.memory import lookup_address from stretch.utils.point_cloud import show_point_cloud @@ -175,6 +175,8 @@ def __init__( recv_servo_port, robot_ip, use_remote_computer, message_type="visual servoing data" ) + # Create this pub obs socket to send observations locally + # TODO: do we actually need this? self.pub_obs_socket = self._create_pub_obs_socket(pub_obs_port) # SEnd actions back to the robot for execution @@ -347,7 +349,7 @@ def arm_to( joint_angles: Optional[np.ndarray] = None, gripper: float = None, head: Optional[np.ndarray] = None, - blocking: bool = False, + blocking: bool = True, timeout: float = 10.0, verbose: bool = False, min_time: float = 2.0, @@ -474,11 +476,11 @@ def arm_to( return False return True - def navigate_to( + def move_base_to( self, xyt: Union[ContinuousNavigationAction, np.ndarray], - relative=False, - blocking=False, + relative: bool = False, + blocking: bool = True, timeout: float = 10.0, verbose: bool = False, ): @@ -488,10 +490,35 @@ def navigate_to( else: _xyt = xyt assert len(_xyt) == 3, "xyt must be a vector of size 3" - next_action = {"xyt": _xyt, "nav_relative": relative, "nav_blocking": blocking} + # If it's relative, compute the relative position right now - this helps handle network issues + if relative: + current_xyt = self.get_base_pose() + if verbose: + print("Current pose", current_xyt) + _xyt = xyt_base_to_global(_xyt, current_xyt) + if verbose: + print("Goal pose in global coordinates", _xyt) + + # We never send a relative motion over wireless - this is because we can run into timing issues. + # Instead, we always send the absolute position and let the robot handle the motions itself. + next_action = {"xyt": _xyt, "nav_relative": False, "nav_blocking": blocking} if self._rerun: self._rerun.update_nav_goal(_xyt) - self.send_action(next_action, timeout=timeout, verbose=verbose) + action = self.send_action(next_action, timeout=timeout, verbose=verbose) + + # Make sure we had time to read + if blocking: + block_id = action["step"] + time.sleep(0.1) + # Now, wait for the command to finish + self._wait_for_action( + block_id, + goal_angle=_xyt[2], + verbose=verbose, + timeout=timeout, + # resend_action=action, + # resend_action=current_action, + ) def reset(self): """Reset everything in the robot's internal state""" @@ -591,6 +618,7 @@ def move_to_nav_posture(self): self.send_action(next_action) self._wait_for_head(constants.STRETCH_NAVIGATION_Q, resend_action={"posture": "navigation"}) self._wait_for_mode("navigation") + self._wait_for_arm(constants.STRETCH_NAVIGATION_Q) assert self.in_navigation_mode() def move_to_manip_posture(self): @@ -599,6 +627,7 @@ def move_to_manip_posture(self): time.sleep(0.1) self._wait_for_head(constants.STRETCH_PREGRASP_Q, resend_action={"posture": "manipulation"}) self._wait_for_mode("manipulation") + self._wait_for_arm(constants.STRETCH_PREGRASP_Q) assert self.in_manipulation_mode() def _wait_for_head( @@ -662,6 +691,44 @@ def _wait_for_head( break time.sleep(0.01) + def _wait_for_arm( + self, q: np.ndarray, timeout: float = 10.0, resend_action: Optional[dict] = None + ) -> bool: + """Wait for the arm to move to a particular configuration. + + Args: + q(np.ndarray): The target joint angles + timeout(float): How long to wait for the arm to move + resend_action(dict): The action to resend if the arm is not moving. If none, do not resend. + + Returns: + bool: Whether the arm successfully moved to the target configuration + """ + t0 = timeit.default_timer() + while not self._finish: + joint_positions, joint_velocities, _ = self.get_joint_state() + if joint_positions is None: + continue + + arm_diff = np.abs(joint_positions[HelloStretchIdx.ARM] - q[HelloStretchIdx.ARM]) + lift_diff = np.abs(joint_positions[HelloStretchIdx.LIFT] - q[HelloStretchIdx.LIFT]) + + if arm_diff < self._arm_joint_tolerance and lift_diff < self._lift_joint_tolerance: + return True + + if resend_action is not None: + self.send_socket.send_pyobj(resend_action) + + t1 = timeit.default_timer() + if t1 - t0 > timeout: + logger.error( + f"Timeout waiting for arm to move to arm={q[HelloStretchIdx.ARM]}, lift={q[HelloStretchIdx.LIFT]}" + ) + return False + + # This should never happen + return False + def _wait_for_mode(self, mode, verbose: bool = False, timeout: float = 20.0): t0 = timeit.default_timer() while True: @@ -683,7 +750,7 @@ def _wait_for_action( timeout: float = 10.0, moving_threshold: Optional[float] = None, angle_threshold: Optional[float] = None, - min_steps_not_moving: Optional[int] = 1, + min_steps_not_moving: Optional[int] = None, goal_angle: Optional[float] = None, goal_angle_threshold: Optional[float] = 0.15, resend_action: Optional[dict] = None, @@ -732,8 +799,8 @@ def _wait_for_action( t0 = timeit.default_timer() continue - moved_dist = np.linalg.norm(pos - last_pos) if last_pos is not None else 0 - angle_dist = angle_difference(ang, last_ang) if last_ang is not None else 0 + moved_dist = np.linalg.norm(pos - last_pos) if last_pos is not None else float("inf") + angle_dist = angle_difference(ang, last_ang) if last_ang is not None else float("inf") if goal_angle is not None: angle_dist_to_goal = angle_difference(ang, goal_angle) at_goal = angle_dist_to_goal < goal_angle_threshold @@ -805,7 +872,8 @@ def _update_obs(self, obs): def _update_pose_graph(self, obs): """Update internal pose graph""" with self._obs_lock: - self._pose_graph = obs["pose_graph"] + if "pose_graph" in obs: + self._pose_graph = obs["pose_graph"] def _update_state(self, state: dict) -> None: """Update state internally with lock. This is expected to be much more responsive than using full observations, which should be reserved for higher level control. @@ -889,9 +957,9 @@ def execute_trajectory( len(pt) == 3 or len(pt) == 2 ), "base trajectory needs to be 2-3 dimensions: x, y, and (optionally) theta" # just_xy = len(pt) == 2 - # self.navigate_to(pt, relative, position_only=just_xy, blocking=False) + # self.move_base_to(pt, relative, position_only=just_xy, blocking=False) last_waypoint = i == len(trajectory) - 1 - self.navigate_to( + self.move_base_to( pt, relative, blocking=last_waypoint, @@ -1004,18 +1072,6 @@ def send_action( # Empty it out for the next one current_action = next_action - # Make sure we had time to read - # time.sleep(0.1) - if blocking: - # Wait for the command to finish - self._wait_for_action( - block_id, - goal_angle=goal_angle, - verbose=verbose, - timeout=timeout, - # resend_action=current_action, - ) - # Returns the current action in case we want to do something with it like resend return current_action @@ -1253,6 +1309,9 @@ def stop(self): if self._rerun_thread is not None: self._rerun_thread.join() + # Delete pub obs socket + self.pub_obs_socket.close() + # Close the sockets and context self.recv_socket.close() self.recv_state_socket.close() diff --git a/src/stretch/app/goto_and_execute_skill.py b/src/stretch/app/goto_and_execute_skill.py index 9b57b54a5..2e96a557a 100644 --- a/src/stretch/app/goto_and_execute_skill.py +++ b/src/stretch/app/goto_and_execute_skill.py @@ -309,7 +309,7 @@ def demo_main( blocking=True, ) robot.switch_to_navigation_mode() - robot.navigate_to([0.20, 0, 1.1], relative=True) + robot.move_base_to([0.20, 0, 1.1], relative=True) robot.switch_to_manipulation_mode() robot.arm_to( @@ -324,7 +324,7 @@ def demo_main( blocking=True, ) robot.switch_to_navigation_mode() - robot.navigate_to([0, 0, -1.5], relative=True) + robot.move_base_to([0, 0, -1.5], relative=True) print("- Task finished, going home...") res = planner.plan(robot.get_base_pose(), start_location) diff --git a/src/stretch/app/grasp_object.py b/src/stretch/app/grasp_object.py index 3ade650c9..414e4e0d6 100644 --- a/src/stretch/app/grasp_object.py +++ b/src/stretch/app/grasp_object.py @@ -107,7 +107,7 @@ def main( if reset: robot.move_to_nav_posture() - robot.navigate_to([0.0, 0.0, 0.0], blocking=True, timeout=30.0) + robot.move_base_to([0.0, 0.0, 0.0], blocking=True, timeout=30.0) robot.stop() diff --git a/src/stretch/app/keyboard_teleop.py b/src/stretch/app/keyboard_teleop.py index 786a611e5..24838cd64 100644 --- a/src/stretch/app/keyboard_teleop.py +++ b/src/stretch/app/keyboard_teleop.py @@ -46,7 +46,7 @@ def key_pressed(robot: HomeRobotZmqClient, key): goal_xyt[2] = 0.2 elif key == "d": goal_xyt[2] = -0.2 - robot.navigate_to(goal_xyt, relative=True) + robot.move_base_to(goal_xyt, relative=True) def getch(): diff --git a/src/stretch/app/view_images.py b/src/stretch/app/view_images.py index a0366bd6e..8d13e641d 100644 --- a/src/stretch/app/view_images.py +++ b/src/stretch/app/view_images.py @@ -75,7 +75,7 @@ def main( aruco_detector = GripperArucoDetector() else: aruco_detector = None - + print("This code is running from a file outside of the Docker image!") print("Starting the robot...") robot.start() robot.move_to_manip_posture() @@ -95,8 +95,12 @@ def main( # Get image from robot obs = robot.get_observation() if obs is None: + print("Waiting for observation...") + time.sleep(0.1) continue if obs.rgb is None: + print("Waiting for RGB image...") + time.sleep(0.1) continue # Low res images used for visual servoing and ML servo = robot.get_servo_observation() diff --git a/src/stretch/config/default_planner.yaml b/src/stretch/config/default_planner.yaml index 786242eff..f9706cfee 100644 --- a/src/stretch/config/default_planner.yaml +++ b/src/stretch/config/default_planner.yaml @@ -48,7 +48,7 @@ filters: motion: moving_threshold: 0.001 # How much the robot has to move to be considered "moving" angle_threshold: 0.01 # How much the robot has to rotate to be considered "rotating" - min_steps_not_moving: 2 # How many steps the robot has to not move before we consider it "stopped" + min_steps_not_moving: 5 # How many steps the robot has to not move before we consider it "stopped" joint_tolerance: arm: 0.05 base_x: 0.05 diff --git a/src/stretch/core/robot.py b/src/stretch/core/robot.py index 0dccd6807..ae5894d8a 100644 --- a/src/stretch/core/robot.py +++ b/src/stretch/core/robot.py @@ -37,7 +37,7 @@ def __init__(self): self._base_control_mode = ControlMode.IDLE @abstractmethod - def navigate_to( + def move_base_to( self, xyt: Union[Iterable[float], ContinuousNavigationAction], relative=False, diff --git a/src/stretch/utils/dummy_stretch_client.py b/src/stretch/utils/dummy_stretch_client.py index 7111b71b9..3dba5f35a 100644 --- a/src/stretch/utils/dummy_stretch_client.py +++ b/src/stretch/utils/dummy_stretch_client.py @@ -56,7 +56,7 @@ def __init__( self._robot_model = self self.dof = 3 + 2 + 4 + 2 - def navigate_to(self, xyt, relative=False, blocking=False): + def move_base_to(self, xyt, relative=False, blocking=False): """Move to xyt in global coordinates or relative coordinates.""" raise NotImplementedError() diff --git a/src/stretch_ros2_bridge/README.md b/src/stretch_ros2_bridge/README.md index 7488c5853..1b18465a0 100644 --- a/src/stretch_ros2_bridge/README.md +++ b/src/stretch_ros2_bridge/README.md @@ -67,7 +67,7 @@ r.manip.goto_joint_positions(state) # Navigation r.switch_to_navigation_mode() -r.nav.navigate_to([0.5, 0, 0]) # For Navigation +r.nav.move_base_to([0.5, 0, 0]) # For Navigation ``` ### Running with ORB_SLAM3 diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py index ac37a1f40..4355602db 100644 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py @@ -282,7 +282,7 @@ def execute_trajectory(self, *args, **kwargs): """Open-loop trajectory execution wrapper. Executes a multi-step trajectory; this is always blocking since it waits to reach each one in turn.""" return self.nav.execute_trajectory(*args, **kwargs) - def navigate_to( + def move_base_to( self, xyt: Iterable[float], relative: bool = False, @@ -291,7 +291,7 @@ def navigate_to( """ Move to xyt in global coordinates or relative coordinates. Cannot be used in manipulation mode. """ - return self.nav.navigate_to(xyt, relative=relative, blocking=blocking) + return self.nav.move_base_to(xyt, relative=relative, blocking=blocking) def get_observation( self, diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py index 6bdc709d6..674df5161 100644 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py @@ -138,7 +138,7 @@ def execute_trajectory( len(pt) == 3 or len(pt) == 2 ), "base trajectory needs to be 2-3 dimensions: x, y, and (optionally) theta" just_xy = len(pt) == 2 - self.navigate_to(pt, relative, position_only=just_xy, blocking=False) + self.move_base_to(pt, relative, position_only=just_xy, blocking=False) self.wait_for_waypoint( pt, pos_err_threshold=pos_err_threshold, @@ -147,7 +147,7 @@ def execute_trajectory( verbose=verbose, timeout=per_waypoint_timeout, ) - self.navigate_to(pt, blocking=True) + self.move_base_to(pt, blocking=True) @enforce_enabled def set_velocity(self, v, w): @@ -162,7 +162,7 @@ def set_velocity(self, v, w): self._ros_client.velocity_pub.publish(msg) @enforce_enabled - def navigate_to( + def move_base_to( self, xyt: List[float], relative: bool = False, @@ -214,7 +214,7 @@ def navigate_to( @enforce_enabled def home(self): - self.navigate_to([0.0, 0.0, 0.0], blocking=True) + self.move_base_to([0.0, 0.0, 0.0], blocking=True) # Helper methods diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py index fcdc9aa0e..a8b8db838 100755 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py @@ -157,7 +157,7 @@ def handle_action(self, action: Dict[str, Any]): self.client.in_navigation_mode(), ) print(f"{action['xyt']} {action['nav_relative']} {action['nav_blocking']}") - self.client.navigate_to( + self.client.move_base_to( action["xyt"], relative=action["nav_relative"], )