From 8796ec98ddd5fbe59a58cadded7aa8bd8dd9623e Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Wed, 5 May 2021 12:57:13 -0600 Subject: [PATCH 01/10] Updated Wheel encoders for Round Measured the wheel radius and wheel base and updated the wheel encoder node to match. Noetic displays a timestamp warning for every message that is not later than the last message recieved. This makes the repl unusable. Added a minimum delta time between publishing so avoid that problem. Still need to figure out the minimum delta. This issue of spamming warnings is under active discussion on the devs github. --- src/localisation/src/wheel_encoder.py | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index e06daf3..a593eea 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -17,6 +17,10 @@ class WheelEncoder: def __init__(self): self.name = rospy.get_param('rover_name', default='small_scout_1') + print("Subscribing to /{}/imu".format(self.name)) + print("Subscribing to /{}/joint_states".format(self.name)) + print("Publishing to /{}/odom".format(self.name)) + rospy.Subscriber("/{}/imu".format(self.name), Imu, self.imuCallback) rospy.Subscriber("/{}/joint_states".format(self.name), JointState, self.jointStatesCallback) self.odom_pub = rospy.Publisher("/{}/odom".format(self.name), Odometry, queue_size=50) @@ -29,8 +33,10 @@ def __init__(self): self.previous_front_right_wheel_angle = 0 self.previous_back_left_wheel_angle = 0 self.previous_back_right_wheel_angle = 0 - self.wheel_radius = 0.27 # meters - self.track_width = 1.75 # meters + #self.wheel_radius = 0.27 # meters + self.wheel_radius = 0.17 # meters + # self.track_width = 1.75 # meters + self.track_width = 1 # meters self.sample_rate = 1 self.message_count = 0 @@ -59,7 +65,11 @@ def jointStatesCallback(self, data): current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() - + + # Check that sufficient time has passed to avoid duplicate time stamps + if dt < 1: # Requre at least 1 second to have passed between function execution + return + ### Calculate angular velocity ### # Angular displacement in radians - could use all four wheels, start with just two @@ -117,12 +127,15 @@ def jointStatesCallback(self, data): #self.theta += v_wtheta # Composing your odometry message - + # The odometry message contains pose and twist information. The pose represents your current robot pose in the the odom frame (x, y, theta). # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.theta) + # Check if the timestamp will be different + + # first, we'll publish the transform over tf self.odom_broadcaster.sendTransform( (self.x, self.y, 0.), @@ -172,6 +185,8 @@ def shutdownHandler(): rospy.init_node('wheel_encoder', anonymous=True) + print("Wheel encoder node started") + # Register shutdown handler (includes ctrl-c handling) rospy.on_shutdown( shutdownHandler ) From 17a3a2cb6ffae23c17c608db78f2d803bfb3cdd8 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Fri, 21 May 2021 16:43:15 -0600 Subject: [PATCH 02/10] Handle Wheel pose in degrees and IMU offset For some reason the pose handler for the wheels is in degrees so had to add a conversion to radians (this must be new) The IMU's yaw is offset from the rover by 45 degrees Had to add a factor of 2pi. I can see no reason why this was needed but results in a usable wheel encoder. --- src/localisation/src/wheel_encoder.py | 54 ++++++++++++++++++--------- 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index a593eea..b71e854 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -2,6 +2,7 @@ # Generates odometry from wheel poses +import math import sys import tf.transformations as transform from sensor_msgs.msg import JointState, Imu @@ -38,6 +39,7 @@ def __init__(self): # self.track_width = 1.75 # meters self.track_width = 1 # meters self.sample_rate = 1 + self.msg_min_delta = 0.1 # seconds <--- Changing this from 0.1 spoils the accuracy. That should not be the case! self.message_count = 0 def imuCallback(self, data): @@ -47,7 +49,7 @@ def imuCallback(self, data): data.orientation.z, data.orientation.w) roll, pitch, yaw = transform.euler_from_quaternion(q) # in [-pi, pi] - self.theta = yaw + self.theta = yaw + 0.78 # rotate 45 degrees def jointStatesCallback(self, data): @@ -57,17 +59,18 @@ def jointStatesCallback(self, data): if self.message_count % self.sample_rate != 0: return - - back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ] - back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ] - front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ] - front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ] + + # Divide by 180 to get radians + back_left_wheel_angle = data.position[ data.name.index('bl_wheel_joint') ]*math.pi/180 + back_right_wheel_angle = data.position[ data.name.index('br_wheel_joint') ]*math.pi/180 + front_left_wheel_angle = data.position[ data.name.index('fl_wheel_joint') ]*math.pi/180 + front_right_wheel_angle = data.position[ data.name.index('fr_wheel_joint') ]*math.pi/180 current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() # Check that sufficient time has passed to avoid duplicate time stamps - if dt < 1: # Requre at least 1 second to have passed between function execution + if dt < self.msg_min_delta: # Specify the minimum time between messages for function execution (to prevent timestamp warning killing the repl) return ### Calculate angular velocity ### @@ -102,10 +105,16 @@ def jointStatesCallback(self, data): v_left = omega_left * self.wheel_radius v_right = omega_right * self.wheel_radius - v_rx = ( v_right + v_left ) /2 + print("Wheel Radius: ", self.wheel_radius) + + v_rx = ( v_right + v_left ) / 2.0 v_ry = 0 + + print("Velocity: ", v_rx, " m/s") + print("Yaw: ", self.theta) + # We increase the track width by a factor of 4 to match empirical tests - v_rtheta = ( v_right - v_left ) / self.track_width + v_rtheta = ( v_right - v_left ) / 2*self.track_width # Velocities and pose in the odom frame # The velocities expressed in the robot base frame can be transformed into the odom frame. @@ -113,15 +122,26 @@ def jointStatesCallback(self, data): # We are operating in the x-y plane, we ignore altitude - which will result in errors as the robot climbs up and down. # Integrate the velocities over time (multiply by the time differential dt and sum) - v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt - v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt - #v_wx = v_rx*cos(self.theta+v_rtheta/2) - #v_wy = v_rx*sin(self.theta+v_rtheta/2) + #v_wx = (v_rx * cos(self.theta) - v_ry * sin(self.theta)) * dt + #v_wy = (v_rx * sin(self.theta) + v_ry * cos(self.theta)) * dt - v_wtheta = v_rtheta * dt + # We know our heading from the imu - no need to calculate it from wheel velocities + v_wx = v_rx*cos(self.theta) + v_wy = v_rx*sin(self.theta) + + print("X displacement: ", v_wx, " m") + print("Y displacement: ", v_wy, " m") + print("Delta_t: ", dt, " s") + + print("Back Left Wheel Angle: ", back_left_wheel_angle) + + #v_wtheta = v_rtheta * dt - self.x += v_wx - self.y += v_wy + self.x += v_wx*2*math.pi # I don't understand why this 2pi factor is needed + self.y += v_wy*2*math.pi + + print("X coord: ", self.x) + print("Y coord: ", self.y) # Replaced with IMU theta #self.theta += v_wtheta @@ -176,7 +196,7 @@ def jointStatesCallback(self, data): self.previous_front_left_wheel_angle = front_left_wheel_angle self.previous_front_right_wheel_angle = front_right_wheel_angle self.previous_back_left_wheel_angle = back_left_wheel_angle - self.previous_back_right_wheel_angle = back_right_wheel_angle + self.previous_back_right_wheel_angle = back_right_wheel_angle def shutdownHandler(): print("Wheel encoder shutting down.") From 0011f66dbce66a28dff37eb80c72b9033a5667ff Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Fri, 25 Jun 2021 16:41:49 -0600 Subject: [PATCH 03/10] Added EKF support --- src/localisation/launch/wheel_encoder.xml | 2 +- src/localisation/src/wheel_encoder.py | 34 +++++++++++------------ src/scoot/launch/scoot.launch | 9 +++--- src/scoot/launch/scoot.xml | 9 +++++- 4 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/localisation/launch/wheel_encoder.xml b/src/localisation/launch/wheel_encoder.xml index f45a60e..84f0216 100644 --- a/src/localisation/launch/wheel_encoder.xml +++ b/src/localisation/launch/wheel_encoder.xml @@ -1,5 +1,5 @@ - + diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index b71e854..c67f8cd 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -39,7 +39,7 @@ def __init__(self): # self.track_width = 1.75 # meters self.track_width = 1 # meters self.sample_rate = 1 - self.msg_min_delta = 0.1 # seconds <--- Changing this from 0.1 spoils the accuracy. That should not be the case! + self.msg_min_delta = 0.5 # seconds <--- Changing this from 0.1 spoils the accuracy. That should not be the case! self.message_count = 0 def imuCallback(self, data): @@ -157,22 +157,22 @@ def jointStatesCallback(self, data): # first, we'll publish the transform over tf - self.odom_broadcaster.sendTransform( - (self.x, self.y, 0.), - odom_quat, - current_time, - "{}_tf/base_footprint".format(self.name), - "odom" - ) - + #self.odom_broadcaster.sendTransform( + # (self.x, self.y, 0.), + # odom_quat, + # current_time, + # "{}_/base_footprint".format(self.name), + # "odom" + #) + # And link up the map - self.odom_broadcaster.sendTransform( - (0, 0, 0), - tf.transformations.quaternion_from_euler(0, 0, 0), - current_time, - "odom", - "map" - ) + #self.odom_broadcaster.sendTransform( + # (0, 0, 0), + # tf.transformations.quaternion_from_euler(0, 0, 0), + # current_time, + # "odom", + # "map" + #) # next, we'll publish the odometry message over ROS odom = Odometry() @@ -183,7 +183,7 @@ def jointStatesCallback(self, data): odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*odom_quat)) # set the velocity - odom.child_frame_id = "{}_tf/base_footprint".format(self.name) + odom.child_frame_id = "{}_base_footprint".format(self.name) odom.twist.twist = Twist(Vector3(v_rx, v_ry, 0), Vector3(0, 0, v_rtheta)) # publish the message diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch index 1d4486b..463edba 100644 --- a/src/scoot/launch/scoot.launch +++ b/src/scoot/launch/scoot.launch @@ -50,23 +50,23 @@ - diff --git a/src/scoot/launch/scoot.xml b/src/scoot/launch/scoot.xml index 3fd04f7..3c08cac 100644 --- a/src/scoot/launch/scoot.xml +++ b/src/scoot/launch/scoot.xml @@ -33,8 +33,15 @@ - + + + + + + + + From f31b6fb5fd3a1ead095bac0f239cab1fb0c938c9 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Fri, 9 Jul 2021 15:18:59 -0600 Subject: [PATCH 04/10] changed print statements to rospy loginfo --- src/localisation/src/wheel_encoder.py | 28 +++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index c67f8cd..cba45fb 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -18,9 +18,9 @@ class WheelEncoder: def __init__(self): self.name = rospy.get_param('rover_name', default='small_scout_1') - print("Subscribing to /{}/imu".format(self.name)) - print("Subscribing to /{}/joint_states".format(self.name)) - print("Publishing to /{}/odom".format(self.name)) + rospy.loginfo("Subscribing to /{}/imu".format(self.name)) + rospy.loginfo("Subscribing to /{}/joint_states".format(self.name)) + rospy.loginfo("Publishing to /{}/odom".format(self.name)) rospy.Subscriber("/{}/imu".format(self.name), Imu, self.imuCallback) rospy.Subscriber("/{}/joint_states".format(self.name), JointState, self.jointStatesCallback) @@ -105,13 +105,13 @@ def jointStatesCallback(self, data): v_left = omega_left * self.wheel_radius v_right = omega_right * self.wheel_radius - print("Wheel Radius: ", self.wheel_radius) + rospy.loginfo("Wheel Radius: ", self.wheel_radius) v_rx = ( v_right + v_left ) / 2.0 v_ry = 0 - print("Velocity: ", v_rx, " m/s") - print("Yaw: ", self.theta) + rospy.loginfo("Velocity: ", v_rx, " m/s") + rospy.loginfo("Yaw: ", self.theta) # We increase the track width by a factor of 4 to match empirical tests v_rtheta = ( v_right - v_left ) / 2*self.track_width @@ -129,19 +129,19 @@ def jointStatesCallback(self, data): v_wx = v_rx*cos(self.theta) v_wy = v_rx*sin(self.theta) - print("X displacement: ", v_wx, " m") - print("Y displacement: ", v_wy, " m") - print("Delta_t: ", dt, " s") + rospy.loginfo("X displacement: ", v_wx, " m") + rospy.loginfo("Y displacement: ", v_wy, " m") + rospy.loginfo("Delta_t: ", dt, " s") - print("Back Left Wheel Angle: ", back_left_wheel_angle) + rospy.loginfo("Back Left Wheel Angle: ", back_left_wheel_angle) #v_wtheta = v_rtheta * dt self.x += v_wx*2*math.pi # I don't understand why this 2pi factor is needed self.y += v_wy*2*math.pi - print("X coord: ", self.x) - print("Y coord: ", self.y) + rospy.loginfo("X coord: ", self.x) + rospy.loginfo("Y coord: ", self.y) # Replaced with IMU theta #self.theta += v_wtheta @@ -199,13 +199,13 @@ def jointStatesCallback(self, data): self.previous_back_right_wheel_angle = back_right_wheel_angle def shutdownHandler(): - print("Wheel encoder shutting down.") + rospy.loginfo("Wheel encoder shutting down.") if __name__ == '__main__': rospy.init_node('wheel_encoder', anonymous=True) - print("Wheel encoder node started") + rospy.loginfo("Wheel encoder node started") # Register shutdown handler (includes ctrl-c handling) rospy.on_shutdown( shutdownHandler ) From da8cbaa5afd773ab8acf7572ab6c3f648e70f887 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Fri, 9 Jul 2021 15:47:33 -0600 Subject: [PATCH 05/10] changed ekf to odom, fake odom to odom/fake, and wheel encoder to odom/wheel --- src/localisation/src/wheel_encoder.py | 4 ++-- src/navigation/src/bug_nav.py | 6 +++--- src/object_detection/src/Cubesat_Detection.py | 2 +- src/object_detection/src/Leg_Detection.py | 2 +- src/object_detection/src/Logo_Detection.py | 2 +- src/object_detection/src/Volatile_Detection.py | 2 +- src/scoot/launch/scoot.launch | 4 ++-- src/scoot/src/Driver.py | 2 +- src/scoot/src/FakeLocalization.py | 2 +- src/scoot/src/Scoot.py | 2 +- src/scripts/src/deltaDelta.py | 6 +++--- 11 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index cba45fb..c4d7ba9 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -20,11 +20,11 @@ def __init__(self): rospy.loginfo("Subscribing to /{}/imu".format(self.name)) rospy.loginfo("Subscribing to /{}/joint_states".format(self.name)) - rospy.loginfo("Publishing to /{}/odom".format(self.name)) + rospy.loginfo("Publishing to /{}/odom/wheel".format(self.name)) rospy.Subscriber("/{}/imu".format(self.name), Imu, self.imuCallback) rospy.Subscriber("/{}/joint_states".format(self.name), JointState, self.jointStatesCallback) - self.odom_pub = rospy.Publisher("/{}/odom".format(self.name), Odometry, queue_size=50) + self.odom_pub = rospy.Publisher("/{}/odom/wheel".format(self.name), Odometry, queue_size=50) self.odom_broadcaster = tf.TransformBroadcaster() self.last_time = rospy.Time.now() self.x = 0 diff --git a/src/navigation/src/bug_nav.py b/src/navigation/src/bug_nav.py index 4fdbf1b..77b23f5 100755 --- a/src/navigation/src/bug_nav.py +++ b/src/navigation/src/bug_nav.py @@ -234,7 +234,7 @@ def at( self ): def init_listener(): - rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, estimated_location_handler) + rospy.Subscriber('/small_scout_1/odom', Odometry, estimated_location_handler) rospy.Subscriber('/small_scout_1/laser/scan', LaserScan, lidar_handler) rospy.Subscriber("/small_scout_1/imu", Imu, imu_handler) rospy.Subscriber('/small_scout_1/obstacle', Obstacles, obstacle_handler) @@ -666,8 +666,8 @@ def bug_algorithm(tx, ty, bug_type): global stats_printed global total_time_start - print "Waiting for location data on '/small_scout_1/odom/filtered...'" - rospy.wait_for_message('/small_scout_1/odom/filtered', Odometry,) + print "Waiting for location data on '/small_scout_1/odom...'" + rospy.wait_for_message('/small_scout_1/odom', Odometry,) print "... received." print("Waiting for break service...") diff --git a/src/object_detection/src/Cubesat_Detection.py b/src/object_detection/src/Cubesat_Detection.py index 8961d61..b500213 100755 --- a/src/object_detection/src/Cubesat_Detection.py +++ b/src/object_detection/src/Cubesat_Detection.py @@ -35,7 +35,7 @@ def __init__(self): self.bridge = CvBridge() self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback) - # self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback) + # self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback) self.left_camera_subscriber = rospy.Subscriber('/small_scout_1/camera/left/image_raw', Image, self.cam_callback) self.cubesat_detection_image_left_publisher = rospy.Publisher('/small_scout_1/cubesat_detections/image/left', Image, queue_size=100) diff --git a/src/object_detection/src/Leg_Detection.py b/src/object_detection/src/Leg_Detection.py index d29d73a..bdec539 100644 --- a/src/object_detection/src/Leg_Detection.py +++ b/src/object_detection/src/Leg_Detection.py @@ -34,7 +34,7 @@ def __init__(self): self.bridge = CvBridge() self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback) - self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback) + self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback) self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image) self.leg_detection_image_left_publisher = rospy.Publisher('/small_scout_1/leg_detections/image/left', Image, queue_size=100) diff --git a/src/object_detection/src/Logo_Detection.py b/src/object_detection/src/Logo_Detection.py index fa28721..fbc2f90 100755 --- a/src/object_detection/src/Logo_Detection.py +++ b/src/object_detection/src/Logo_Detection.py @@ -33,7 +33,7 @@ def __init__(self): self.bridge = CvBridge() self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback) - self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback) + self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback) self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image) self.logo_detection_image_left_publisher = rospy.Publisher('/small_scout_1/logo_detections/image/left', Image, queue_size=100) diff --git a/src/object_detection/src/Volatile_Detection.py b/src/object_detection/src/Volatile_Detection.py index 8f115ff..c8f8939 100755 --- a/src/object_detection/src/Volatile_Detection.py +++ b/src/object_detection/src/Volatile_Detection.py @@ -32,7 +32,7 @@ def __init__(self): self.bridge = CvBridge() self.point_cloud_subscriber = rospy.Subscriber('/small_scout_1/points2', PointCloud2, self.pc_callback) - self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, self.odom_callback) + self.scoot_odom_subscriber = rospy.Subscriber('/small_scout_1/odom', Odometry, self.odom_callback) self.left_camera_subscriber = message_filters.Subscriber('/small_scout_1/camera/left/image_raw', Image) self.volatile_detection_image_left_publisher = rospy.Publisher('/small_scout_1/volatile_detections/image/left', Image, queue_size=100) diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch index 463edba..fa426d8 100644 --- a/src/scoot/launch/scoot.launch +++ b/src/scoot/launch/scoot.launch @@ -59,7 +59,7 @@ - + [false, false, false, false, false, true, true, false, false, @@ -105,7 +105,7 @@ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1] - + diff --git a/src/scoot/src/Driver.py b/src/scoot/src/Driver.py index f4f2ffb..4a04660 100755 --- a/src/scoot/src/Driver.py +++ b/src/scoot/src/Driver.py @@ -106,7 +106,7 @@ def __init__(self): # Subscribers rospy.Subscriber('/' + self.rover_name + '/obstacle', Obstacles, self._obstacle) rospy.Subscriber('/' + self.rover_name + '/detections', Detection, self._vision) - rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom) + rospy.Subscriber('/' + self.rover_name + '/odom', Odometry, self._odom) # Services self.control = rospy.Service('control', Core, self._control) diff --git a/src/scoot/src/FakeLocalization.py b/src/scoot/src/FakeLocalization.py index 33a018f..9191f86 100755 --- a/src/scoot/src/FakeLocalization.py +++ b/src/scoot/src/FakeLocalization.py @@ -7,7 +7,7 @@ def publish_fake_odom(): rover_name = rospy.get_param('rover_name', default='small_scout_1') - pub = rospy.Publisher('/' + rover_name + '/odometry/filtered', Odometry, queue_size=10) + pub = rospy.Publisher('/' + rover_name + '/odom/fake', Odometry, queue_size=10) rospy.init_node('odom', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): diff --git a/src/scoot/src/Scoot.py b/src/scoot/src/Scoot.py index 1bd4bdc..4087390 100755 --- a/src/scoot/src/Scoot.py +++ b/src/scoot/src/Scoot.py @@ -249,7 +249,7 @@ def start(self, **kwargs): rospy.loginfo("Done waiting for rover specific services") # Subscribe to topics. - rospy.Subscriber('/' + self.rover_name + '/odometry/filtered', Odometry, self._odom) + rospy.Subscriber('/' + self.rover_name + '/odom', Odometry, self._odom) rospy.Subscriber('/' + self.rover_name + '/joint_states', JointState, self._joint_states) rospy.Subscriber('/srcp2/score', msg.ScoreMsg, self._score) # Transform listener. Use this to transform between coordinate spaces. diff --git a/src/scripts/src/deltaDelta.py b/src/scripts/src/deltaDelta.py index 22395ca..71c193b 100755 --- a/src/scripts/src/deltaDelta.py +++ b/src/scripts/src/deltaDelta.py @@ -26,9 +26,9 @@ def run(): global wheel global fake global ekf - rospy.Subscriber('/small_scout_1/odom/', Odometry, _wheel) - rospy.Subscriber('/small_scout_1/odom/filtered', Odometry, _fake) - rospy.Subscriber('/small_scout_1/odometry/filtered', Odometry, _ekf) + rospy.Subscriber('/small_scout_1/odom/wheel', Odometry, _wheel) + rospy.Subscriber('/small_scout_1/odom/fake', Odometry, _fake) + rospy.Subscriber('/small_scout_1/odom', Odometry, _ekf) rospy.init_node("odom_dif_test") rospy.sleep(0.5) delta_f_e = dist(fake, ekf) From d590f2e0b9691955eb1a4a862fd4cb9dbb74d4af Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Fri, 9 Jul 2021 15:53:33 -0600 Subject: [PATCH 06/10] fixed rospy loginfo errors --- src/localisation/src/wheel_encoder.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/localisation/src/wheel_encoder.py b/src/localisation/src/wheel_encoder.py index c4d7ba9..5eb166a 100755 --- a/src/localisation/src/wheel_encoder.py +++ b/src/localisation/src/wheel_encoder.py @@ -105,13 +105,13 @@ def jointStatesCallback(self, data): v_left = omega_left * self.wheel_radius v_right = omega_right * self.wheel_radius - rospy.loginfo("Wheel Radius: ", self.wheel_radius) + rospy.loginfo("Wheel Radius: " + str(self.wheel_radius)) v_rx = ( v_right + v_left ) / 2.0 v_ry = 0 - rospy.loginfo("Velocity: ", v_rx, " m/s") - rospy.loginfo("Yaw: ", self.theta) + rospy.loginfo("Velocity: " + str(v_rx) + " m/s") + rospy.loginfo("Yaw: " + str(self.theta)) # We increase the track width by a factor of 4 to match empirical tests v_rtheta = ( v_right - v_left ) / 2*self.track_width @@ -129,19 +129,19 @@ def jointStatesCallback(self, data): v_wx = v_rx*cos(self.theta) v_wy = v_rx*sin(self.theta) - rospy.loginfo("X displacement: ", v_wx, " m") - rospy.loginfo("Y displacement: ", v_wy, " m") - rospy.loginfo("Delta_t: ", dt, " s") + rospy.loginfo("X displacement: " + str(v_wx) + " m") + rospy.loginfo("Y displacement: " + str(v_wy) + " m") + rospy.loginfo("Delta_t: " + str(dt) + " s") - rospy.loginfo("Back Left Wheel Angle: ", back_left_wheel_angle) + rospy.loginfo("Back Left Wheel Angle: " + str(back_left_wheel_angle)) #v_wtheta = v_rtheta * dt self.x += v_wx*2*math.pi # I don't understand why this 2pi factor is needed self.y += v_wy*2*math.pi - rospy.loginfo("X coord: ", self.x) - rospy.loginfo("Y coord: ", self.y) + rospy.loginfo("X coord: " + str(self.x)) + rospy.loginfo("Y coord: " + str(self.y)) # Replaced with IMU theta #self.theta += v_wtheta From 639db5bcf9ba26b30bccf87d382a3fea425f73f4 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Fri, 9 Jul 2021 16:20:14 -0600 Subject: [PATCH 07/10] fixed odometry remapping --- src/scoot/launch/scoot.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch index fa426d8..3ffc5a9 100644 --- a/src/scoot/launch/scoot.launch +++ b/src/scoot/launch/scoot.launch @@ -105,7 +105,7 @@ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1] - + From d0eabbdc64c4a1ba007824453f2d471bcfa9c6a8 Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Fri, 9 Jul 2021 16:48:01 -0600 Subject: [PATCH 08/10] wheel encoder is launched by launch file --- src/localisation/launch/wheel_encoder.xml | 6 +++--- src/scoot/launch/scoot.launch | 2 -- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/localisation/launch/wheel_encoder.xml b/src/localisation/launch/wheel_encoder.xml index 84f0216..a29235d 100644 --- a/src/localisation/launch/wheel_encoder.xml +++ b/src/localisation/launch/wheel_encoder.xml @@ -1,6 +1,6 @@ - + - - + + diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch index 3ffc5a9..6ace4a9 100644 --- a/src/scoot/launch/scoot.launch +++ b/src/scoot/launch/scoot.launch @@ -18,11 +18,9 @@ - From 948c4254b822d57d97694cebe18b927c1fe819ec Mon Sep 17 00:00:00 2001 From: Abby Pribisova Date: Sun, 11 Jul 2021 20:12:14 -0600 Subject: [PATCH 09/10] moved drive controller into group namespace --- src/scoot/launch/scoot.launch | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/scoot/launch/scoot.launch b/src/scoot/launch/scoot.launch index 4ee298f..c4adaa2 100644 --- a/src/scoot/launch/scoot.launch +++ b/src/scoot/launch/scoot.launch @@ -44,11 +44,10 @@ + + - - - From 24d2720248df8a00efa20a2f070182bd6b1d3d4f Mon Sep 17 00:00:00 2001 From: Carter Frost Date: Fri, 16 Jul 2021 01:15:25 -0700 Subject: [PATCH 10/10] src/scripts/src/deltaDelta.py: fixed prints for python3 --- src/scripts/src/deltaDelta.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/scripts/src/deltaDelta.py b/src/scripts/src/deltaDelta.py index 71c193b..b0333b3 100755 --- a/src/scripts/src/deltaDelta.py +++ b/src/scripts/src/deltaDelta.py @@ -36,9 +36,9 @@ def run(): delta_f_w = dist(fake, wheel) while not rospy.is_shutdown(): rospy.sleep(0.3) - print "delta_f_e:", delta_f_e - dist(fake, ekf) - print "delta_w_e:", delta_w_e - dist(wheel, ekf) - print "delta_f_w:", delta_f_w - dist(fake, wheel) + print("delta_f_e:", delta_f_e - dist(fake, ekf)) + print("delta_w_e:", delta_w_e - dist(wheel, ekf)) + print("delta_f_w:", delta_f_w - dist(fake, wheel)) if __name__ == '__main__': run()