Compute gains on on_activate

christophfroehlich · christophfroehlich · commit 46772344206e · 2023-11-19T13:43:39.000Z
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -724,7 +724,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
         params_.controller_plugin.c_str(), ex.what());
       return CallbackReturn::FAILURE;
     }
-    if (!traj_contr_->initialize(get_node()))
+    if (traj_contr_->initialize(get_node()) == false)
     {
       RCLCPP_FATAL(
         logger,
@@ -1011,6 +1011,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     cmd_timeout_ = 0.0;
   }
 
+  // update gains of PID controller
+  if (use_closed_loop_pid_adapter_)
+  {
+    trajectory_msgs::msg::JointTrajectory traj;
+    traj.points.push_back(state_current_);
+    traj_contr_->computeGains(traj);
+  }
+
   return CallbackReturn::SUCCESS;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -724,7 +724,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(`
`724`	`724`	`params_.controller_plugin.c_str(), ex.what());`
`725`	`725`	`return CallbackReturn::FAILURE;`
`726`	`726`	`}`
`727`		`- if (!traj_contr_->initialize(get_node()))`
	`727`	`+ if (traj_contr_->initialize(get_node()) == false)`
`728`	`728`	`{`
`729`	`729`	`RCLCPP_FATAL(`
`730`	`730`	`logger,`
`@@ -1011,6 +1011,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(`
`1011`	`1011`	`cmd_timeout_ = 0.0;`
`1012`	`1012`	`}`
`1013`	`1013`
	`1014`	`+ // update gains of PID controller`
	`1015`	`+ if (use_closed_loop_pid_adapter_)`
	`1016`	`+ {`
	`1017`	`+ trajectory_msgs::msg::JointTrajectory traj;`
	`1018`	`+ traj.points.push_back(state_current_);`
	`1019`	`+ traj_contr_->computeGains(traj);`
	`1020`	`+ }`
	`1021`	`+`
`1014`	`1022`	`return CallbackReturn::SUCCESS;`
`1015`	`1023`	`}`
`1016`	`1024`