删除自定义的控制相机转动的service,使用servo_write自定义服务来控制所有舵机

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -20,10 +20,8 @@ add_service_files(FILES
                   DigitalWrite.srv
                   ServoRead.srv
                   ServoWrite.srv
-                  SetSpeed.srv
                   SetServoSpeed.srv
                   UpdatePID.srv
-                  CameraControl.srv
                   GetInfraredDistance.srv
                   DynamicPID.srv
                  )

src/ros_arduino_bridge/ros_arduino_msgs/srv/CameraControl.srv

@@ -1,3 +0,0 @@
-uint8 servo_id
-float32 servo_angle
----

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoRead.srv

@@ -1,3 +1,7 @@
-uint8 id
 ---
-uint8 position
+uint8 position_0
+uint8 position_1
+uint8 position_2
+uint8 position_3
+uint8 position_4
+uint8 position_5

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoWrite.srv

@@ -1,3 +1,3 @@
-uint8 id
-uint8 position
+uint8 servo_id
+uint8 servo_angle
 ---

src/ros_arduino_bridge/ros_arduino_msgs/srv/SetSpeed.srv

@@ -1,2 +0,0 @@
-float32 speed
----

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -50,15 +50,15 @@ class ArduinoROS():
         #读取模拟引脚数值的服务
         rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
 
-        #控制摄像头转动的服务
-        rospy.Service('~camera_control', CameraControl, self.CameraControlHandler)
-
         # A service to get new pid params
         rospy.Service('~dynamic_pid', DynamicPID, self.DynamicPIDHandler)
 
         #用来获取红外测据传感器距离信息的服务
         rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
         
+        #设置指定舵机转动到指定角度的服务
+        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
+
         #当节点退出的时候执行的回调函数
         rospy.on_shutdown(self.shutdown)
 
@@ -137,10 +137,6 @@ class ArduinoROS():
         value = self.controller.analog_read(req.pin)
         return AnalogReadResponse(value)
 
-    def CameraControlHandler(self, req):
-        self.controller.camera_angle(req.servo_id, req.servo_angle)
-        return CameraControlResponse()
-
     def GetInfraredDistanceHandler(self, req):
         value = self.controller.detect_distance()
         return GetInfraredDistanceResponse(value[0]/100.0, value[1]/100.0, value[2]/100.0)
@@ -150,6 +146,10 @@ class ArduinoROS():
                                    req.value3, req.value4, 0, 50)
         return DynamicPIDResponse()
 
+    def ServoWriteHandler(self, req):
+        self.controller.servo_write(req.servo_id, req.servo_angle)
+        return ServoWriteResponse()
+
     #断开与下位机的连接,先向小车发送停止运动命令,最后断开通信接口
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -328,15 +328,11 @@ class Arduino:
         except:
             return None
 
-    def camera_angle(self, servo_id, servo_angle):
-        ''' control camera angle
-        '''
-        #rospy.loginfo("camera operation ID:" + str(operation))
+    #控制指定舵机转动到指定角度,servo id从0-5,共计6个舵机
+    def servo_write(self, servo_id, servo_angle):
         if self.is_use_serial:
             return self.execute_ack('s %d %d' %(servo_id, servo_angle))
         else:
-            '''control camera angle up/down left/right by IIC
-            '''
             cmd = (' %d %d\r' %(servo_id, servo_angle))
             try:
                 self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('s'), [ord(c) for c in cmd])

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -12,7 +12,7 @@ import roslib; roslib.load_manifest('teleop_twist_keyboard')
 import rospy
 from geometry_msgs.msg import Twist
 import sys, select, termios, tty
-from ros_arduino_msgs.srv import CameraControl
+from ros_arduino_msgs.srv import ServoWrite
 
 msg = """
 从键盘读取控制命令遥控小车移动,控制按键如下:
@@ -76,9 +76,9 @@ def angles(angle):
 
 #调用service: mobilebase_arduino/camera_control控制相机上下转动
 def client_srv(ser_id, ser_angle):
-    rospy.wait_for_service("mobilebase_arduino/camera_control")
+    rospy.wait_for_service("mobilebase_arduino/servo_write")
     try:
-        camera_client = rospy.ServiceProxy("mobilebase_arduino/camera_control", CameraControl)
+        camera_client = rospy.ServiceProxy("mobilebase_arduino/servo_write", ServoWrite)
         resp = camera_client.call(ser_id, ser_angle)
     except (rospy.ServiceException, rospy.ROSException) as e:
         rospy.logwarn("Service call failed: %s"%e)