Added reset_odometry service to base controller

ros_arduino_python/nodes/arduino_node.py

@@ -28,6 +28,7 @@ from ros_arduino_python.servo_controller import Servo, ServoController
 #from ros_arduino_python.follow_controller import FollowController
 from ros_arduino_python.joint_state_publisher import JointStatePublisher
 from geometry_msgs.msg import Twist
+from std_srvs.srv import Empty, EmptyResponse
 import os, time
 import thread
 from math import radians
@@ -107,6 +108,9 @@ class ArduinoROS():
         # A service to read the value of an analog sensor
         rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
 
+        # A service to reset the odometry values to 0
+        rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)
+
         # Initialize the device
         self.device = Arduino(self.port, self.baud, self.timeout)
         
@@ -304,6 +308,13 @@ class ArduinoROS():
     def AnalogReadHandler(self, req):
         value = self.device.analog_read(req.pin)
         return AnalogReadResponse(value)
+
+    def ResetOdometryHandler(self, req):
+        if self.use_base_controller:
+            self.myBaseController.reset_odometry()
+        else:
+            rospy.logwarn("Not using base controller!")
+        return EmptyResponse()
         
     def shutdown(self):
         rospy.loginfo("Shutting down Arduino node...")

ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -232,7 +232,7 @@ class BaseController:
                 self.arduino.drive(self.v_left, self.v_right)
                 
             self.t_next = now + self.t_delta
-            
+
     def stop(self):
         self.stopped = True
         self.arduino.drive(0, 0)
@@ -259,9 +259,8 @@ class BaseController:
         self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
         self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
         
-
+    def reset_odometry(self):
+        self.x = 0.0
+        self.y = 0.0
+        self.th = 0.0
         
-
-    
-
-