增加控制摄像头舵机转动的代码

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -37,6 +37,8 @@ add_service_files(FILES
                   SetSpeed.srv
                   SetServoSpeed.srv
                   UpdatePID.srv
+                  CameraControl.srv
+                  GetInfraredDistance.srv
                  )
 
 generate_messages(   

src/ros_arduino_bridge/ros_arduino_msgs/srv/CameraControl.srv

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

src/ros_arduino_bridge/ros_arduino_msgs/srv/GetInfraredDistance.srv

@@ -0,0 +1,4 @@
+---
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -3,7 +3,7 @@
 
 """
   Copyright:2016-2020 www.corvin.cn ROS小课堂
-  Description: A ROS Node for the Arduino microcontroller
+  Description: A ROS Node for the Arduino DUE microcontroller
   Author: corvin
   History:
     20200706:init this file.
@@ -45,7 +45,7 @@ class ArduinoROS():
         self.cmd_vel = Twist()
 
         # A cmd_vel publisher so we can stop the robot when shutting down
-        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
+        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
 
         # A service to turn set the direction of a digital pin (0 = input, 1 = output)
         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
@@ -62,6 +62,15 @@ class ArduinoROS():
         # A service to read the value of an analog sensor
         rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
 
+        # A service to control camera angle
+        rospy.Service('~camera_control', CameraControl, self.CameraControlHandler)
+
+        # A service to get new pid params
+        rospy.Service('~dynamic_pid', DynamicPID, self.DynamicPIDHandler)
+
+        # A service to return infrared sensor distance
+        rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
+
         # Initialize the controlller
         self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
@@ -147,7 +156,20 @@ class ArduinoROS():
         self.controller.light_show(req.value)
         return LightShowResponse()
 
-    # Stop the robot
+    def CameraControlHandler(self, req):
+        self.controller.camera_angle(req.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)
+
+    def DynamicPIDHandler(self, req):
+        self.controller.update_pid(req.value1, req.value2, 0, 50,
+                                   req.value3, req.value4, 0, 50)
+        return DynamicPIDResponse()
+
+    # Stop the robot arduino connect
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")
         try:

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

@@ -48,6 +48,7 @@ class Arduino:
         # An array to cache digital sensor readings
         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
 
+    #与下位机arduino建立连接
     def connect(self):
         if self.is_use_serial:
             self.serial_connect()
@@ -55,6 +56,7 @@ class Arduino:
             self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
             self.i2c_connect()
 
+    #使用串口连接进行通信
     def serial_connect(self):
         try:
             rospy.loginfo("Connecting to Arduino on port: " + self.serial_port)
@@ -76,6 +78,7 @@ class Arduino:
             rospy.logerr(traceback.print_exc(file=sys.stdout))
             os._exit(1)
 
+    #使用IIC总线进行数据通信
     def i2c_connect(self):
         try:
             rospy.loginfo("Connecting to Arduino on IIC addr:"+str(self.i2c_slave_addr))
@@ -367,6 +370,21 @@ class Arduino:
         except:
             return None
 
+    def camera_angle(self, angle):
+        ''' control camera angle
+        '''
+        #rospy.loginfo("camera operation ID:" + str(operation))
+        if self.is_use_serial:
+            return self.execute_ack('s 0 %d' %(angle))
+        else:
+            '''control camera angle up/down by IIC
+            '''
+            cmd = (' 0 %d\r' %(angle))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('s'), [ord(c) for c in cmd])
+            except:
+                return None
+
     def drive(self, AWheel, BWheel):
         ''' Speeds are given in encoder ticks per PID interval
         '''
@@ -383,6 +401,7 @@ class Arduino:
         except:
             return None
 
+    #使用串口发送停止电机转动命令
     def stop(self):
         ''' Stop all three motors.
         '''
@@ -466,7 +485,6 @@ class Arduino:
             except:
                 return None
 
-
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
         if self.is_use_serial:
             return self.execute_array('h')

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -2,7 +2,7 @@
 # coding:utf-8
 
 # Copyright: 2016-2020 wwww.corvin.cn ROS小课堂
-# Description: 通过键盘遥控小车行进。
+# Description: 通过键盘遥控小车行进，摄像头舵机转动。
 
 from __future__ import print_function
 
@@ -12,6 +12,7 @@ from geometry_msgs.msg import Twist
 
 import sys, select, termios, tty
 
+from ros_arduino_msgs.srv import CameraControl
 import time
 
 msg = """
@@ -28,6 +29,10 @@ 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 : Camera UP
+2 : Camera Down
 
 CTRL-C to quit
 """
@@ -48,6 +53,13 @@ speedBindings={
         'c':(1,.9),
     }
 
+cameraUpBindings={
+        '1':(5),
+    }
+cameraDownBindings={
+        '2':(-5),
+    }
+
 def getKey():
     tty.setraw(sys.stdin.fileno())
     select.select([sys.stdin], [], [], 0)
@@ -62,10 +74,21 @@ def vels(speed,turn):
 def angles(angle):
     return "currently:\tangle %s " % (angle)
 
+
+#调用service: mobilebase_arduino/camera_control控制相机上下转动
+def client_srv():
+    rospy.wait_for_service("mobilebase_arduino/camera_control")
+    try:
+        gripper_client = rospy.ServiceProxy("mobilebase_arduino/camera_control",CameraControl)
+        resp = gripper_client.call(angle)
+    except rospy.ServiceException, e:
+        rospy.logwarn("Service call failed: %s"%e)
+
+
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
-    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 5)
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 2)
     rospy.init_node('teleop_twist_keyboard')
 
     speed = rospy.get_param("~speed", 0.3)
@@ -97,6 +120,24 @@ if __name__=="__main__":
                 if (status == 14):
                     print(msg)
                 status = (status + 1) % 15
+            elif key in cameraUpBindings.keys():
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+                print(angles(angle))
+                if (angle < 90 and angle >=0):
+                    angle = angle + cameraUpBindings[key]
+                    client_srv()
+            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()
             else:
                 x = 0
                 y = 0