Skip to content

Commit

Permalink
preconfigure lint changes
Browse files Browse the repository at this point in the history
  • Loading branch information
kabirkedia committed Feb 14, 2025
1 parent 86f4622 commit 7340c1b
Show file tree
Hide file tree
Showing 3 changed files with 14 additions and 18 deletions.
1 change: 0 additions & 1 deletion spot_examples/docs/simple_sub.md
Original file line number Diff line number Diff line change
Expand Up @@ -52,4 +52,3 @@ Make sure the required environment variable `ROBOT_NAME` is set and the configur

## Author
- Kabir Kedia <kabirk@andrew.cmu.edu>

13 changes: 6 additions & 7 deletions spot_examples/spot_examples/robot_commander.py
Original file line number Diff line number Diff line change
@@ -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__))
Expand Down Expand Up @@ -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")
Expand Down Expand Up @@ -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
return result
18 changes: 8 additions & 10 deletions spot_examples/spot_examples/simple_sub.py
Original file line number Diff line number Diff line change
@@ -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")

Expand All @@ -52,5 +49,6 @@ def main() -> None:
is_busy = False
latest_message = None # Clear the buffer after processing


if __name__ == "__main__":
main()

0 comments on commit 7340c1b

Please sign in to comment.