提交动态调节pid参数

src/robot_calibration/CMakeLists.txt

@@ -10,6 +10,8 @@ project(robot_calibration)
 find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
+  roscpp
+  dynamic_reconfigure
 )
 
 ## System dependencies are found with CMake's conventions
@@ -87,10 +89,9 @@ find_package(catkin REQUIRED COMPONENTS
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+   config/Tutorials.cfg
+   )
 
 ###################################
 ## catkin specific configuration ##
@@ -103,8 +104,8 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES carebot_calibration
-#  CATKIN_DEPENDS roscpp rospy std_msgs
+  LIBRARIES robot_calibration
+  CATKIN_DEPENDS roscpp rospy std_msgs dynamic_reconfigure
 #  DEPENDS system_lib
 )
 
@@ -132,7 +133,7 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/carebot_calibration_node.cpp)
+add_executable(dynamic_tutorials src/pid_dynamic.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -142,7 +143,10 @@ include_directories(
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(dynamic_tutorials ${PROJECT_NAME}_gencfg)
+
+
+target_link_libraries(dynamic_tutorials ${catkin_LIBRARIES})
 
 ## Specify libraries to link a library or executable target against
 # target_link_libraries(${PROJECT_NAME}_node

src/robot_calibration/README.md

@@ -0,0 +1,17 @@
+# blackTornadoRobot参数标定
+参数标定功能包，可以标定角速度、线速度、PID参数
+
+### 标定PID参数
+首先启动标定程序
+
+```sh
+$ roslaunch robot_calibration pid.launch
+```
+
+然后运行键盘控制程序
+
+```sh
+$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+```
+
+可以在跳出来的rqt_reconfigure窗口拖动参数动态调整pid参数，并在rqt_plot界面观察当前效果。

src/robot_calibration/config/Tutorials.cfg

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+     
+"""
+      Copyright: 2016-2020(c) ROS小课堂 www.corvin.cn
+      FileName: dynamic.cfg
+      Author: corvin, jally
+      Description:
+        dynamic_reconfigure需要使用的cfg配置文件,用于调试pid参数
+      History:
+        20200420: init this file.
+"""
+     
+PACKAGE = "dynamic_tutorials"
+     
+from dynamic_reconfigure.parameter_generator_catkin import *
+     
+gen = ParameterGenerator()
+     
+gen.add("AWheel_Kp", int_t, 0, "wheel A P", 20, 10, 35)
+gen.add("AWheel_Kd", int_t, 0, "wheel A D", 20, 10, 50)
+gen.add("BWheel_Kp", int_t, 0, "wheel B P", 20, 10, 35)
+gen.add("BWheel_Kd", int_t, 0, "wheel B D", 20, 10, 50)
+gen.add("CWheel_Kp", int_t, 0, "wheel C P", 20, 10, 35)
+gen.add("CWheel_Kd", int_t, 0, "wheel C D", 20, 10, 50)
+gen.add("debugPID" , bool_t,0, "whether debug or not", True )
+     
+log_enum = gen.enum([ gen.const("info", int_t, 0, "log print flag:INFO"),
+                      gen.const("warn", int_t, 1, "log print flag:WARN"),
+                      gen.const("error", int_t, 2, "log print flag:ERROR")],
+                      "Set Log Level")
+gen.add("log_level", int_t, 0, "Set Log Level", 0, 0, 2, edit_method=log_enum)
+     
+exit(gen.generate(PACKAGE, "dynamic_tutorials", "Tutorials"))

src/robot_calibration/launch/pid.launch

@@ -0,0 +1,16 @@
+<!--
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description:
+    标定底盘pid的launch文件.
+  Author: jally
+  History:
+    20200420: init this file.
+-->
+<launch>
+   <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>
+   <node pkg="robot_calibration" type="dynamic_tutorials" name="dynamic_tutorials" output="screen" />
+   <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>

src/robot_calibration/package.xml

@@ -50,11 +50,16 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>rospy</build_depend>
+  <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/robot_calibration/src/pid_dynamic.cpp

@@ -0,0 +1,73 @@
+/********************************************************
+* Copyright: 2016-2020(c) ROS小课堂 www.corvin.cn
+* Filename: pid_dynamic.cpp
+* Author: corvin, jally
+* Descrition: 使用dynamic_reconfigure
+*   动态更新pid参数.
+* History:
+*   20200420: init this file.
+*******************************************************/
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <dynamic_reconfigure/server.h>
+#include <robot_calibration/TutorialsConfig.h>
+#include <ros_arduino_msgs/DynamicPID.h>
+
+using namespace std;
+
+int AWheel_Kp = 22;
+int AWheel_Kd = 34;
+int BWheel_Kp = 22;
+int BWheel_Kd = 34;
+int CWheel_Kp = 22;
+int CWheel_Kd = 34;
+
+void dynamic_callback(dynamic_tutorials::TutorialsConfig &config)
+{
+    ROS_INFO("Reconfigure Request: %d %d %d %d %d %d",
+        config.AWheel_Kp,
+        config.AWheel_Kd,
+        config.BWheel_Kp,
+        config.BWheel_Kd,
+        config.CWheel_Kp,
+        config.CWheel_Kd);
+
+	AWheel_Kp = config.AWheel_Kp;
+	AWheel_Kd = config.AWheel_Kd;
+	BWheel_Kp = config.BWheel_Kp;
+	BWheel_Kd = config.BWheel_Kd;
+	CWheel_Kp = config.CWheel_Kp;
+	CWheel_Kd = config.CWheel_Kd;
+
+        ros::NodeHandle nh;
+        ros::ServiceClient client = nh.serviceClient<ros_arduino_msgs::DynamicPID>("/mobilebase_arduino/dynamic_pid");
+        ros_arduino_msgs::DynamicPID srv;
+        srv.request.value1 = AWheel_Kp;
+	srv.request.value2 = AWheel_Kd;
+	srv.request.value3 = BWheel_Kp;
+	srv.request.value4 = BWheel_Kd;
+	srv.request.value5 = CWheel_Kp;
+	srv.request.value6 = CWheel_Kd;
+        //发布pid参数
+        if (client.call(srv))
+	{
+	}
+	else
+	{
+	    ROS_ERROR("Failed to call service DynamicPID");
+	}
+               
+}
+
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "dynamic_tutorials");
+	dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig> server;
+	dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig>::CallbackType callback;
+
+        callback = boost::bind(&dynamic_callback, _1);
+	server.setCallback(callback);
+	ros::spin();
+	return 0;
+}

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -25,6 +25,7 @@ add_service_files(FILES
                   GripperControl.srv
                   GripperAngle.srv
                   GetInfraredDistance.srv
+                  DynamicPID.srv
                  )
 
 generate_messages(

src/ros_arduino_bridge/ros_arduino_msgs/srv/DynamicPID.srv

@@ -0,0 +1,7 @@
+uint8 value1
+uint8 value2
+uint8 value3
+uint8 value4
+uint8 value5
+uint8 value6
+---

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -18,7 +18,6 @@ from ros_arduino_python.arduino_sensors import *
 from ros_arduino_python.arduino_driver import Arduino
 from ros_arduino_python.base_controller import BaseController
 
-
 class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
@@ -75,6 +74,9 @@ class ArduinoROS():
         # A service to control servo angle
         rospy.Service('~gripper_angle', GripperAngle, self.GripperAngleHandler)
 
+        # 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)
 
@@ -173,6 +175,12 @@ class ArduinoROS():
         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,
+                                   req.value5, req.value6, 0, 50 )
+        return DynamicPIDResponse()
+
     # Stop the robot
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")
@@ -189,7 +197,7 @@ class ArduinoROS():
             pass
         finally:
             os._exit(0)
-
+    
 
 if __name__ == '__main__':
     try:

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

@@ -268,6 +268,7 @@ class Arduino:
         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)
+        #rospy.loginfo("pid params now:"+cmd)
 
     def i2c_update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
                          BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -6,6 +6,7 @@
 import os
 import rospy
 import roslib; roslib.load_manifest('ros_arduino_python')
+import dynamic_reconfigure.client
 
 from math import sin, cos, pi, sqrt
 from geometry_msgs.msg import Quaternion, Twist
@@ -13,9 +14,9 @@ from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
 from std_msgs.msg import Int32
 
-
 """ Class to receive Twist commands and publish wheel Odometry data """
 class BaseController:
+    
     def __init__(self, is_use_serial, arduino, base_frame, name="base_controllers_node"):
         self.use_serial = is_use_serial
         self.arduino    = arduino
@@ -26,31 +27,34 @@ class BaseController:
         self.rate       = float(rospy.get_param("~base_controller_rate", 20))
         self.timeout    = rospy.get_param("~base_controller_timeout", 0.7)
 
+        self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
+	
         pid_params = dict()
-        pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.058)
-        pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
-        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", 11)
-        pid_params['gear_reduction']     = rospy.get_param("~gear_reduction", 46)
-        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 15)
-        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
-        pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
-        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)
-
-        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 15)
-        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
-        pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
-        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)
-
-        pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 15)
-        pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 16)
-        pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
-        pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
-
-        self.debugPID     = rospy.get_param('~debugPID', False)
+	pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.058)
+	pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
+	pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", 11)
+	pid_params['gear_reduction']     = rospy.get_param("~gear_reduction", 46)
+	pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 15)
+	pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
+	pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
+	pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)
+
+	pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 15)
+	pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
+	pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
+	pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)
+
+	pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 15)
+	pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 15)
+	pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
+	pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
+
         self.accel_limit  = rospy.get_param('~accel_limit', 0.05)
         self.linear_scale_correction  = rospy.get_param("~linear_scale_correction",  1.0)
         self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
 
+        #rospy.loginfo("pid_params_dict: "+pid_params)
+
         # Set up PID parameters and check for missing values
         self.setup_pid(pid_params)