yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Added reset_odometry service to base controller

Browse Source
This commit is contained in:
Patrick Goebel 2016-03-27 20:14:34 -07:00
parent 6441cb5abe
commit afad6f10d9
2 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ros_arduino_python/nodes/arduino_node.py
View File

@ -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.follow_controller import FollowController
from ros_arduino_python.joint_state_publisher import JointStatePublisher from ros_arduino_python.joint_state_publisher import JointStatePublisher
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from std_srvs.srv import Empty, EmptyResponse
import os, time import os, time
import thread import thread
from math import radians from math import radians
@ -107,6 +108,9 @@ class ArduinoROS():
# A service to read the value of an analog sensor # A service to read the value of an analog sensor
rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) 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 # Initialize the device
self.device = Arduino(self.port, self.baud, self.timeout) self.device = Arduino(self.port, self.baud, self.timeout)
@ -304,6 +308,13 @@ class ArduinoROS():
def AnalogReadHandler(self, req): def AnalogReadHandler(self, req):
value = self.device.analog_read(req.pin) value = self.device.analog_read(req.pin)
return AnalogReadResponse(value) 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): def shutdown(self):
rospy.loginfo("Shutting down Arduino node...") rospy.loginfo("Shutting down Arduino node...")

11
ros_arduino_python/src/ros_arduino_python/base_controller.py
View File

@ -232,7 +232,7 @@ class BaseController:
self.arduino.drive(self.v_left, self.v_right) self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta self.t_next = now + self.t_delta
def stop(self): def stop(self):
self.stopped = True self.stopped = True
self.arduino.drive(0, 0) 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_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) 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