Skip to content

Commit

Permalink
Fixed pylint findings
Browse files Browse the repository at this point in the history
  • Loading branch information
gabryelreyes committed Jul 19, 2024
1 parent e00a7cb commit c93a53f
Show file tree
Hide file tree
Showing 13 changed files with 129 additions and 144 deletions.
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
""" Controller for Obstacle proto."""

import sys
from controller import Robot
from controller import Robot # pylint: disable=import-error

# Debounce time of collision in seconds.
DEBOUNCE_TIME = 1
Expand Down
85 changes: 0 additions & 85 deletions webots/controllers/PlatoonSupervisor/PlatoonSupervisor.py

This file was deleted.

8 changes: 6 additions & 2 deletions webots/controllers/Supervisor/robot_observer.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,16 @@

import math


class RobotObserver():
"""The robot observer provides position and orientation information
about a robot by using a supervisor.
"""

def __init__(self, robot_node):
self._robot_node = robot_node
self._ref_position = [0, 0, 0] # x, y, z
self._ref_orientation_euler = [0, 0, 0, 0] # roll, pitch and yaw angle
self._ref_position = [0, 0, 0] # x, y, z
self._ref_orientation_euler = [0, 0, 0, 0] # roll, pitch and yaw angle
self._factor_m_to_mm = 1000

def _get_position(self):
Expand Down Expand Up @@ -133,6 +135,7 @@ def rad_to_deg(angle_rad):
"""
return angle_rad * 180 / math.pi


def has_position_changed(position, position_old):
"""Returns whether the position changed.
Expand All @@ -152,6 +155,7 @@ def has_position_changed(position, position_old):

return has_changed


def has_orientation_changed(orientation, orientation_old):
"""Returns whether the orientation changed.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,16 @@

import math
import sys
from controller import Supervisor
from robot_observer import RobotObserver
from controller import Supervisor # pylint: disable=import-error
from robot_observer import RobotObserver, has_position_changed, has_orientation_changed

# The PROTO DEF name must be given!
ROBOT_NAME = "ROBOT"

# The supervisor receiver name.
SUPERVISOR_RX_NAME = "serialComRx"


def serial_com_read(serial_com_rx_device):
"""Read from serial communication device.
Expand All @@ -30,6 +31,7 @@ def serial_com_read(serial_com_rx_device):

return rx_data


def rad_to_deg(angle_rad):
"""Convert angle in rad to degree.
Expand All @@ -41,6 +43,7 @@ def rad_to_deg(angle_rad):
"""
return angle_rad * 180 / math.pi


def convert_angle_to_2pi(angle):
"""Convert angle from range [-PI; PI] in rad to [0; 2PI].
Expand All @@ -55,6 +58,7 @@ def convert_angle_to_2pi(angle):

return angle


def convert_webots_angle_to_ru_angle(webots_angle):
"""Convert Webots angle to RadonUlzer angle.
Expand All @@ -73,51 +77,15 @@ def convert_webots_angle_to_ru_angle(webots_angle):

webots_angle_2pi = convert_angle_to_2pi(webots_angle)

# TODO Handling the negative range.
# TO DO Handling the negative range.

return webots_angle_2pi

def has_position_changed(position, position_old):
"""Returns whether the position changed.
Args:
position (list[float]): Position 1
position_old (list[float]): Position 2
Returns:
bool: If changed, it will return True otherwise False.
"""
has_changed = False

for idx, value in enumerate(position):
if int(value) != int(position_old[idx]):
has_changed = True
break

return has_changed

def has_orientation_changed(orientation, orientation_old):
"""Returns whether the orientation changed.
Args:
orientation (list[float]): Orientation 1
orientation_old (list[float]): Orientation 2
Returns:
bool: If changed, it will return True otherwise False.
"""
has_changed = False

for idx, value in enumerate(orientation):
if int(rad_to_deg(value)) != int(rad_to_deg(orientation_old[idx])):
has_changed = True
break

return has_changed

class OdometryData(): # pylint: disable=too-few-public-methods
class OdometryData(): # pylint: disable=too-few-public-methods
"""Odometry data container.
"""

def __init__(self) -> None:
self.x = 0
self.y = 0
Expand All @@ -130,6 +98,10 @@ def reset(self):
self.y = 0
self.yaw_angle = 0


# pylint: disable=too-many-branches
# pylint: disable=too-many-statements
# pylint: disable=too-many-locals
def main_loop():
"""Main loop:
- Perform simulation steps until Webots is stopping the controller-
Expand All @@ -150,7 +122,8 @@ def main_loop():
supervisor_com_rx = supervisor.getDevice(SUPERVISOR_RX_NAME)

if supervisor_com_rx is None:
print(f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.")
print(
f"No supervisor communication possible, because {SUPERVISOR_RX_NAME} not found.")
else:
supervisor_com_rx.enable(timestep)

Expand Down Expand Up @@ -192,9 +165,10 @@ def main_loop():

# Robot odometry data received?
elif command[0] == "ODO":
robot_odometry.x = int(command[1]) # [mm]
robot_odometry.y = int(command[2]) # [mm]
robot_odometry.yaw_angle = float(command[3]) / 1000.0 # [rad]
robot_odometry.x = int(command[1]) # [mm]
robot_odometry.y = int(command[2]) # [mm]
robot_odometry.yaw_angle = float(
command[3]) / 1000.0 # [rad]

# Unknown command.
else:
Expand All @@ -203,15 +177,18 @@ def main_loop():
robot_position = robot_observer.get_rel_position()
robot_orientation = robot_observer.get_rel_orientation()

any_change = has_position_changed(robot_position, robot_position_old)
any_change = has_position_changed(
robot_position, robot_position_old)

if any_change is False:
any_change = has_orientation_changed(robot_orientation, robot_orientation_old)
any_change = has_orientation_changed(
robot_orientation, robot_orientation_old)

if any_change is True:

robot_yaw_angle = robot_orientation[2]
yaw_ru_angle = convert_webots_angle_to_ru_angle(robot_yaw_angle)
yaw_ru_angle = convert_webots_angle_to_ru_angle(
robot_yaw_angle)

print(f"{int(robot_position[0])}, ", end="")
print(f"{int(robot_position[1])}, ", end="")
Expand All @@ -225,4 +202,5 @@ def main_loop():

return status


sys.exit(main_loop())
88 changes: 88 additions & 0 deletions webots/controllers/platoon_supervisor/platoon_supervisor.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
""" Platoon Supervisor controller for Webots.
Moves the obstacle up and down when the space key is pressed. """

import sys
from controller import Supervisor # pylint: disable=import-error

DEBOUNCE_TIME = 0.2 # Debounce time for the spacebar in seconds
OBSTACLE_NAME = "OBSTACLE" # Name of the obstacle node
TRANSLATION_FIELD = "translation" # Field name for the translation

RAISED_HEIGHT = 0.4 # Active height in meters
LOWERED_HEIGHT = 0.06 # Rest height in meters

# Create the Supervisor instance.
supervisor = Supervisor()

# Get the keyboard device
keyboard = supervisor.getKeyboard()


def main_loop():
"""Main loop:
- Perform simulation steps until Webots is stopping the controller-
Returns:
number: Status
"""

status = 0

# Get the obstacle node
obstacle_node = supervisor.getFromDef(OBSTACLE_NAME)

if obstacle_node is None:

print(f"Robot DEF {OBSTACLE_NAME} not found.")
status = -1

else:
# Get the translation field of the obstacle node
translation_field = obstacle_node.getField(TRANSLATION_FIELD)

if translation_field is None:
print(
f"Translation field {TRANSLATION_FIELD} of {OBSTACLE_NAME} not found.")
status = -1

if status == 0:
# Get the time step of the current world.
timestep = int(supervisor.getBasicTimeStep())

# Enable the keyboard device
keyboard.enable(timestep)

# Get current time for debouncing
last_time = supervisor.getTime()

# Set the initial state to raised obstacle
is_obstacle_lowered = False
curr_pos = translation_field.getSFVec3f()
translation_field.setSFVec3f(
[curr_pos[0], curr_pos[1], RAISED_HEIGHT])

while supervisor.step(timestep) != -1:
if keyboard.getKey() == ord(' '): # If space key is pressed
if (supervisor.getTime() - last_time) > DEBOUNCE_TIME: # Debounce space key

# Update last time
last_time = supervisor.getTime()

# Get the current position of the obstacle
curr_pos = translation_field.getSFVec3f()

# Move the obstacle
if is_obstacle_lowered is True: # Raise the obstacle
translation_field.setSFVec3f(
[curr_pos[0], curr_pos[1], RAISED_HEIGHT])
else: # Lower the obstacle
translation_field.setSFVec3f(
[curr_pos[0], curr_pos[1], LOWERED_HEIGHT])

# Toggle the obstacle state
is_obstacle_lowered = not is_obstacle_lowered

return status


sys.exit(main_loop())
2 changes: 1 addition & 1 deletion webots/protos/Obstacle.proto
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,6 @@ PROTO Obstacle [
]
name IS name
boundingObject USE BODY
controller "Obstacle"
controller "obstacle"
}
}
2 changes: 1 addition & 1 deletion webots/worlds/ETrack.wbt
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,6 @@ DEF ROBOT Zumo32U4 {
}
Supervisor {
name "Supervisor"
controller "Supervisor"
controller "supervisor"
supervisor TRUE
}
Loading

0 comments on commit c93a53f

Please sign in to comment.