From 482644484dab680223be9a8526670ac2df4d3c40 Mon Sep 17 00:00:00 2001 From: Christian Luu <35637788+christianvluu@users.noreply.github.com> Date: Wed, 29 Nov 2023 22:40:01 -0500 Subject: [PATCH 1/2] CI (#19) --- .github/workflows/.pylintrc | 10 ++++++ .github/workflows/pylint.yml | 35 +++++++++++++++++++ .../buggy/scripts/visualization/telematics.py | 25 ++++++------- 3 files changed, 55 insertions(+), 15 deletions(-) create mode 100644 .github/workflows/.pylintrc create mode 100644 .github/workflows/pylint.yml diff --git a/.github/workflows/.pylintrc b/.github/workflows/.pylintrc new file mode 100644 index 00000000..05decfd0 --- /dev/null +++ b/.github/workflows/.pylintrc @@ -0,0 +1,10 @@ +[MESSAGES CONTROL] + +disable=all + +enable= + bad-indentation, + trailing-whitespace, + wrong-import-order, + unused-variable, + unused-import diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml new file mode 100644 index 00000000..8ccf8a98 --- /dev/null +++ b/.github/workflows/pylint.yml @@ -0,0 +1,35 @@ +name: Pylint + +on: [push] + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.8", "3.9", "3.10"] + steps: + - uses: actions/checkout@v3 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install pylint + - name: Getting changed Python files and Analysing the code with pylint + run: | + git fetch origin ${CI_MERGE_REQUEST_TARGET_BRANCH_NAME} + echo CI_COMMIT_SHA=${GITHUB_SHA} + tmp_files=$(git diff --name-only origin/main ${CI_COMMIT_SHA}) + echo "Changed files are $tmp_files" + if [ -z "$(echo "$tmp_files" | grep "\.py")" ]; then exit 0; else echo "Python files are found"; fi + tmp_pfiles=$(echo "$tmp_files" | grep "\.py") + echo "Python files are $tmp_pfiles" + if [[ -z "$tmp_pfiles" ]]; then + echo "No files to lint" + else + echo "Running Linter!" + pylint --rc-file=/home/runner/work/RoboBuggy2/RoboBuggy2/.github/workflows/.pylintrc $tmp_pfiles + fi diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index fe99c4fd..2951b6f7 100755 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ b/rb_ws/src/buggy/scripts/visualization/telematics.py @@ -28,14 +28,13 @@ def __init__(self): self.gnss1_fixinfo_publisher = rospy.Publisher("/gnss1/fix_info_republished", String, queue_size=10) self.gnss1_fixinfo_int_publisher = rospy.Publisher("/gnss1/fix_info_republished_int", Int8, queue_size=10) self.gnss1_fixinfo_subscriber = rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher)) - + self.gnss2_fixinfo_publisher = rospy.Publisher("/gnss2/fix_info_republished", String, queue_size=10) self.gnss2_fixinfo_int_publisher = rospy.Publisher("/gnss2/fix_info_republished_int", Int8, queue_size=10) self.gnss2_fixinfo_subscriber = rospy.Subscriber("/gnss2/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher)) - + def convert_odometry_to_navsatfix(self, msg): """Convert Odometry-type to NavSatFix-type for plotting on Foxglove -x` Args: msg (Odometry): odometry as per INS """ @@ -48,7 +47,7 @@ def convert_odometry_to_navsatfix(self, msg): new_msg.longitude = long new_msg.altitude = down self.odom_publisher.publish(new_msg) - + def convert_navsatfix_to_pose_covariance(self, msg, publishers): """Convert NavSatFix-type and related covariance matrix to Pose-type and array respectively for easy visualization in Foxglove. @@ -70,14 +69,14 @@ def convert_navsatfix_to_pose_covariance(self, msg, publishers): round(msg.position_covariance[4], 4), round(msg.position_covariance[8], 4)] publishers[1].publish(covariance) - + def republish_fixinfo(self, msg, publishers): """ convert gnss/fixinfo to a string for visualization in foxglove """ fix_type = msg.fix_type fix_string = "fix type: " - + if (fix_type == 0): fix_string += "FIX_3D" elif (fix_type == 1): @@ -92,17 +91,13 @@ def republish_fixinfo(self, msg, publishers): fix_string += "FIX_RTK_FLOAT" else: fix_string += "FIX_RTK_FIXED" - - fix_string += "\nsbas_used: " + str(msg.sbas_used) + + fix_string += "\nsbas_used: " + str(msg.sbas_used) fix_string += "\ndngss_used: " + str(msg.dngss_used) publishers[0].publish(fix_string) publishers[1].publish(fix_type) - - - if __name__ == "__main__": - rospy.init_node("telematics") - telem = Telematics() - rospy.spin() - + rospy.init_node("telematics") + telem = Telematics() + rospy.spin() From 061e9523d1764f50d719f33ff34969a0c58e97b0 Mon Sep 17 00:00:00 2001 From: TiaSinghania <59597284+TiaSinghania@users.noreply.github.com> Date: Thu, 8 Feb 2024 18:49:25 -0500 Subject: [PATCH 2/2] Sw/UI velocity (#38) * Create velocity_updater.py Basic framework of VelocityUpdater class, just applies constant acceleration to the buggy * updated engine, keyboard import erroring * added publisher/subscriber for manual velocity (can publish via foxglove node) * fixed python reqs * cleaned up comments * integrated manual controller with simulated control * removed print debugs + bound scale to arrow keys * reversed binding * final cleanup --------- Co-authored-by: PatXue <95881915+PatXue@users.noreply.github.com> Co-authored-by: TiaSinghania --- python-requirements.txt | 2 + rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 4 ++ rb_ws/src/buggy/launch/sim_2d_single.launch | 2 +- .../src/buggy/scripts/2d_sim/controller_2d.py | 8 +--- rb_ws/src/buggy/scripts/2d_sim/engine.py | 26 ++++------- rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 44 +++++++++++++++++++ rb_ws/src/buggy/scripts/auton/autonsystem.py | 18 +++----- 7 files changed, 68 insertions(+), 36 deletions(-) create mode 100644 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py diff --git a/python-requirements.txt b/python-requirements.txt index cbc5d7de..254c5dac 100755 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -8,3 +8,5 @@ pymap3d==3.0.1 scipy==1.10.1 trimesh==3.23.5 utm==0.7.0 +keyboard==0.13.5 +tk==0.1.0 \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch index 61ad3111..4f78a3d9 100644 --- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch +++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch @@ -21,6 +21,8 @@ args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/> + + diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch index 3554bfcc..75cc97c3 100644 --- a/rb_ws/src/buggy/launch/sim_2d_single.launch +++ b/rb_ws/src/buggy/launch/sim_2d_single.launch @@ -20,4 +20,4 @@ - + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py index 9598c485..2caa89dd 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py @@ -1,8 +1,8 @@ #! /usr/bin/env python3 +import sys +import threading import rospy from std_msgs.msg import Float64 -import threading -import numpy as np class Controller: def __init__(self, buggy_name): @@ -14,7 +14,6 @@ def __init__(self, buggy_name): self.lock = threading.Lock() self.set_steering(0) self.set_velocity(0) - def set_steering(self, angle: float): """Set the steering angle and publish to simulator engine @@ -26,9 +25,7 @@ def set_steering(self, angle: float): with self.lock: self.steering_angle = angle - self.steering_publisher.publish(msg) - def set_velocity(self, vel: float): """Set the velocity and publish to simulator engine @@ -40,7 +37,6 @@ def set_velocity(self, vel: float): with self.lock: self.velocity = vel - self.velocity_publisher.publish(msg) if __name__ == "__main__": diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 8bb34526..3d48bd34 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -1,14 +1,13 @@ #! /usr/bin/env python3 -import rospy +import sys +import threading from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance from std_msgs.msg import Float64 from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry -import threading import numpy as np import utm -import time -import sys +import rospy class Simulator: @@ -20,7 +19,7 @@ class Simulator: # Start positions for Outdoor track START_LAT = 40.443024364623916 START_LONG = -79.9409643423245 - NOISE = True # Noisy outputs for nav/odom? + NOISE = True # Noisy outputs for nav/odom? def __init__(self, starting_pose, velocity, buggy_name): """ @@ -36,10 +35,10 @@ def __init__(self, starting_pose, velocity, buggy_name): self.steering_subscriber = rospy.Subscriber( buggy_name + "/input/steering", Float64, self.update_steering_angle ) + # To read from velocity self.velocity_subscriber = rospy.Subscriber( buggy_name + "/velocity", Float64, self.update_velocity ) - # to plot on Foxglove (no noise) self.navsatfix_publisher = rospy.Publisher( buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1 @@ -49,7 +48,6 @@ def __init__(self, starting_pose, velocity, buggy_name): self.navsatfix_noisy_publisher = rospy.Publisher( buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1 ) - # (UTM east, UTM north, HEADING(degs)) self.starting_poses = { "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110), @@ -68,8 +66,7 @@ def __init__(self, starting_pose, velocity, buggy_name): # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG) # self.e_utm = utm_coords[0] # self.n_utm = utm_coords[1] - - self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose] + self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose] self.velocity = velocity # m/s self.steering_angle = 0 # degrees @@ -92,10 +89,10 @@ def update_velocity(self, data: Float64): Args: data (Float64): velocity in m/s + source (string): whether incoming data is manual or simulated """ with self.lock: self.velocity = data.data - def get_steering_arc(self): # Adapted from simulator.py (Joseph Li) # calculate the radius of the steering arc @@ -109,7 +106,6 @@ def get_steering_arc(self): return np.inf return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle)) - def dynamics(self, state, v): """ Calculates continuous time bicycle dynamics as a function of state and velocity @@ -121,7 +117,7 @@ def dynamics(self, state, v): dstate (np.Array): time derivative of state from dynamics """ l = Simulator.WHEELBASE - x, y, theta, delta = state + _, _, theta, delta = state return np.array([v * np.cos(theta), v * np.sin(theta), @@ -136,7 +132,6 @@ def step(self): n_utm = self.n_utm velocity = self.velocity steering_angle = self.steering_angle - # Calculate new position if steering_angle == 0.0: # Straight @@ -146,9 +141,7 @@ def step(self): else: # steering radius radius = self.get_steering_arc() - distance = velocity / self.rate - delta_heading = distance / radius heading_new = heading + np.rad2deg(delta_heading) / 2 e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new)) @@ -159,12 +152,10 @@ def step(self): self.e_utm = e_utm_new self.n_utm = n_utm_new self.heading = heading_new - def publish(self): """Publishes the pose the arrow in visualizer should be at""" p = Pose() time_stamp = rospy.Time.now() - with self.lock: p.position.x = self.e_utm p.position.y = self.n_utm @@ -300,3 +291,4 @@ def loop(self): buggy_name = sys.argv[3] sim = Simulator(starting_pose, velocity, buggy_name) sim.loop() + diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py new file mode 100644 index 00000000..8a2061c4 --- /dev/null +++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python3 +import sys +import threading +import tkinter as tk +from controller_2d import Controller +import rospy + +class VelocityUI: + def __init__(self, init_vel: float, buggy_name: str): + self.buggy_vel = 0 # So the buggy doesn't start moving without user input + self.controller = Controller(buggy_name) + self.lock = threading.Lock() + + self.root = tk.Tk() + + self.root.title('Manual Velocity') + self.root.geometry('600x100') + self.root.configure(background = '#342d66') + + self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30) + self.scale.pack() + + self.root.bind("", lambda i: self.scale.set(self.scale.get() + 2)) + self.root.bind("", lambda d: self.scale.set(self.scale.get() - 2)) + + def step(self): + """sets velocity of buggy to the current scale value + called once every tick + """ + self.root.update_idletasks() + self.root.update() + '''Update velocity of buggy''' + self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision + self.controller.set_velocity(self.buggy_vel) + +if __name__ == "__main__": + rospy.init_node("velocity_ui") + init_vel = float(sys.argv[1]) + buggy_name = sys.argv[2] + vel = VelocityUI(init_vel, buggy_name) + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + vel.step() + rate.sleep() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index ba43285c..8b50232d 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 -import rospy import sys +from threading import Lock + +import numpy as np +import rospy # ROS Message Imports from std_msgs.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry -import numpy as np -from threading import Lock - from trajectory import Trajectory from world import World from controller import Controller @@ -35,24 +35,20 @@ class AutonSystem: controller: Controller = None brake_controller: BrakeController = None lock = None - steer_publisher = None - ticks = 0 def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None: self.trajectory = trajectory self.controller = controller self.brake_controller = brake_controller - self.lock = Lock() self.ticks = 0 self.msg = None - if (is_sim): rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg) - + rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg) self.covariance_warning_publisher = rospy.Publisher( buggy_name + "/debug/is_high_covariance", Bool, queue_size=1 ) @@ -84,11 +80,10 @@ def update_speed(self, msg): def update_msg(self, msg): with self.lock: self.msg = msg - + def tick_caller(self): while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait rospy.sleep(0.001) - # wait for covariance matrix to be better while ((not rospy.is_shutdown()) and (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)): @@ -175,7 +170,6 @@ def tick(self): ctrller = ModelPredictiveController(buggy_name, start_index) if (ctrller == None): raise Exception("Invalid Controller Argument") - auton_system = AutonSystem( trajectory, ctrller,