Made changes related to setpid values using service call

Author: hnsheth07

external/dynamixel-workbench-msgs/dynamixel_workbench_msgs/CMakeLists.txt

@@ -47,6 +47,7 @@ add_service_files(
   WheelCommand.srv
   GetDynamixelInfo.srv
   DynamixelCommand.srv
+  SetPID.srv
 )
 
 generate_messages(

external/dynamixel-workbench-msgs/dynamixel_workbench_msgs/srv/SetPID.srv

@@ -0,0 +1,8 @@
+# This message is used to set PID for different joints
+
+uint8 joint_num #Possible values: joint_1_id : 1, joint_2_1_id: 2, joint_2_2_id: 3, joint_3_id: 4 joint_4_id: 5, joint_5_id: 6 , gripper: 7, pan: 8, tilt: 9
+uint8 P
+uint8 I
+uint8 D
+---
+bool result

external/dynamixel-workbench/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/position_control.h

@@ -31,6 +31,7 @@
 #include <dynamixel_workbench_toolbox/dynamixel_workbench.h>
 #include <dynamixel_workbench_msgs/DynamixelStateList.h>
 #include <dynamixel_workbench_msgs/JointCommand.h>
+#include <dynamixel_workbench_msgs/SetPID.h>
 
 // Probably should make these constexpr
 #define GRIPPER_OPEN_MOTOR_POS -1.147412
@@ -66,6 +67,7 @@ class PositionControl
 
   // ROS Service Server
   ros::ServiceServer joint_command_server_;
+  ros::ServiceServer set_pid_server_;
 
   // ROS Service Client
 
@@ -128,6 +130,7 @@ class PositionControl
   void initServer();
   // bool jointCommandMsgCallback(dynamixel_workbench_msgs::JointCommand::Request &req,
                                // dynamixel_workbench_msgs::JointCommand::Response &res);
+  bool setPIDMsgCallback(dynamixel_workbench_msgs::SetPID::Request &req,dynamixel_workbench_msgs::SetPID::Response &res);
   void goalJointPositionCallback(const sensor_msgs::JointState::ConstPtr &msg);
   void gripperCloseCallback(const std_msgs::Empty::ConstPtr &msg);
   void gripperOpenCallback(const std_msgs::Empty::ConstPtr &msg);

external/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp

@@ -280,6 +280,7 @@ void PositionControl::initSubscriber()
 void PositionControl::initServer()
 {
     // joint_command_server_ = node_handle_.advertiseService("joint_command", &PositionControl::jointCommandMsgCallback, this);
+    set_pid_server_ = node_handle_.advertiseService("SetPID", &PositionControl::setPIDMsgCallback, this);
 }
 
 void PositionControl::dynamixelStatePublish()
@@ -512,29 +513,15 @@ void PositionControl::controlLoop()
     cameraStatePublish();
 }
 
-// bool PositionControl::jointCommandMsgCallback(dynamixel_workbench_msgs::JointCommand::Request &req,
-//                                               dynamixel_workbench_msgs::JointCommand::Response &res)
-// {
-//   int32_t goal_position = 0;
-//   int32_t present_position = 0;
-
-//   if (req.unit == "rad")
-//   {
-//     goal_position = dxl_wb_->convertRadian2Value(req.id, req.goal_position);
-//   }
-//   else if (req.unit == "raw")
-//   {
-//     goal_position = req.goal_position;
-//   }
-//   else
-//   {
-//     goal_position = req.goal_position;
-//   }
-
-//   bool ret = dxl_wb_->goalPosition(req.id, goal_position);
+bool PositionControl::setPIDMsgCallback(dynamixel_workbench_msgs::SetPID::Request &req,dynamixel_workbench_msgs::SetPID::Response &res)
+{
+    int joint_num = req.joint_num;
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_P_Gain", req.P);
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_I_Gain", req.I);
+    dxl_wb_arm_->itemWrite(dxl_id_arm_[joint_num], "Position_D_Gain", req.D);
 
-//   res.result = ret;
-// }
+    res.result = true;
+}
 
 void PositionControl::goalJointPositionCallback(const sensor_msgs::JointState::ConstPtr& msg)
 {