diff --git a/spot_examples/docs/simple_sub.md b/spot_examples/docs/simple_sub.md index 06de65918..4c1c02de3 100644 --- a/spot_examples/docs/simple_sub.md +++ b/spot_examples/docs/simple_sub.md @@ -52,4 +52,3 @@ Make sure the required environment variable `ROBOT_NAME` is set and the configur ## Author - Kabir Kedia - diff --git a/spot_examples/spot_examples/robot_commander.py b/spot_examples/spot_examples/robot_commander.py index c211829ad..afa1a721b 100644 --- a/spot_examples/spot_examples/robot_commander.py +++ b/spot_examples/spot_examples/robot_commander.py @@ -1,25 +1,24 @@ -import argparse import logging from typing import Optional -import synchros2.process as ros_process import synchros2.scope as ros_scope from bosdyn.client.frame_helpers import BODY_FRAME_NAME, VISION_FRAME_NAME -from bosdyn.client.math_helpers import Quat, SE2Pose, SE3Pose +from bosdyn.client.math_helpers import Quat, SE3Pose from bosdyn.client.robot_command import RobotCommandBuilder from bosdyn_msgs.conversions import convert +from geometry_msgs.msg import Pose from rclpy.node import Node from synchros2.action_client import ActionClientWrapper from synchros2.tf_listener_wrapper import TFListenerWrapper from synchros2.utilities import fqn, namespace_with from spot_msgs.action import RobotCommand # type: ignore -from geometry_msgs.msg import Pose from .simple_spot_commander import SimpleSpotCommander # Where we want the robot to walk to relative to itself + class RobotCommander: def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None) -> None: self._logger = logging.getLogger(fqn(self.__class__)) @@ -61,11 +60,11 @@ def initialize_robot(self) -> bool: self._logger.info("Successfully stood up.") return True - def walk_forward_with_world_frame_goal(self, waypoint) -> None: + def walk_forward_with_world_frame_goal(self, waypoint: Pose) -> bool: """ Walk forward to a goal in the world frame. Arguments: - waypoint: The waypoint to walk to with respect to the body frame. + waypoint: The waypoint to walk to with respect to the body frame. Type is geometry_msgs.msg.Pose """ self._logger.info("Walking") @@ -108,4 +107,4 @@ def walk_forward_with_world_frame_goal(self, waypoint) -> None: self._logger.info(f"Navigated to waypoint: {waypoint}") else: self._logger.info(f"Failed to navigate to waypoint: {waypoint}") - return result \ No newline at end of file + return result diff --git a/spot_examples/spot_examples/simple_sub.py b/spot_examples/spot_examples/simple_sub.py index 293552649..a546f42e3 100644 --- a/spot_examples/spot_examples/simple_sub.py +++ b/spot_examples/spot_examples/simple_sub.py @@ -1,31 +1,28 @@ # This file interfaces with spot to send action. Helper files are robot_commander and simple_spot_commander import os -from typing import List from contextlib import closing -from std_msgs.msg import String -from geometry_msgs.msg import Pose, PoseArray -from synchros2.subscription import Subscription import synchros2.process as ros_process +from geometry_msgs.msg import Pose +from synchros2.subscription import Subscription + from .robot_commander import RobotCommander -from ament_index_python.packages import get_package_share_directory @ros_process.main() def main() -> None: # print("here") - robot_name= os.getenv("ROBOT_NAME") # Get robot name from environment variable + robot_name = os.getenv("ROBOT_NAME") # Get robot name from environment variable robot_commander = RobotCommander(robot_name=robot_name) - + robot_commander._logger.info(f"name:{robot_commander._robot_name}") result = robot_commander.initialize_robot() if result is False: robot_commander._logger.info("Failed to initialize robot") - return - - robot_commander._logger.info("Initialized robot") + return + robot_commander._logger.info("Initialized robot") topic_data = Subscription(Pose, "/pose_commands") @@ -52,5 +49,6 @@ def main() -> None: is_busy = False latest_message = None # Clear the buffer after processing + if __name__ == "__main__": main()