Call activate() of traj_contr_

christophfroehlich · christophfroehlich · commit 49274359f015 · 2023-12-06T13:02:11.000Z
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -220,7 +220,7 @@ controller_interface::return_type JointTrajectoryController::update(
       }
       if (traj_contr_)
       {
-        // set start time of trajectory to traj_contr_
+        // switch RT buffer of traj_contr_
         traj_contr_->start();
       }
     }
@@ -1041,9 +1041,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     cmd_timeout_ = 0.0;
   }
 
-  if (traj_contr_)
+  // activate traj_contr_, e.g., update gains
+  if (traj_contr_ && traj_contr_->activate() == false)
   {
-    traj_contr_->activate();
+    RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
+    return CallbackReturn::ERROR;
   }
 
   return CallbackReturn::SUCCESS;

Original file line number	Diff line number	Diff line change
`@@ -220,7 +220,7 @@ controller_interface::return_type JointTrajectoryController::update(`
`220`	`220`	`}`
`221`	`221`	`if (traj_contr_)`
`222`	`222`	`{`
`223`		`- // set start time of trajectory to traj_contr_`
	`223`	`+ // switch RT buffer of traj_contr_`
`224`	`224`	`traj_contr_->start();`
`225`	`225`	`}`
`226`	`226`	`}`
`@@ -1041,9 +1041,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(`
`1041`	`1041`	`cmd_timeout_ = 0.0;`
`1042`	`1042`	`}`
`1043`	`1043`
`1044`		`- if (traj_contr_)`
	`1044`	`+ // activate traj_contr_, e.g., update gains`
	`1045`	`+ if (traj_contr_ && traj_contr_->activate() == false)`
`1045`	`1046`	`{`
`1046`		`- traj_contr_->activate();`
	`1047`	`+ RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");`
	`1048`	`+ return CallbackReturn::ERROR;`
`1047`	`1049`	`}`
`1048`	`1050`
`1049`	`1051`	`return CallbackReturn::SUCCESS;`