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

Cleaned up object constructors and fixed set_speed calculation

Browse Source
This commit is contained in:
Patrick Goebel 2015-12-13 16:35:26 -08:00
parent ab0f16a6ff
commit aefe39bf41
2 changed files with 49 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ros_arduino_python/nodes/arduino_node.py
View File

@ -25,15 +25,18 @@ from ros_arduino_python.arduino_sensors import *
from ros_arduino_msgs.srv import * from ros_arduino_msgs.srv import *
from ros_arduino_python.base_controller import BaseController from ros_arduino_python.base_controller import BaseController
from ros_arduino_python.servo_controller import Servo, ServoController 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 ros_arduino_python.joint_state_publisher import JointStatePublisher
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
import os, time import os, time
import thread import thread
from math import radians from math import radians
controller_types = { "follow_controller" : FollowController }
class ArduinoROS(): class ArduinoROS():
def __init__(self): def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG) rospy.init_node('Arduino', log_level=rospy.INFO)
# Cleanup when termniating the node # Cleanup when termniating the node
rospy.on_shutdown(self.shutdown) rospy.on_shutdown(self.shutdown)
@ -137,7 +140,7 @@ class ArduinoROS():
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
# Initialize any joints (servos) # Initialize any joints (servos)
self.joints = dict() self.device.joints = dict()
# Read in the joints (if any) # Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict()) joint_params = rospy.get_param("~joints", dict())
@ -147,23 +150,42 @@ class ArduinoROS():
# Configure each servo # Configure each servo
for name, params in joint_params.iteritems(): for name, params in joint_params.iteritems():
self.joints[name] = Servo(self.device, name) self.device.joints[name] = Servo(self.device, name)
# Display the joint setup on the terminal # Display the joint setup on the terminal
rospy.loginfo(name + " " + str(params)) rospy.loginfo(name + " " + str(params))
# The servo controller determines when to read and write position values to the servos # The servo controller determines when to read and write position values to the servos
self.servo_controller = ServoController(self.device, self.joints, "ServoController") self.servo_controller = ServoController(self.device, "ServoController")
# The joint state publisher publishes the latest joint values on the /joint_states topic # The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher() self.joint_state_publisher = JointStatePublisher()
# # Initialize any trajectory action follow controllers
# controllers = rospy.get_param("~controllers", dict())
#
# self.device.controllers = list()
#
# for name, params in controllers.items():
# try:
# controller = controller_types[params["type"]](self.device, name)
# self.device.controllers.append(controller)
# except Exception as e:
# if type(e) == KeyError:
# rospy.logerr("Unrecognized controller: " + params["type"])
# else:
# rospy.logerr(str(type(e)) + str(e))
#
# for controller in self.device.controllers:
# controller.startup()
else: else:
self.have_joints = False self.have_joints = False
# Initialize the base controller if used # Initialize the base controller if used
if self.use_base_controller: if self.use_base_controller:
self.myBaseController = BaseController(self.device, self.base_frame) self.myBaseController = BaseController(self.device, self.base_frame)
print "==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller # Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown(): while not rospy.is_shutdown():
@ -180,7 +202,7 @@ class ArduinoROS():
if self.have_joints: if self.have_joints:
mutex.acquire() mutex.acquire()
self.servo_controller.poll() self.servo_controller.poll()
self.joint_state_publisher.poll(self.joints.values()) self.joint_state_publisher.poll(self.device.joints.values())
mutex.release() mutex.release()
# Publish all sensor values on a single topic for convenience # Publish all sensor values on a single topic for convenience
@ -209,10 +231,10 @@ class ArduinoROS():
def SetServoSpeedWriteHandler(self, req): def SetServoSpeedWriteHandler(self, req):
index = self.joint.values['pin'].index(req.pin) index = self.joint.values['pin'].index(req.pin)
name = self.joints.keys[index] name = self.device.joints.keys[index]
# Convert servo speed in deg/s to a step delay in milliseconds # Convert servo speed in deg/s to a step delay in milliseconds
step_delay = self.joints[name].get_step_delay(req.value) step_delay = self.device.joints[name].get_step_delay(req.value)
# Update the servo speed # Update the servo speed
self.device.config_servo(pin, step_delay) self.device.config_servo(pin, step_delay)
@ -254,7 +276,7 @@ class ArduinoROS():
# Detach any servos # Detach any servos
if self.have_joints: if self.have_joints:
rospy.loginfo("Detaching servos...") rospy.loginfo("Detaching servos...")
for joint in self.joints.values(): for joint in self.device.joints.values():
self.device.detach_servo(joint.pin) self.device.detach_servo(joint.pin)
rospy.sleep(0.1) rospy.sleep(0.1)

32
ros_arduino_python/src/ros_arduino_python/servo_controller.py
View File

@ -23,6 +23,7 @@
import rospy import rospy
from std_msgs.msg import Float64 from std_msgs.msg import Float64
from ros_arduino_msgs.srv import Relax, Enable, SetSpeed, SetSpeedResponse, RelaxResponse, EnableResponse from ros_arduino_msgs.srv import Relax, Enable, SetSpeed, SetSpeedResponse, RelaxResponse, EnableResponse
from controllers import *
from math import radians, degrees from math import radians, degrees
@ -30,6 +31,7 @@ class Joint:
def __init__(self, device, name): def __init__(self, device, name):
self.device = device self.device = device
self.name = name self.name = name
self.controller = None
self.target_position = 0.0 # radians self.target_position = 0.0 # radians
self.position = 0.0 # radians self.position = 0.0 # radians
@ -50,11 +52,12 @@ class Servo(Joint):
# A value of 0.24 seconds per 60 degrees is typical. # A value of 0.24 seconds per 60 degrees is typical.
self.rated_speed = rospy.get_param(n + "rated_speed", 0.24) # seconds per 60 degrees self.rated_speed = rospy.get_param(n + "rated_speed", 0.24) # seconds per 60 degrees
self.max_deg_per_sec = 60.0 / self.rated_speed # Conversion factors to compute servo speed to step delay between updates
self.ms_per_deg = 1000.0 / self.max_deg_per_sec self.max_rad_per_sec = radians(60.0) / self.rated_speed
self.ms_per_rad = 1000.0 / self.max_rad_per_sec
# Convert initial servo speed in deg/s to a step delay in milliseconds # Convert initial servo speed in deg/s to a step delay in milliseconds
step_delay = self.get_step_delay(rospy.get_param(n + "init_speed", 90.0)) step_delay = self.get_step_delay(radians(rospy.get_param(n + "init_speed", 90.0)))
# Update the servo speed # Update the servo speed
self.device.config_servo(self.pin, step_delay) self.device.config_servo(self.pin, step_delay)
@ -70,7 +73,7 @@ class Servo(Joint):
self.invert = rospy.get_param(n + "invert", False) self.invert = rospy.get_param(n + "invert", False)
# Where to we want the servo positioned # Where to we want the servo positioned
self.desired = self.neutral - radians(rospy.get_param(n + "init_position", 0)) self.desired = self.neutral + radians(rospy.get_param(n + "init_position", 0))
# Where is the servo positioned now # Where is the servo positioned now
self.position = 0.0 self.position = 0.0
@ -100,22 +103,22 @@ class Servo(Joint):
# Set the target position for the next servo controller update # Set the target position for the next servo controller update
self.desired = target_adjusted self.desired = target_adjusted
def get_step_delay(self, target_speed=90): def get_step_delay(self, target_speed=1.0):
# Don't allow negative speeds # Don't allow negative speeds
target_speed = abs(target_speed) target_speed = abs(target_speed)
if target_speed > self.max_deg_per_sec: if target_speed > self.max_rad_per_sec:
rospy.logdebug("Target speed exceeds max servo speed. Using max.") rospy.logdebug("Target speed exceeds max servo speed. Using max.")
step_delay = 0 step_delay = 0
else: else:
# Catch division by zero and set to slowest speed possible # Catch division by zero and set to slowest speed possible
try: try:
step_delay = self.ms_per_deg * (self.max_deg_per_sec / target_speed - 1) step_delay = 1000.0 / degrees(1.0) * (1.0 / target_speed - 1.0 / self.max_rad_per_sec)
except: except:
step_delay = 32767 step_delay = 32767
# Minimum step delay is 1 millisecond # Minimum step delay is 0 millisecond
step_delay = max(1, step_delay) step_delay = max(0, step_delay)
return int(step_delay) return int(step_delay)
@ -139,7 +142,7 @@ class Servo(Joint):
return EnableResponse() return EnableResponse()
def set_speed_cb(self, req): def set_speed_cb(self, req):
# Convert servo speed in deg/s to a step delay in milliseconds # Convert servo speed in rad/s to a step delay in milliseconds
step_delay = self.get_step_delay(req.value) step_delay = self.get_step_delay(req.value)
# Update the servo speed # Update the servo speed
@ -147,15 +150,16 @@ class Servo(Joint):
return SetSpeedResponse() return SetSpeedResponse()
class ServoController(): class ServoController(Controller):
def __init__(self, device, joints, name): def __init__(self, device, name):
self.device = device Controller.__init__(self, device, name)
self.servos = list() self.servos = list()
joint_update_rate = rospy.get_param("~joint_update_rate", 10.0) joint_update_rate = rospy.get_param("~joint_update_rate", 10.0)
# Get the servo objects from the joint list # Get the servo objects from the joint list
for servo in joints.values(): for servo in self.device.joints.values():
self.servos.append(servo) self.servos.append(servo)
servo.position_last = servo.get_current_position() servo.position_last = servo.get_current_position()