Skip to content

Commit

Permalink
pep257 lint
Browse files Browse the repository at this point in the history
  • Loading branch information
Serafadam committed Jan 3, 2024
1 parent cc9cb28 commit e85014f
Show file tree
Hide file tree
Showing 7 changed files with 148 additions and 105 deletions.
77 changes: 46 additions & 31 deletions robot_py/robot_py/robot/api/ros/ros_interface.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# needed by RH
# needed by RH to import launch
import sys
del sys.path[0]
sys.path.append('')
Expand Down Expand Up @@ -38,12 +38,15 @@

class ROSInterface:
"""
A class that manages ROS2 functionalities for a robot or a system. It includes initializing ROS2 context,
creating and managing nodes, publishers, and subscribers. It also handles the startup and shutdown processes
A class that manages ROS2 functionalities for a robot or a system.
It includes initializing ROS2 context,
creating and managing nodes, publishers, and subscribers.
It also handles the startup and shutdown processes
for ROS2.
Attributes:
ros_proc: Process for running ROS2 hardware-related commands.
Attributes
----------
_name (str): Name of the ROS2 node.
_context (rclpy.context.Context | None): The ROS2 context.
_node (rclpy.node.Node | None): The ROS2 node.
Expand All @@ -54,8 +57,14 @@ class ROSInterface:
_timers (dict[str, Timer]): Dictionary of ROS2 timers.
_tf_buffer: The TF2 buffer.
_tf_listener: The TF2 listener.
Methods:
_executor (Executor): The ROS2 executor.
_executor_thread (threading.Thread): The thread for the ROS2 executor.
_launch_service (LaunchService): The ROS2 launch service.
_stop_event (multiprocessing.Event): The event for stopping the ROS2 launch service.
_process (multiprocessing.Process): The process for running the ROS2 launch service.
Methods
-------
get_node(): Returns the current ROS2 node.
start_hardware_process(): Starts the hardware process for ROS2.
start(): Initializes and starts the ROS2 node and executor.
Expand All @@ -70,19 +79,24 @@ class ROSInterface:
create_action_client(action_name, action_type): Creates an action client for a given action.
call_async_action_simple(action_name, goal): Calls an action asynchronously.
call_async_action(action_name, goal, goal_response_callback, goal_result_callback, goal_feedback_callback): Calls an action asynchronously.
"""

def __init__(self, name: str, namespace='/rae') -> None:
"""
Initializes the ROS2Manager instance.
Initialize the ROS2Manager instance.
Args:
----
name (str): The name of the ROS2 node.
namespace (str): The namespace of the ROS2 node.
"""
self._namespace = namespace
self._ros_proc = None
self._rs = None
self._name = name
self._launch_service = None
self._stop_event = None
self._process = None
self._context: rclpy.context.Context | None = None
self._node: rclpy.node.Node | None = None
self._publishers: dict[str, Publisher] = {}
Expand All @@ -97,22 +111,15 @@ def get_node(self):
return self._node

def start_hardware_process(self):
"""
Starts RAE hardware drivers in a separate process.
"""
self._rs = LaunchService(noninteractive=True)
"""Start RAE hardware drivers in a separate process."""
self._launch_service = LaunchService(noninteractive=True)
ld = LaunchDescription([IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('rae_hw'), 'launch', 'control.launch.py')))])
self._stop_event = multiprocessing.Event()
self._process = multiprocessing.Process(target=self._run_process, args=(self._stop_event, ld), daemon=True)
self._process = multiprocessing.Process(
target=self._run_process, args=(self._stop_event, ld), daemon=True)
self._process.start()
print('test')
# env = dict(os.environ)
# script_name = os.path.join(get_package_share_directory(
# 'robot_py'), 'scripts', 'start_ros.sh')
# self._ros_proc = subprocess.Popen(
# f"bash -c 'chmod +x {script_name} ; {script_name}'", shell=True, env=env, preexec_fn=os.setsid
# )

def _run_process(self, stop_event, launch_description):
loop = asyncio.get_event_loop()
launch_service = LaunchService()
Expand All @@ -122,9 +129,15 @@ def _run_process(self, stop_event, launch_description):
if not launch_task.done():
asyncio.ensure_future(launch_service.shutdown(), loop=loop)
loop.run_until_complete(launch_task)

def start(self, start_hardware) -> None:
"""
Runs RAE hardware drivers process.Initializes and starts the ROS2 node and executor. It sets up the ROS2 context and starts the ROS2 spin.
Run RAE hardware drivers process.Initializes and starts the ROS2 node and executor. It sets up the ROS2 context and starts the ROS2 spin.
Args:
----
start_hardware (bool): Whether to start the hardware process or not.
"""
if start_hardware:
self.start_hardware_process()
Expand Down Expand Up @@ -153,16 +166,15 @@ def _spin(self):
log.info("rlcpy thread> Done")

def stop_ros_process(self):
"""
Stops the ROS2 hardware process by terminating the related subprocess.
"""

"""Stop the ROS2 hardware process by terminating the related subprocess."""
self._stop_event.set()
self._process.join()

def stop(self) -> None:
"""
Shuts down RAE drivers, ROS2 node and context. This includes stopping the executor, destroying publishers and subscribers,
Shut down RAE drivers, ROS2 node and context.
This includes stopping the executor, destroying publishers subscribers, service clients, action clients, timers
and shutting down the ROS2 context.
"""
self.stop_ros_process()
Expand Down Expand Up @@ -334,19 +346,22 @@ def cancel_action(self, action_name: str):

def get_frame_position(self, source_frame, target_frame) -> TransformStamped:
"""
Gets the position of a frame relative to another frame.
Get the position of a frame relative to another frame.
Args:
----
source_frame (str): The source frame.
target_frame (str): The target frame.
Returns:
Returns
-------
TransformStamped: The position of the source frame relative to the target frame.
"""
try:
transform = self._tf_buffer.lookup_transform(
target_frame, source_frame, rclpy.time.Time())
return transform
except TransformException as e:
log.error(e)
return None
return None
14 changes: 9 additions & 5 deletions robot_py/robot_py/robot/audio.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,22 +10,26 @@ class AudioController:
"""
A class for controlling the robot's audio.
Attributes:
Attributes
----------
ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities.
audio_client (Client): A ROS2 client for playing audio.
assets_path (str): The path to the robot's assets directory.
Methods:
Methods
-------
create_and_send_request(audio_file_path): Creates and sends a request to play an audio file.
play_audio_file(audio_file_path): Plays an audio file.
honk(): Plays a horn sound.
play_random_sfx(): Plays a random sound effect.
"""

def __init__(self, ros_interface):
self.ros_interface = ros_interface
self.audio_client = self.ros_interface.create_service_client(
self._ros_interface = ros_interface
self._ros_interface.create_service_client(
'/play_audio', PlayAudio)
self.assets_path = os.path.join(
self._assets_path = os.path.join(
get_package_share_directory('robot_py'), 'assets')
log.info("Audio Controller ready")

Expand Down
24 changes: 15 additions & 9 deletions robot_py/robot_py/robot/display.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@ def quaternion_to_rotation_matrix(q):
"""
Convert a quaternion into a rotation matrix.
Parameters:
Arguments
-----------
q (tuple): A quaternion represented as (q_w, q_x, q_y, q_z).
Returns:
Returns
-------
numpy.ndarray: A 3x3 rotation matrix.
"""
q_w, q_x, q_y, q_z = q
sq_w, sq_x, sq_y, sq_z = q_w ** 2, q_x ** 2, q_y ** 2, q_z ** 2
Expand All @@ -37,21 +40,24 @@ class DisplayController:
"""
A class for controlling the robot's display.
Attributes:
ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities.
bridge (CvBridge): An object for converting between ROS2 and OpenCV image formats.
screen_width (int): The width of the robot's display.
screen_height (int): The height of the robot's display.
assets_path (str): The path to the robot's assets directory.
Attributes
----------
_ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities.
_bridge (CvBridge): An object for converting between ROS2 and OpenCV image formats.
_screen_width (int): The width of the robot's display.
_screen_height (int): The height of the robot's display.
_assets_path (str): The path to the robot's assets directory.
Methods:
Methods
-------
stop(): Stops the display.
display_default(): Displays the default image on the robot's display.
display_face(payload): Displays a face on the robot's display.
display_image(image_data): Displays an image on the robot's display.
display_imu_data(imu_data): Displays IMU data on the robot's display.
display_animation(): Displays an animation on the robot's display.
ball_callback(): Callback method for displaying an animation on the robot's display.
"""

def __init__(self, ros_interface):
Expand Down
8 changes: 6 additions & 2 deletions robot_py/robot_py/robot/led.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,16 @@ class LEDController:
"""
A class for controlling the robot's LEDs.
Attributes:
Attributes
----------
ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities.
Methods:
Methods
-------
set_leds(payload): Sets the robot's LEDs to a given color.
"""

def __init__(self, ros_interface):
self._ros_interface = ros_interface
self._ros_interface.create_publisher("/leds", LEDControl)
Expand Down
16 changes: 11 additions & 5 deletions robot_py/robot_py/robot/movement.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,14 @@ class MovementController:
"""
A class for controlling the robot's movement.
Attributes:
Attributes
----------
ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities.
Methods:
Methods
-------
move(linear, angular): Moves the robot in a given direction.
"""

def __init__(self, ros_interface):
Expand All @@ -20,11 +23,13 @@ def __init__(self, ros_interface):

def move(self, linear, angular):
"""
Moves the robot in a given direction.
Move the robot in a given direction.
Args:
----
linear (float): The linear velocity.
angular (float): The angular velocity.
"""
twist_msg = Twist()
twist_msg.linear.x = float(linear)
Expand All @@ -37,9 +42,10 @@ def move(self, linear, angular):

def get_odom_position(self) -> TransformStamped:
"""
Gets the robot's current position relative to odom frame. Returns None if the robot's position is not available.
Get the robot's current position relative to odom frame. Returns None if the robot's position is not available.
Returns:
Returns
-------
TransformStamped: The robot's current position.
"""
Expand Down
Loading

0 comments on commit e85014f

Please sign in to comment.