新增获取电池剩余电量的功能,并在sensor/batPercent话题中发布

src/robot_bringup/launch/robot_bringup.launch

@@ -24,7 +24,7 @@
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
         <param name="output_frame" value="odom_combined"/>
         <param name="freq" value="10.0"/>
-        <param name="sensor_timeout" value="1.0"/>
+        <param name="sensor_timeout" value="1.2"/>
         <param name="odom_used" value="true"/>
         <param name="imu_used" value="true"/>
         <param name="vo_used" value="false"/>

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -1,8 +1,17 @@
-# For a direct USB cable connection, the port name is typically
-# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:arduino节点运行时加载的参数,下面对参数进行解释:
+#   is_use_serial:与arduino通信是否使用串口,false的话就是IIC通信
+#   serial_port:串口通信时arduino的端口号
+#   serial_baud:串口通信时波特率
+#   i2c_smbus_num:系统管理总线号,树莓派为1
+#   isc_slave_addr:arduino的IIC设备地址
+# Author: corvin
+# History:
+#   20200330:init this file.
 
 is_use_serial: True
 
+# /dev/ttyACM# where is # is a number such as 0, 1 etc
 serial_port: /dev/ttyACM0
 serial_baud: 57600
 
@@ -50,9 +59,10 @@ CWheel_Ki: 0
 CWheel_Ko: 50
 
 # === Sensor definitions.Sensor type can be one of the follow (case sensitive!):
-#	  * Analog
-#	  * Digital
+#  * Analog
+#  * Digital
 sensors: {
-  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 10, direction: input},
+  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
+  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1, direction: input},
 }
 

src/ros_arduino_bridge/ros_arduino_python/launch/arduino.launch

@@ -11,4 +11,3 @@
       <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
    </node>
 </launch>
-

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -3,7 +3,6 @@
 """
     A ROS Node for the Arduino microcontroller
 """
-
 import os
 import rospy
 import thread
@@ -34,7 +33,7 @@ class ArduinoROS():
         self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
 
         # Overall loop rate: should be faster than fastest sensor rate
-        self.rate = int(rospy.get_param("~rate", 20))
+        self.rate = int(rospy.get_param("~rate", 25))
         loop_rate = rospy.Rate(self.rate)
 
         # Initialize a Twist message
@@ -95,14 +94,14 @@ class ArduinoROS():
                 sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
             elif params['type'] == 'Analog':
                 sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
-            elif params['type'] == 'Voltage':
-                sensor = VoltageSensor(self.controller, name, params['pin'], params['rate'], self.base_frame)
+            elif params['type'] == 'BAT_PERCENT':
+                sensor = BatPercent(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'MotorTotalCurrent':
                 sensor = MotorTotalCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
 
             try:
                 self.mySensors.append(sensor)
-                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
+                rospy.loginfo(name + " " + str(params) + " published on topic " + "/sensor/" + name)
             except:
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
 
@@ -158,19 +157,17 @@ class ArduinoROS():
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")
         try:
-            rospy.logwarn("Stopping the robot...")
+            rospy.logwarn("Stopping the robot move...")
             self.cmd_vel_pub.Publish(Twist())
-            rospy.sleep(2)
         except:
             pass
 
-        # Close the serial port
+        # Close the serial port or IIC
         try:
             self.controller.close()
         except:
             pass
         finally:
-            rospy.logwarn("Serial port closed.")
             os._exit(0)
 
 
@@ -178,7 +175,7 @@ if __name__ == '__main__':
     try:
         myArduino = ArduinoROS()
     except SerialException:
-        rospy.logerr("Serial exception trying to open port.")
+        rospy.logerr("ERROR trying to open serial port.")
         os._exit(0)
     else:
         rospy.logerr("Get other unknown error !")

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

@@ -2,15 +2,18 @@
 #-*- coding:utf-8 -*-
 
 """
-    A Python driver for the Arduino microcontroller.
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description: A Python driver for the Arduino microcontroller.
+  Author: corvin
+  History:
+    20200329:增加获取电池剩余电量百分比函数.
 """
-
-from math import pi as PI
 from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
 from serial import Serial
 import sys, traceback
 import numpy as np
+import roslib, rospy
 
 
 class Arduino:
@@ -30,7 +33,7 @@ class Arduino:
         self.i2c_smbus_num  = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
 
-        self.timeout  = timeout
+        self.timeout = timeout
 
         self.encoder_count    = 0
         self.writeTimeout     = timeout
@@ -54,7 +57,7 @@ class Arduino:
 
     def serial_connect(self):
         try:
-            print "Connecting to Arduino on port", self.serial_port, "..."
+            rospy.loginfo("Connecting to Arduino on port: " + self.serial_port)
             self.serial_port = Serial(port=self.serial_port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
             # The next line is necessary to give the firmware time to wake up.
             time.sleep(1)
@@ -66,35 +69,29 @@ class Arduino:
                     raise SerialException
 
             self.beep_ring(1)
-            print "Connected at", self.baudrate
-            print "Arduino is ready !"
+            rospy.loginfo("Arduino connected baudrate: "+str(self.baudrate))
         except SerialException:
-            print "Serial Exception:"
-            print sys.exc_info()
-            print "Traceback follows:"
-            traceback.print_exc(file=sys.stdout)
-            print "Cannot connect to Arduino!"
+            rospy.logerr("Cannot connect to Arduino !")
+            rospy.logerr(sys.exc_info())
+            rospy.logerr(traceback.print_exc(file=sys.stdout))
             os._exit(1)
 
     def i2c_connect(self):
         try:
-            print "Connecting to Arduino on IIC addr ", self.i2c_slave_addr, "..."
+            rospy.loginfo("Connecting to Arduino on IIC addr:"+self.i2c_slave_addr)
             test = self.i2c_get_baud()
             if test != self.baudrate:
                 time.sleep(1)
-                print "Connecting ..."
                 test = self.i2c_get_baud()
                 if test != self.baudrate:
                     raise Exception
 
             self.beep_ring(1)
-            print "Connected at", self.baudrate
-            print "Arduino is ready !"
+            rospy.loginfo("Arduino is connected by IIC !")
         except Exception:
-            print sys.exc_info()
-            print "Traceback follows:"
-            traceback.print_exc(file=sys.stdout)
-            print "Cannot connect to Arduino IIC slave addr !"
+            rospy.logerr(sys.exc_info())
+            rospy.logerr(traceback.print_exc(file=sys.stdout))
+            rospy.logerr("Cannot connect to Arduino IIC slave addr !")
             os._exit(1)
 
     def open(self):
@@ -105,7 +102,6 @@ class Arduino:
     def close(self):
         ''' Close the serial port or i2c bus connect.
         '''
-        print "Now disconnecting ..."
         self.beep_ring(0)
         if self.is_use_serial:
             self.serial_port.close()
@@ -121,12 +117,12 @@ class Arduino:
     def recv(self, timeout=0.5):
         timeout = min(timeout, self.timeout)
         ''' This command should not be used on its own: it is called by the execute commands
-            below in a thread safe manner.  Note: we use read() instead of readline() since
-            readline() tends to return garbage characters from the Arduino
+            below in a thread safe manner
         '''
         c = ''
         value = ''
         attempts = 0
+
         while c != '\r':
             c = self.serial_port.read(1)
             value += c
@@ -185,11 +181,11 @@ class Arduino:
                     self.serial_port.write(cmd + '\r')
                     value = self.recv(self.timeout)
                 except:
-                    print "Exception executing command: " + cmd
+                    rospy.logerr("Exception executing command: " + cmd)
                 attempts += 1
         except:
             self.mutex.release()
-            print "Exception executing command: " + cmd
+            rospy.logerr("Exception executing command: " + cmd)
             value = None
 
         self.mutex.release()
@@ -215,11 +211,11 @@ class Arduino:
                     self.serial_port.write(cmd + '\r')
                     values = self.recv_array()
                 except:
-                    print("Exception executing command: " + cmd)
+                    rospy.logerr("Exception executing command: " + cmd)
                 attempts += 1
         except:
             self.mutex.release()
-            print "Exception executing command: " + cmd
+            rospy.logerr("Exception executing command: " + cmd)
             raise SerialException
             return []
 
@@ -252,12 +248,12 @@ class Arduino:
                     self.serial_port.write(cmd + '\r')
                     ack = self.recv(self.timeout)
                 except:
-                    print "Exception executing command: " + cmd
+                    rospy.logerr("Exception executing command: " + cmd)
                 attempts += 1
         except:
             self.mutex.release()
-            print "execute_ack exception when executing", cmd
-            print sys.exc_info()
+            rospy.logerr("execute_ack exception when executing" + cmd)
+            rospy.logerr(sys.exc_info())
             return 0
 
         self.mutex.release()
@@ -268,14 +264,14 @@ class Arduino:
                          CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
         ''' Set the PID parameters on the Arduino by serial
         '''
-        print "Updating PID parameters"
+        rospy.loginfo("Updating Motors PID parameters ...")
         cmd = 'u '+str(AWheel_Kp)+':'+str(AWheel_Kd)+':'+str(AWheel_Ki)+':'+str(AWheel_Ko)+':'+str(BWheel_Kp)+':'+str(BWheel_Kd)+':'+str(BWheel_Ki)+':'+str(BWheel_Ko)+':'+str(CWheel_Kp)+':'+str(CWheel_Kd)+':'+str(CWheel_Ki)+':'+str(CWheel_Ko)
         self.execute_ack(cmd)
 
     def i2c_update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
                          BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,
                          CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
-        print "Updating PID parameters by IIC"
+        rospy.loginfo("Updating PID parameters by IIC")
 
     def get_baud(self):
         ''' Get the current baud rate on the serial port.
@@ -303,7 +299,7 @@ class Arduino:
     def get_encoder_counts(self):
         values = self.execute_array('e')
         if len(values) != 3:
-            print "Encoder count was not 3"
+            rospy.logerr("Encoder count was not 3")
             raise SerialException
             return None
         else:
@@ -324,12 +320,12 @@ class Arduino:
             result_flag = result_string.index('\r')
             values=[int(e) for e in ''.join(result_string[:result_flag]).split('')]
         except:
-            print sys.exe_info()
+            rospy.logerr(sys.exe_info())
             traceback.print_exc(file=sys.stdout)
             return None
 
         if len(values) != 3:
-            print "Encoder count was not 3"
+            rospy.logwarn("Encoder count was not 3")
             return None
         else:
             return values
@@ -354,8 +350,6 @@ class Arduino:
         #rospy.loginfo("gripper operation ID:" + str(operation))
         return self.execute_ack('z %d' %(operation))
 
-    
-
     def drive(self, AWheel, BWheel, CWheel):
         ''' Speeds are given in encoder ticks per PID interval
         '''
@@ -402,29 +396,32 @@ class Arduino:
     def beep_ring(self, value):
         return self.execute_ack('p %d' %value)
 
-    def detect_voltage(self):
-        return self.execute('g')
-
     def detect_current(self):
         return self.execute('f')
 
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
+        #rospy.loginfo("detect distance")
         return self.execute_array('h')
 
+    def getBatPercent(self): #获取电池的剩余电量百分比
+        percent = self.execute('g')
+        #rospy.loginfo("bat percent:" + str(percent))
+        return percent
+
     def light_show(self, value):
         return self.execute_ack('l %d' %value)
 
     def get_pidin(self):
         values = self.execute_array('i')
         if len(values) != 3:
-            print "pidin was not 3"
+            rospy.logerr("pidin was not 3")
             raise SerialException
             return None
         else:
             return values
 
     def i2c_get_pidin(self):
-        print "IIC Get Pidin !"
+        rospy.loginfo("IIC Get Pidin !")
         #values = self.execute_array('i')
         #if len(values) != 3:
         #    print "pidin was not 3"
@@ -436,14 +433,14 @@ class Arduino:
     def get_pidout(self):
         values = self.execute_array('o')
         if len(values) != 3:
-            print "pidout was not 3"
+            rospy.logerr("pidout was not 3")
             raise SerialException
             return None
         else:
             return values
 
     def i2c_get_pidout(self):
-        print "IIC Get Pidout !"
+        rospy.loginfo("IIC Get Pidout !")
         #values = self.execute_array('o')
         #if len(values) != 3:
         #    print "pidout was not 3"
@@ -464,21 +461,21 @@ if __name__ == "__main__":
     myArduino = Arduino(is_use_serial, serial_port=portName, baudrate=baudRate, i2c_smbus_num=iic_smbus, i2c_slave_addr=iic_num, timeout=0.5)
     myArduino.connect()
 
-    print "Sleeping for 1 second..."
+    rospy.loginfo("Sleeping for 1 second...")
     time.sleep(1)
 
-    print "Test Beep ring 3 times..."
+    rospy.loginfo("Test Beep ring 3 times...")
     for i in range(3):
         myArduino.beep_ring(1)
         time.sleep(2.0)
 
-    print "Did you heard the beep ring ?"
-    print "Now print infrared sensors value:"
+    rospy.loginfo("Did you heard the beep ring ?")
+    rospy.loginfo("Now print infrared sensors value:")
     values = myArduino.detect_distance()
     distances = np.array([values[0], values[1], values[2]])
-    print distances/100.0
+    rospy.loginf(distances/100.0)
 
     myArduino.stop()
     myArduino.close()
-    print "Shutting down Arduino."
+    rospy.logwarn("Shutting down Arduino !")
 

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -1,9 +1,11 @@
 #!/usr/bin/env python
+#_*_ coding:utf-8 _*_
 
 """
     Sensor class for the arudino_python package
+    History:
+        20200330:增加获取电池剩余电量函数.
 """
-
 import roslib; roslib.load_manifest('ros_arduino_python')
 import rospy
 import numpy as np
@@ -41,6 +43,7 @@ class Sensor(object):
         self.t_delta = rospy.Duration(1.0 / self.rate)
         self.t_next = rospy.Time.now() + self.t_delta
 
+
     def poll(self):
         now = rospy.Time.now()
         if now > self.t_next:
@@ -48,11 +51,13 @@ class Sensor(object):
                 try:
                     self.value = self.read_value()
                 except:
+                    rospy.logerr("Sensor read value error !")
                     return
             else:
                 try:
                     self.ack = self.write_value()
                 except:
+                    rospy.logerr("Sensor write value error !")
                     return
 
             # For range sensors, assign the value to the range message field
@@ -76,17 +81,10 @@ class AnalogSensor(Sensor):
         super(AnalogSensor, self).__init__(*args, **kwargs)
 
         self.message_type = MessageType.ANALOG
-
         self.msg = Analog()
         self.msg.header.frame_id = self.frame_id
 
         self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
-
-        if self.direction == "output":
-            self.controller.pin_mode(self.pin, OUTPUT)
-        else:
-            self.controller.pin_mode(self.pin, INPUT)
-
         self.value = LOW
 
     def read_value(self):
@@ -123,18 +121,11 @@ class DigitalSensor(Sensor):
     def __init__(self, *args, **kwargs):
         super(DigitalSensor, self).__init__(*args, **kwargs)
 
-        self.message_type = MessageType.BOOL
-
+        self.message_type = MessageType.DIGITAL
         self.msg = Digital()
         self.msg.header.frame_id = self.frame_id
 
         self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
-
-        if self.direction == "output":
-            self.controller.pin_mode(self.pin, OUTPUT)
-        else:
-            self.controller.pin_mode(self.pin, INPUT)
-
         self.value = LOW
 
     def read_value(self):
@@ -230,30 +221,18 @@ class MotorTotalCurrent(AnalogFloatSensor):
         super(MotorTotalCurrent, self).__init__(*args, **kwargs)
 
     def read_value(self):
-        mylist=[]
-        for i in range(31):
-            value = self.controller.analog_read(self.pin)
-            mylist.append(value)
-
-        for m in range(30):
-            for n in range(0, 30-m):
-                if mylist[n] > mylist[n+1]:
-                    mylist[n], mylist[n+1] = mylist[n+1], mylist[n]
-
         midVal = mylist[15]
         result = (midVal/1024.0*4523.00 - 4523.00/2)/100
         return Decimal(result).quantize(Decimal('0.00'))
 
-class VoltageSensor(AnalogFloatSensor):
+class BatPercent(DigitalSensor):
     def __init__(self, *args, **kwargs):
-        super(VoltageSensor, self).__init__(*args, **kwargs)
+        super(BatPercent, self).__init__(*args, **kwargs)
 
     def read_value(self):
-        refVol = 4.77
-        voltage = self.controller.analog_read(self.pin)/1024.0*refVol*5
-        return round(voltage,2)
+        percent = self.controller.getBatPercent()
+        return int(percent)
 
 
 if __name__ == '__main__':
-    myController = Controller()
-
+    rospy.loginf("arduino sensor main function ...")

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -1,8 +1,9 @@
 #!/usr/bin/env python
 # coding:utf-8
 
+# Copyright: 2016-2020 wwww.corvin.cn ROS小课堂
 # Description: 通过键盘遥控小车行进、抓取。
-# History: 
+# History:
 #   20200317 增加小车抓取功能 by jally
 
 from __future__ import print_function
@@ -106,16 +107,15 @@ def client_srv():
         rospy.logwarn("Service call failed: %s"%e)
 
 
-
 if __name__=="__main__":
     settings = termios.tcgetattr(sys.stdin)
 
-    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 5)
     rospy.init_node('teleop_twist_keyboard')
 
-    speed = rospy.get_param("~speed", 0.12)
-    turn = rospy.get_param("~turn", 0.5)
-    
+    speed = rospy.get_param("~speed", 0.2)
+    turn = rospy.get_param("~turn", 0.8)
+
     x = 0
     y = 0
     z = 0
@@ -169,3 +169,4 @@ if __name__=="__main__":
         pub.publish(twist)
 
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+