Преглед изворни кода

新增获取电流的函数,发布于/sensor/CurrentValue话题;修改部分README文件

Jally пре 4 година
родитељ
комит
cb1c261c7b

+ 1 - 1
src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/kinetic/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

+ 1 - 1
src/robot_calibration/README.md

@@ -14,4 +14,4 @@ $ roslaunch robot_calibration pid.launch
 $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
 ```
 
-可以在跳出来的rqt_reconfigure窗口拖动参数动态调整pid参数,并在rqt_plot界面观察当前效果。
+可以在跳出来的rqt_reconfigure窗口拖动参数动态调整pid参数,并在rqt_plot界面观察当前效果。调整完后,将最终参数填入/home/zjl/blackTornadoRobot/src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml中。

+ 3 - 0
src/robot_calibration/launch/pid.launch

@@ -7,10 +7,13 @@
     20200420: init this file.
 -->
 <launch>
+   <!--启动mobilebase arduino,并加载my_arduino_params.yaml/-->
    <node name="mobilebase_arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen" required="true">
       <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
    </node>
+   <!--启动动态调节pid参数的节点/-->
    <node pkg="robot_calibration" type="dynamic_tutorials" name="dynamic_tutorials" output="screen" />
+   <!--启动rqt_reconfiure和rqt_plot界面/-->
    <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="rqt_reconfigure" output="screen" />
    <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot" output="screen" />
 </launch>

+ 1 - 1
src/ros_arduino_bridge/ros_arduino_msgs/msg/Digital.msg

@@ -1,4 +1,4 @@
 # Reading on a digital pin
 Header header
-uint8 value
+float32 value
 

+ 1 - 0
src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -63,5 +63,6 @@ CWheel_Ko: 50
 sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
   batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1, direction: input},
+  CurrentValue: {pin: 0, type: CurrentValue, rate: 0.1, direction: input}
 }
 

+ 2 - 0
src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -112,6 +112,8 @@ class ArduinoROS():
                 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)
+            elif params['type'] == 'CurrentValue':
+                sensor = CurrentValue(self.controller, name, params['pin'], params['rate'], self.base_frame)
 
             try:
                 self.mySensors.append(sensor)

+ 7 - 4
src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -4,10 +4,11 @@
 """
   Copyright: 2016-2020 ROS小课堂 www.corvin.cn
   Description: A Python driver for the Arduino microcontroller.
-  Author: corvin
+  Author: corvin, jally
   History:
     20200329:增加获取电池剩余电量百分比函数.
     20200412:增加发布红外测距信息的服务.
+    20200423:增加获取电流传感器信息
 """
 from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
@@ -404,9 +405,6 @@ class Arduino:
     def beep_ring(self, value):
         return self.execute_ack('p %d' %value)
 
-    def detect_current(self):
-        return self.execute('f')
-
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
         return self.execute_array('h')
 
@@ -415,6 +413,11 @@ class Arduino:
         #rospy.loginfo("bat percent:" + str(percent))
         return percent
 
+    def getCurrentValue(self): #获取电流值
+        currentvalue = self.execute('f')
+        #rospy.loginfo("get current value:" + str(currentvalue))
+        return currentvalue
+
     def light_show(self, value):
         return self.execute_ack('l %d' %value)
 

+ 10 - 0
src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -5,6 +5,7 @@
     Sensor class for the arudino_python package
     History:
         20200330:增加获取电池剩余电量函数.
+        20200423:增加获取电流函数
 """
 import roslib; roslib.load_manifest('ros_arduino_python')
 import rospy
@@ -233,6 +234,15 @@ class BatPercent(DigitalSensor):
         percent = self.controller.getBatPercent()
         return int(percent)
 
+class CurrentValue(DigitalSensor):
+    def __init__(self, *args, **kwargs):
+        super(CurrentValue, self).__init__(*args, **kwargs)
+
+    def read_value(self):
+        currentvalue = self.controller.getCurrentValue()
+        #rospy.loginfo("read currentvalue: "+currentvalue)
+        return float(currentvalue)
+
 
 if __name__ == '__main__':
     rospy.loginf("arduino sensor main function ...")

+ 17 - 0
src/voice_system/voice_alarm/README.md

@@ -0,0 +1,17 @@
+# blackTornadoRobot语音报警功能包
+可以对当前机器人的一些状态进行报警提示。
+
+### 低电量报警
+首先启动小车
+
+```sh
+$ roslaunch robot_bringup robot_bringup.launch
+```
+
+然后启动低电量报警
+
+```sh
+$ roslaunch voice_alarm voice_alarm.launch
+```
+
+在launch文件中可以修改低电量报警的警戒线值,默认需要立即充电的警戒线为10%,电量较低的预警线为20%.