Compute gains on on_activate

christophfroehlich · christophfroehlich · commit ce28eb36b4b0 · 2023-10-15T17:13:19.000Z
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -738,7 +738,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,
@@ -1023,6 +1023,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   }
   rt_is_holding_.writeFromNonRT(true);
 
+  // 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
`@@ -738,7 +738,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(`
`738`	`738`	`params_.controller_plugin.c_str(), ex.what());`
`739`	`739`	`return CallbackReturn::FAILURE;`
`740`	`740`	`}`
`741`		`- if (!traj_contr_->initialize(get_node()))`
	`741`	`+ if (traj_contr_->initialize(get_node()) == false)`
`742`	`742`	`{`
`743`	`743`	`RCLCPP_FATAL(`
`744`	`744`	`logger,`
`@@ -1023,6 +1023,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(`
`1023`	`1023`	`}`
`1024`	`1024`	`rt_is_holding_.writeFromNonRT(true);`
`1025`	`1025`
	`1026`	`+ // update gains of PID controller`
	`1027`	`+ if (use_closed_loop_pid_adapter_)`
	`1028`	`+ {`
	`1029`	`+ trajectory_msgs::msg::JointTrajectory traj;`
	`1030`	`+ traj.points.push_back(state_current_);`
	`1031`	`+ traj_contr_->computeGains(traj);`
	`1032`	`+ }`
	`1033`	`+`
`1026`	`1034`	`return CallbackReturn::SUCCESS;`
`1027`	`1035`	`}`
`1028`	`1036`