增加可以使用键盘3,4控制s6上左右转动舵机的代码

src/ros_arduino_bridge/ros_arduino_msgs/srv/CameraControl.srv

@@ -1,2 +1,3 @@
-float32 angle
+uint8 servo_id
+float32 servo_angle
 ---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -157,7 +157,7 @@ class ArduinoROS():
         return LightShowResponse()
 
     def CameraControlHandler(self, req):
-        self.controller.camera_angle(req.angle)
+        self.controller.camera_angle(req.servo_id, req.servo_angle)
         return CameraControlResponse()
 
     def GetInfraredDistanceHandler(self, req):

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

@@ -370,16 +370,16 @@ class Arduino:
         except:
             return None
 
-    def camera_angle(self, angle):
+    def camera_angle(self, servo_id, servo_angle):
         ''' control camera angle
         '''
         #rospy.loginfo("camera operation ID:" + str(operation))
         if self.is_use_serial:
-            return self.execute_ack('s 0 %d' %(angle))
+            return self.execute_ack('s %d %d' %(servo_id, servo_angle))
         else:
-            '''control camera angle up/down by IIC
+            '''control camera angle up/down left/right by IIC
             '''
-            cmd = (' 0 %d\r' %(angle))
+            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])
             except:

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -3,6 +3,8 @@
 
 # Copyright: 2016-2020 wwww.corvin.cn ROS小课堂
 # Description: 通过键盘遥控小车行进，摄像头舵机转动。
+# History:
+#    20200726: 增加控制左右转动舵机的代码.
 
 from __future__ import print_function
 
@@ -29,10 +31,13 @@ anything else : stop
 q/z : increase/decrease max speeds by 10%
 w/x : increase/decrease only linear speed by 10%
 e/c : increase/decrease only angular speed by 10%
-1/2 : increase/decrease gripper angle by 5°
+1/2 : increase/decrease up/down servo angle by 5°
+3/4 : increase/decrease left/right servo angle by 5°
 
 1 : Camera UP
 2 : Camera Down
+3 : Camera Left
+4 : Camera Right
 
 CTRL-C to quit
 """
@@ -59,6 +64,12 @@ cameraUpBindings={
 cameraDownBindings={
         '2':(-5),
     }
+cameraLeftBindings={
+        '3':(5),
+    }
+cameraRightBindings={
+        '4':(-5),
+    }
 
 def getKey():
     tty.setraw(sys.stdin.fileno())
@@ -76,11 +87,11 @@ def angles(angle):
 
 
 #调用service: mobilebase_arduino/camera_control控制相机上下转动
-def client_srv():
+def client_srv(ser_id, ser_angle):
     rospy.wait_for_service("mobilebase_arduino/camera_control")
     try:
-        gripper_client = rospy.ServiceProxy("mobilebase_arduino/camera_control",CameraControl)
-        resp = gripper_client.call(angle)
+        camera_client = rospy.ServiceProxy("mobilebase_arduino/camera_control", CameraControl)
+        resp = camera_client.call(ser_id, ser_angle)
     except rospy.ServiceException, e:
         rospy.logwarn("Service call failed: %s"%e)
 
@@ -99,7 +110,8 @@ if __name__=="__main__":
     z = 0
     th = 0
     status = 0
-    angle = 10
+    upDownAngle = 10
+    leftRightAngle = 90
     command = 2
 
     try:
@@ -125,19 +137,37 @@ if __name__=="__main__":
                 y = 0
                 z = 0
                 th = 0
-                print(angles(angle))
-                if (angle < 90 and angle >=0):
-                    angle = angle + cameraUpBindings[key]
-                    client_srv()
+                print(angles(upDownAngle))
+                if (upDownAngle < 90 and upDownAngle >=0):
+                    upDownAngle = upDownAngle + cameraUpBindings[key]
+                    client_srv(0, upDownAngle)
             elif key in cameraDownBindings.keys():
                 x = 0
                 y = 0
                 z = 0
                 th = 0
-                print(angles(angle))
-                if (angle <= 90 and angle >0):
-                    angle = angle + cameraDownBindings[key]
-                    client_srv()
+                print(angles(upDownAngle))
+                if (upDownAngle <= 90 and upDownAngle >0):
+                    upDownAngle = upDownAngle + cameraDownBindings[key]
+                    client_srv(0, upDownAngle)
+            elif key in cameraLeftBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                print(angles(leftRightAngle))
+                if (leftRightAngle < 180 and leftRightAngle >=0):
+                    leftRightAngle = leftRightAngle + cameraLeftBindings[key]
+                    client_srv(1, leftRightAngle)
+            elif key in cameraRightBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                print(angles(leftRightAngle))
+                if (leftRightAngle <= 180 and leftRightAngle >0):
+                    leftRightAngle = leftRightAngle + cameraRightBindings[key]
+                    client_srv(1, leftRightAngle)
             else:
                 x = 0
                 y = 0