From d8d32f135d6d4977e3dd4380205a8423c9a7a997 Mon Sep 17 00:00:00 2001 From: Achllle Date: Sat, 8 May 2021 18:22:30 +0000 Subject: [PATCH] Fix #134 for ROS1, add warning --- ROS/osr/src/rover.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROS/osr/src/rover.py b/ROS/osr/src/rover.py index 6253723..57678c0 100755 --- a/ROS/osr/src/rover.py +++ b/ROS/osr/src/rover.py @@ -324,7 +324,12 @@ def forward_kinematics(self): drive_angular_velocity = (self.curr_velocities['drive_left_middle'] + self.curr_velocities['drive_right_middle']) / 2. self.curr_twist.twist.linear.x = drive_angular_velocity * self.wheel_radius # now calculate angular velocity from its relation with linear velocity and turning radius - self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius + try: + self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius + except ZeroDivisionError: + rospy.logwarn_throttle(1, "Current turning radius was calculated as zero which" + "is an illegal value. Check your wheel calibration." + self.curr_twist.twist.angular.z = 0. # turning in place is currently unsupported # covariance self.curr_twist.covariance = 36 * [0.0,]