新增更新pid参数的自定义消息,更新自定义服务方式更新pid参数的服务定义

src/CMakeLists.txt

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

src/robot_calibration/src/pid_dynamic.cpp

@@ -25,12 +25,12 @@ 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);
+    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;
@@ -39,25 +39,25 @@ void dynamic_callback(dynamic_tutorials::TutorialsConfig &config)
 	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))
+    ros::NodeHandle nh;
+    ros::ServiceClient client = nh.serviceClient<ros_arduino_msgs::DynamicPID>("/mobilebase_arduino/dynamic_pid");
+    ros_arduino_msgs::DynamicPID srv;
+    srv.request.A_kp = AWheel_Kp;
+	srv.request.A_kd = AWheel_Kd;
+	srv.request.B_kp = BWheel_Kp;
+	srv.request.B_kd = BWheel_Kd;
+	srv.request.C_kp = CWheel_Kp;
+	srv.request.C_kd = CWheel_Kd;
+
+    //发布pid参数
+    if (client.call(srv))
 	{
-            ROS_INFO("Call service DynamicPID sucessfully.");
+        ROS_INFO("Call service DynamicPID sucessfully.");
 	}
 	else
 	{
 	    ROS_ERROR("Failed to call service DynamicPID");
 	}
-               
 }
 
 
@@ -67,8 +67,9 @@ int main(int argc, char **argv)
 	dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig> server;
 	dynamic_reconfigure::Server<dynamic_tutorials::TutorialsConfig>::CallbackType callback;
 
-        callback = boost::bind(&dynamic_callback, _1);
+    callback = boost::bind(&dynamic_callback, _1);
 	server.setCallback(callback);
 	ros::spin();
+
 	return 0;
 }

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -10,6 +10,7 @@ add_message_files(FILES
                   Digital.msg
                   SensorState.msg
                   InfraredSensors.msg
+                  UpdatePID.msg
                  )
 
 add_service_files(FILES

src/ros_arduino_bridge/ros_arduino_msgs/msg/UpdatePID.msg

@@ -0,0 +1,7 @@
+# Define Motors kp,kd params
+uint8 A_kp
+uint8 A_kd
+uint8 B_kp
+uint8 B_kd
+uint8 C_kp
+uint8 C_kd

src/ros_arduino_bridge/ros_arduino_msgs/srv/DynamicPID.srv

@@ -1,7 +1,7 @@
-uint8 value1
-uint8 value2
-uint8 value3
-uint8 value4
-uint8 value5
-uint8 value6
+uint8 A_kp
+uint8 A_kd
+uint8 B_kp
+uint8 B_kd
+uint8 C_kp
+uint8 C_kd
 ---

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -48,17 +48,17 @@ accel_limit: 0.05
 AWheel_Kp: 22
 AWheel_Kd: 30
 AWheel_Ki: 0
-AWheel_Ko: 50
+AWheel_Ko: 10
 
 BWheel_Kp: 22
 BWheel_Kd: 30
 BWheel_Ki: 0
-BWheel_Ko: 50
+BWheel_Ko: 10
 
 CWheel_Kp: 22
 CWheel_Kd: 30
 CWheel_Ki: 0
-CWheel_Ko: 50
+CWheel_Ko: 10
 
 # Sensor definitions (case sensitive!):
 sensors: {

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -178,9 +178,9 @@ class ArduinoROS():
         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 )
+        self.controller.update_pid(req.A_kp, req.A_kd, 0, 10,
+                                   req.B_kp, req.B_kd, 0, 10,
+                                   req.C_kp, req.C_kd, 0, 10 )
         return DynamicPIDResponse()
 
     # Stop the robot arduino connect

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

@@ -358,7 +358,6 @@ class Arduino:
             return None
 
 
-
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port
         '''