From 09825828e8be78f2bfe12daefceefc9166ec107e Mon Sep 17 00:00:00 2001 From: chosterto Date: Sun, 5 Mar 2023 15:21:01 -0500 Subject: [PATCH 1/4] working on mouse control for rover --- ROS_WS/src/control/src/mouse_drive_control.py | 78 +++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100755 ROS_WS/src/control/src/mouse_drive_control.py diff --git a/ROS_WS/src/control/src/mouse_drive_control.py b/ROS_WS/src/control/src/mouse_drive_control.py new file mode 100755 index 0000000..308976d --- /dev/null +++ b/ROS_WS/src/control/src/mouse_drive_control.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +import rospy +from geometry_msgs.msg import Twist +import gi +gi.require_version('Gtk', '2.0') +from gi.repository import Gtk, Gdk + +rospy.init_node('car_controller2', anonymous=False) +publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) +vel_msg = Twist() +is_toggled = False +size = 250 + +max_speed_mps = 0.5 + + +def on_move(box, event): + global vel_msg, is_toggled + y_pos = size - event.y + x_pos = size - event.x + if abs(y_pos) <= size and abs(x_pos) <= size and is_toggled: + vel_msg.linear.x = max_speed_mps / size * y_pos + vel_msg.angular.z = max_speed_mps / size * x_pos + else: + vel_msg.linear.x = 0 + vel_msg.angular.z = 0 + + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.y = 0 + vel_msg.angular.x = 0 + + publisher.publish(vel_msg) + + +def on_leave(box, event): + global vel_msg + vel_msg.linear.x = 0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 0 + publisher.publish(vel_msg) + +def on_click(box, event): + global is_toggled + is_toggled = True + on_move(box, event) + +def on_release(box, event): + global is_toggled, vel_msg + is_toggled = False + vel_msg.linear.x = 0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 0 + publisher.publish(vel_msg) + + + +if __name__ == '__main__': + window = Gtk.Window() + box = Gtk.EventBox() + window.set_default_size(size * 2, size * 2) + box.connect('motion-notify-event', on_move) + box.connect('leave-notify-event', on_leave) + box.connect('button-press-event', on_click) + box.connect('button-release-event', on_release) + + box.add_events(Gdk.EventMask.POINTER_MOTION_MASK) + window.add(box) + window.show_all() + print('\nClick and drag at any point in the window to move the robot, hit the x button at the top right to exit') + window.connect('destroy', Gtk.main_quit) + Gtk.main() From 26b48e3adc2d5ecfae4fffee1ee0bd73eb18b91f Mon Sep 17 00:00:00 2001 From: chosterto Date: Thu, 9 Mar 2023 13:35:02 -0500 Subject: [PATCH 2/4] Got cairo working --- ROS_WS/src/control/src/mouse_drive_control.py | 30 ++++++++++++------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/ROS_WS/src/control/src/mouse_drive_control.py b/ROS_WS/src/control/src/mouse_drive_control.py index 308976d..1fc43cd 100755 --- a/ROS_WS/src/control/src/mouse_drive_control.py +++ b/ROS_WS/src/control/src/mouse_drive_control.py @@ -11,16 +11,17 @@ is_toggled = False size = 250 -max_speed_mps = 0.5 +max_speed = 0.1 def on_move(box, event): + print(event.x, event.y) global vel_msg, is_toggled y_pos = size - event.y x_pos = size - event.x if abs(y_pos) <= size and abs(x_pos) <= size and is_toggled: - vel_msg.linear.x = max_speed_mps / size * y_pos - vel_msg.angular.z = max_speed_mps / size * x_pos + vel_msg.linear.x = max_speed / size * y_pos + vel_msg.angular.z = max_speed / size * x_pos else: vel_msg.linear.x = 0 vel_msg.angular.z = 0 @@ -43,11 +44,13 @@ def on_leave(box, event): vel_msg.angular.z = 0 publisher.publish(vel_msg) + def on_click(box, event): global is_toggled is_toggled = True on_move(box, event) + def on_release(box, event): global is_toggled, vel_msg is_toggled = False @@ -58,20 +61,27 @@ def on_release(box, event): vel_msg.angular.y = 0 vel_msg.angular.z = 0 publisher.publish(vel_msg) + +def on_draw(box, event): + cr = box.window.cairo_create() + cr.set_source_rgb(1.0, 0.0, 0.0) + cr.rectangle(50, 50, 100, 100) + cr.fill() if __name__ == '__main__': window = Gtk.Window() - box = Gtk.EventBox() + darea = Gtk.DrawingArea() window.set_default_size(size * 2, size * 2) - box.connect('motion-notify-event', on_move) - box.connect('leave-notify-event', on_leave) - box.connect('button-press-event', on_click) - box.connect('button-release-event', on_release) + darea.connect('motion-notify-event', on_move) + darea.connect('leave-notify-event', on_leave) + darea.connect('button-press-event', on_click) + darea.connect('button-release-event', on_release) + darea.connect('expose-event', on_draw) - box.add_events(Gdk.EventMask.POINTER_MOTION_MASK) - window.add(box) + darea.add_events(Gdk.EventMask.POINTER_MOTION_MASK) + window.add(darea) window.show_all() print('\nClick and drag at any point in the window to move the robot, hit the x button at the top right to exit') window.connect('destroy', Gtk.main_quit) From 22b834da88d058e28604a8bb2cbf8044f0281281 Mon Sep 17 00:00:00 2001 From: chosterto Date: Thu, 9 Mar 2023 19:29:42 -0500 Subject: [PATCH 3/4] Added very useful red cursor --- ROS_WS/src/control/src/mouse_drive_control.py | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/ROS_WS/src/control/src/mouse_drive_control.py b/ROS_WS/src/control/src/mouse_drive_control.py index 1fc43cd..594b70c 100755 --- a/ROS_WS/src/control/src/mouse_drive_control.py +++ b/ROS_WS/src/control/src/mouse_drive_control.py @@ -15,8 +15,16 @@ def on_move(box, event): - print(event.x, event.y) global vel_msg, is_toggled + box.window.invalidate_rect(None, False) + while Gtk.events_pending(): + Gtk.main_iteration() + cr = box.window.cairo_create() + cr.set_source_rgb(1.0, 0.0, 0.0) + cr.move_to(size, size) + cr.line_to(event.x, event.y) + cr.stroke() + y_pos = size - event.y x_pos = size - event.x if abs(y_pos) <= size and abs(x_pos) <= size and is_toggled: @@ -61,13 +69,6 @@ def on_release(box, event): vel_msg.angular.y = 0 vel_msg.angular.z = 0 publisher.publish(vel_msg) - - -def on_draw(box, event): - cr = box.window.cairo_create() - cr.set_source_rgb(1.0, 0.0, 0.0) - cr.rectangle(50, 50, 100, 100) - cr.fill() if __name__ == '__main__': @@ -78,9 +79,12 @@ def on_draw(box, event): darea.connect('leave-notify-event', on_leave) darea.connect('button-press-event', on_click) darea.connect('button-release-event', on_release) - darea.connect('expose-event', on_draw) - darea.add_events(Gdk.EventMask.POINTER_MOTION_MASK) + darea.add_events( + Gdk.EventMask.POINTER_MOTION_MASK | + Gdk.EventMask.BUTTON_RELEASE_MASK | + Gdk.EventMask.BUTTON_PRESS_MASK + ) window.add(darea) window.show_all() print('\nClick and drag at any point in the window to move the robot, hit the x button at the top right to exit') From f6f17626a054bfcbee473044d25430ef03da6a42 Mon Sep 17 00:00:00 2001 From: chosterto Date: Wed, 5 Apr 2023 22:34:43 -0400 Subject: [PATCH 4/4] gps simulation config --- ROS_WS/src/simulation/robot/rover.gazebo | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ROS_WS/src/simulation/robot/rover.gazebo b/ROS_WS/src/simulation/robot/rover.gazebo index ae01bd4..406d7b4 100644 --- a/ROS_WS/src/simulation/robot/rover.gazebo +++ b/ROS_WS/src/simulation/robot/rover.gazebo @@ -277,15 +277,14 @@ --> - 1 - gps_link - fix - fix_velocity - 0 0 0 - 0 0 0 - 0 0 0 - 0.0 - 0.0 + true + 1.0 + chassis + /fix + /fix_velocity + + 0.1 0.1 0.1 + 0 0 0 0.001 0.001 0.001