Merge lp:~shadowrobot/sr-ros-interface/Feature_update_controller_params into lp:sr-ros-interface

[Toni Oliver]

Update params in param server when changing controller parameters. Solved the waiting calib problem.

Update params in param server when changing controller parameters. Solved the waiting calib problem.

[markpitchless (markpitchless)]

http://reviewboard.shadow.local/r/106/

=== modified file 'sr_mechanism_controllers/src/srh_fake_joint_calibration_controller.cpp'
--- sr_mechanism_controllers/src/srh_fake_joint_calibration_controller.cpp	2012-08-20 07:44:40 +0000
+++ sr_mechanism_controllers/src/srh_fake_joint_calibration_controller.cpp	2012-11-08 11:52:35 +0000
@@ -118,6 +118,8 @@
       initialize_pids();
       joint_->calibrated_ = true;
       state_ = CALIBRATED;
+      //We add the following line to delay for some time the first publish and allow the correct initialization of the subscribers in calibrate.py
+      last_publish_time_ = robot_->getTime();
       break;
     case CALIBRATED:
       if (pub_calibrated_)

=== modified file 'sr_mechanism_controllers/src/srh_joint_effort_controller.cpp'
--- sr_mechanism_controllers/src/srh_joint_effort_controller.cpp	2012-10-31 06:48:22 +0000
+++ sr_mechanism_controllers/src/srh_joint_effort_controller.cpp	2012-11-08 11:52:35 +0000
@@ -140,6 +140,10 @@
     max_force_demand = req.max_force;
     friction_deadband = req.friction_deadband;
 
+    //Setting the new parameters in the parameter server
+    node_.setParam("max_force", max_force_demand);
+    node_.setParam("friction_deadband", friction_deadband);
+
     return true;
   }
 

=== modified file 'sr_mechanism_controllers/src/srh_joint_position_controller.cpp'
--- sr_mechanism_controllers/src/srh_joint_position_controller.cpp	2012-10-29 14:05:51 +0000
+++ sr_mechanism_controllers/src/srh_joint_position_controller.cpp	2012-11-08 11:52:35 +0000
@@ -165,6 +165,15 @@
     friction_deadband = req.friction_deadband;
     position_deadband = req.deadband;
 
+    //Setting the new parameters in the parameter server
+    node_.setParam("pid/p", req.p);
+    node_.setParam("pid/i", req.i);
+    node_.setParam("pid/d", req.d);
+    node_.setParam("pid/i_clamp", req.i_clamp);
+    node_.setParam("pid/max_force", max_force_demand);
+    node_.setParam("pid/position_deadband", position_deadband);
+    node_.setParam("pid/friction_deadband", friction_deadband);
+
     return true;
   }
 

=== modified file 'sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp'
--- sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp	2012-11-02 12:26:26 +0000
+++ sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp	2012-11-08 11:52:35 +0000
@@ -159,6 +159,16 @@
     friction_deadband = req.friction_deadband;
     velocity_deadband = req.deadband;
 
+    //Setting the new parameters in the parameter server
+    node_.setParam("pid/p", req.p);
+    node_.setParam("pid/i", req.i);
+    node_.setParam("pid/d", req.d);
+    node_.setParam("pid/i_clamp", req.i_clamp);
+
+    node_.setParam("pid/max_force", max_force_demand);
+    node_.setParam("pid/velocity_deadband", velocity_deadband);
+    node_.setParam("pid/friction_deadband", friction_deadband);
+
     return true;
   }
 

=== modified file 'sr_mechanism_controllers/src/srh_mixed_position_velocity_controller.cpp'
--- sr_mechanism_controllers/src/srh_mixed_position_velocity_controller.cpp	2012-10-29 14:05:51 +0000
+++ sr_mechanism_controllers/src/srh_mixed_position_velocity_controller.cpp	2012-11-08 11:52:35 +0000
@@ -196,6 +196,25 @@
     min_velocity_ = req.min_velocity;
     max_velocity_ = req.max_velocity;
 
+    //Setting the new parameters in the parameter server
+    node_.setParam("position_pid/p", req.position_p);
+    node_.setParam("position_pid/i", req.position_i);
+    node_.setParam("position_pid/d", req.position_d);
+    node_.setParam("position_pid/i_clamp", req.position_i_clamp);
+
+    node_.setParam("velocity_pid/p", req.velocity_p);
+    node_.setParam("velocity_pid/i", req.velocity_i);
+    node_.setParam("velocity_pid/d", req.velocity_d);
+    node_.setParam("velocity_pid/i_clamp", req.velocity_i_clamp);
+
+    node_.setParam("position_pid/min_velocity", min_velocity_);
+    node_.setParam("position_pid/max_velocity", max_velocity_);
+    node_.setParam("position_pid/position_deadband", position_deadband);
+
+    node_.setParam("velocity_pid/friction_deadband", friction_deadband);
+    node_.setParam("velocity_pid/max_force", max_force_demand);
+    node_.setParam("motor_min_force_threshold", motor_min_force_threshold);
+
     return true;
   }