diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index fa08337..3298882 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -25,15 +25,18 @@ from ros_arduino_python.arduino_sensors import * from ros_arduino_msgs.srv import * from ros_arduino_python.base_controller import BaseController 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 import os, time import thread from math import radians +controller_types = { "follow_controller" : FollowController } + class ArduinoROS(): def __init__(self): - rospy.init_node('Arduino', log_level=rospy.DEBUG) + rospy.init_node('Arduino', log_level=rospy.INFO) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) @@ -137,7 +140,7 @@ class ArduinoROS(): rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) # Initialize any joints (servos) - self.joints = dict() + self.device.joints = dict() # Read in the joints (if any) joint_params = rospy.get_param("~joints", dict()) @@ -147,23 +150,42 @@ class ArduinoROS(): # Configure each servo 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 rospy.loginfo(name + " " + str(params)) # 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 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: self.have_joints = False - + # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.device, self.base_frame) + + print "==> ROS Arduino Bridge ready for action!" # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): @@ -180,7 +202,7 @@ class ArduinoROS(): if self.have_joints: mutex.acquire() self.servo_controller.poll() - self.joint_state_publisher.poll(self.joints.values()) + self.joint_state_publisher.poll(self.device.joints.values()) mutex.release() # Publish all sensor values on a single topic for convenience @@ -209,10 +231,10 @@ class ArduinoROS(): def SetServoSpeedWriteHandler(self, req): 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 - 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 self.device.config_servo(pin, step_delay) @@ -254,7 +276,7 @@ class ArduinoROS(): # Detach any servos if self.have_joints: rospy.loginfo("Detaching servos...") - for joint in self.joints.values(): + for joint in self.device.joints.values(): self.device.detach_servo(joint.pin) rospy.sleep(0.1) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index 98f941d..e77325a 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -23,6 +23,7 @@ import rospy from std_msgs.msg import Float64 from ros_arduino_msgs.srv import Relax, Enable, SetSpeed, SetSpeedResponse, RelaxResponse, EnableResponse +from controllers import * from math import radians, degrees @@ -30,6 +31,7 @@ class Joint: def __init__(self, device, name): self.device = device self.name = name + self.controller = None self.target_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. 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 - self.ms_per_deg = 1000.0 / self.max_deg_per_sec + # Conversion factors to compute servo speed to step delay between updates + 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 - 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 self.device.config_servo(self.pin, step_delay) @@ -70,7 +73,7 @@ class Servo(Joint): self.invert = rospy.get_param(n + "invert", False) # 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 self.position = 0.0 @@ -100,22 +103,22 @@ class Servo(Joint): # Set the target position for the next servo controller update 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 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.") step_delay = 0 else: # Catch division by zero and set to slowest speed possible 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: step_delay = 32767 - # Minimum step delay is 1 millisecond - step_delay = max(1, step_delay) + # Minimum step delay is 0 millisecond + step_delay = max(0, step_delay) return int(step_delay) @@ -139,7 +142,7 @@ class Servo(Joint): return EnableResponse() 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) # Update the servo speed @@ -147,15 +150,16 @@ class Servo(Joint): return SetSpeedResponse() -class ServoController(): - def __init__(self, device, joints, name): - self.device = device +class ServoController(Controller): + def __init__(self, device, name): + Controller.__init__(self, device, name) + self.servos = list() joint_update_rate = rospy.get_param("~joint_update_rate", 10.0) # Get the servo objects from the joint list - for servo in joints.values(): + for servo in self.device.joints.values(): self.servos.append(servo) servo.position_last = servo.get_current_position()