Added UpdatePID service and dynamic reconfigure support

ros_arduino_msgs/CMakeLists.txt

@@ -35,6 +35,7 @@ add_service_files(FILES
                   ServoWrite.srv
                   SetSpeed.srv
                   SetServoSpeed.srv
+                  UpdatePID.srv
                  )
 
 generate_messages(   

ros_arduino_msgs/srv/UpdatePID.srv

@@ -0,0 +1,5 @@
+float32 Kp
+float32 Kd
+float32 Ki
+float32 Ko
+---

ros_arduino_python/CMakeLists.txt

@@ -1,10 +1,16 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(ros_arduino_python)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED dynamic_reconfigure)
-catkin_package(DEPENDS)
+
 catkin_python_setup()
 
+generate_dynamic_reconfigure_options(cfg/ROSArduinoBridge.cfg)
+
+catkin_package(DEPENDS)
+
+catkin_package(CATKIN_DEPENDS dynamic_reconfigure)
+
 install(DIRECTORY config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
@@ -16,3 +22,5 @@ install(DIRECTORY launch
 install(DIRECTORY nodes
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+

ros_arduino_python/cfg/ROSArduinoBridge.cfg

@@ -0,0 +1,19 @@
+#! /usr/bin/env python
+
+PACKAGE = "ros_arduino_python"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("Kp", double_t, 0, "Kp - Proportional", 10.0, 0.0, 50.0)
+
+gen.add("Kd", double_t, 0, "Kd - Differential", 12.0, 0.0, 50.0)
+
+gen.add("Ki", double_t, 0, "Ki - Integral", 0.0, 0.0, 10.0)
+
+gen.add("Ko", double_t, 0, "Ko - Offset", 50.0, 0.0, 100.0)
+
+gen.add("accel_limit", double_t, 0, "Acceleration limit", 1.0, 0.0, 10.0)
+
+exit(gen.generate(PACKAGE, "ros_arduino_bridge", "ROSArduinoBridge"))

ros_arduino_python/nodes/arduino_node.py

@@ -23,6 +23,9 @@ import rospy
 from ros_arduino_python.arduino_driver import Arduino
 from ros_arduino_python.arduino_sensors import *
 from ros_arduino_msgs.srv import *
+from dynamic_reconfigure.server import Server
+import dynamic_reconfigure.client
+from ros_arduino_python.cfg import ROSArduinoBridgeConfig
 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
@@ -37,7 +40,7 @@ from math import radians
 
 class ArduinoROS():
     def __init__(self):
-        rospy.init_node('Arduino', log_level=rospy.INFO)
+        rospy.init_node('arduino', log_level=rospy.DEBUG)
                 
         # Cleanup when termniating the node
         rospy.on_shutdown(self.shutdown)
@@ -111,23 +114,32 @@ class ArduinoROS():
         # A service to reset the odometry values to 0
         rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)
 
+        # A service to update the PID parameters Kp, Kd, Ki, and Ko
+        rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)
+
+        # Fire up the dynamic_reconfigure server
+        dyn_server = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback)
+
+        # Connect to the dynamic_reconfigure client
+        dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5)
+
         # Initialize the device
         self.device = Arduino(self.port, self.baud, self.timeout)
-        
+
         # Make the connection
         self.device.connect()
-        
+
         rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
      
         # Reserve a thread lock
         mutex = thread.allocate_lock()
 
         # Initialize any sensors
-        self.mySensors = list()
+        self.sensors = list()
-        
+
         # Read in the sensors parameter dictionary
         sensor_params = rospy.get_param("~sensors", dict({}))
-        
+
         # Initialize individual sensors appropriately
         for name, params in sensor_params.iteritems():
             if params['type'].lower() == 'Ping'.lower():
@@ -144,12 +156,14 @@ class ArduinoROS():
                 sensor = PhidgetsVoltage(self.device, name, **params)
             elif params['type'].lower() == 'PhidgetsCurrent'.lower():
                 sensor = PhidgetsCurrent(self.device, name, **params)
+            elif params['type'].lower() == 'IMU'.lower():
+                sensor = IMU(self.device, name, **params)
                 
 #                if params['type'].lower() == 'MaxEZ1'.lower():
 #                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
 #                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
 
-            self.mySensors.append(sensor)
+            self.sensors.append(sensor)
             
             if params['rate'] != None and params['rate'] != 0:
                 rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
@@ -208,7 +222,7 @@ class ArduinoROS():
 
         # Initialize the base controller if used
         if self.use_base_controller:
-            self.myBaseController = BaseController(self.device, self.base_frame)
+            self.base_controller = BaseController(self.device, self.base_frame)
             
         print "\n==> ROS Arduino Bridge ready for action!"
     
@@ -218,7 +232,7 @@ class ArduinoROS():
             try:
                 self.device.serial_port.inWaiting()
             except IOError:
-                rospy.loginfo("Lost serial connection...")
+                rospy.loginfo("Lost serial connection. Waiting to reconnect...")
                 self.device.close()
                 while True:
                     try:
@@ -229,7 +243,7 @@ class ArduinoROS():
                         r.sleep()
                         continue
 
-            for sensor in self.mySensors:
+            for sensor in self.sensors:
                 if sensor.rate != 0:
                     mutex.acquire()
                     sensor.poll()
@@ -237,7 +251,7 @@ class ArduinoROS():
                     
             if self.use_base_controller:
                 mutex.acquire()
-                self.myBaseController.poll()
+                self.base_controller.poll()
                 mutex.release()
                 
             if self.have_joints:
@@ -253,9 +267,10 @@ class ArduinoROS():
                 msg = SensorState()
                 msg.header.frame_id = self.base_frame
                 msg.header.stamp = now
-                for i in range(len(self.mySensors)):
+                for i in range(len(self.sensors)):
-                    msg.name.append(self.mySensors[i].name)
+                    if self.sensors[i].message_type != MessageType.IMU:
-                    msg.value.append(self.mySensors[i].value)
+                        msg.name.append(self.sensors[i].name)
+                        msg.value.append(self.sensors[i].value)
                 try:
                     self.sensorStatePub.publish(msg)
                 except:
@@ -326,10 +341,45 @@ class ArduinoROS():
 
     def ResetOdometryHandler(self, req):
         if self.use_base_controller:
-            self.myBaseController.reset_odometry()
+            self.base_controller.reset_odometry()
         else:
             rospy.logwarn("Not using base controller!")
         return EmptyResponse()
+
+    def UpdatePIDHandler(self, req):
+        if self.use_base_controller:
+           self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko)
+           rospy.loginfo("Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f" %(req.Kp, req.Kd, req.Ki, req.Ko))
+        else:
+            rospy.logwarn("Not using base controller!")
+        return UpdatePIDResponse()
+
+    def dynamic_reconfigure_server_callback(self, config, level):
+        if self.use_base_controller:
+            try:
+                if self.base_controller.Kp != config['Kp']:
+                    self.base_controller.Kp = config['Kp']
+
+                if self.base_controller.Kd != config['Kd']:
+                    self.base_controller.Kd = config['Kd']
+
+                if self.base_controller.Ki != config['Ki']:
+                    self.base_controller.Ki = config['Ki']
+
+                if self.base_controller.Ko != config['Ko']:
+                    self.base_controller.Ko = config['Ko']
+
+                if self.base_controller.accel_limit != config['accel_limit']:
+                    self.base_controller.accel_limit = config['accel_limit']
+                    self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate
+
+                self.device.update_pid(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko)
+
+                rospy.loginfo("Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f" %(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko, self.base_controller.accel_limit))
+            except Exception as e:
+                print e
+
+        return config
         
     def shutdown(self):
         rospy.loginfo("Shutting down Arduino node...")

ros_arduino_python/package.xml

@@ -10,6 +10,8 @@
   <url>http://ros.org/wiki/ros_arduino_python</url>
   
   <buildtool_depend>catkin</buildtool_depend>
+  
+  <build_depend>dynamic_reconfigure</build_depend>
 
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
@@ -19,4 +21,6 @@
   <run_depend>tf</run_depend>
   <run_depend>ros_arduino_msgs</run_depend>
   <run_depend>python-serial</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+
 </package>