Reset trajectory at deactivation and test it

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -987,6 +987,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
 
   subscriber_is_active_ = false;
 
+  traj_point_active_ptr_ = nullptr;
+  traj_external_point_ptr_.reset();
+  traj_home_point_ptr_.reset();
+  traj_msg_home_ptr_.reset();
+
   return CallbackReturn::SUCCESS;
 }
 

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -285,44 +285,38 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
   // first update
   traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
 
-  // wait so controller process the second point when deactivated
+  // wait for reaching the first point
+  // controller would process the second point when deactivated below
   traj_controller_->update(
     rclcpp::Time(static_cast<uint64_t>(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15));
-  // deactivated
-  state = traj_controller_->get_node()->deactivate();
-  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
-
-  const auto allowed_delta = 0.05;
+  EXPECT_TRUE(traj_controller_->has_active_traj());
   if (traj_controller_->has_position_command_interface())
   {
-    EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta);
-    EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta);
-    EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta);
-  }
-
-  if (traj_controller_->has_velocity_command_interface())
-  {
-    EXPECT_LE(0.0, joint_vel_[0]);
-    EXPECT_LE(0.0, joint_vel_[1]);
-    EXPECT_LE(0.0, joint_vel_[2]);
+    EXPECT_NEAR(points.at(0).at(0), joint_pos_[0], COMMON_THRESHOLD);
+    EXPECT_NEAR(points.at(0).at(1), joint_pos_[1], COMMON_THRESHOLD);
+    EXPECT_NEAR(points.at(0).at(2), joint_pos_[2], COMMON_THRESHOLD);
   }
 
-  if (traj_controller_->has_effort_command_interface())
-  {
-    EXPECT_LE(0.0, joint_eff_[0]);
-    EXPECT_LE(0.0, joint_eff_[1]);
-    EXPECT_LE(0.0, joint_eff_[2]);
-  }
+  // deactivate
+  std::vector<double> deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]};
+  state = traj_controller_->get_node()->deactivate();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+  // it should be holding the current point
+  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
+  expectHoldingPointDeactivated(deactivated_positions);
 
-  // reactivated
-  // wait so controller process the third point when reactivated
+  // reactivate
+  // wait so controller would have processed the third point when reactivated -> but it shouldn't
   std::this_thread::sleep_for(std::chrono::milliseconds(3000));
-  ActivateTrajectoryController();
+
+  ActivateTrajectoryController(false, deactivated_positions);
   state = traj_controller_->get_state();
   ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE);
 
-  // TODO(christophfroehlich) add test if there is no active trajectory after
-  // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController)
+  // it should still be holding the position at time of deactivation
+  // i.e., active but trivial trajectory (one point only)
+  traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1));
+  expectHoldingPoint(deactivated_positions);
 
   executor.cancel();
 }

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -232,7 +232,9 @@ class TrajectoryControllerTest : public ::testing::Test
     ActivateTrajectoryController(separate_cmd_and_state_values);
   }
 
-  void ActivateTrajectoryController(bool separate_cmd_and_state_values = false)
+  void ActivateTrajectoryController(
+    bool separate_cmd_and_state_values = false,
+    const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS)
   {
     std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
     std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
@@ -266,14 +268,14 @@ class TrajectoryControllerTest : public ::testing::Test
 
       // Add to export lists and set initial values
       cmd_interfaces.emplace_back(pos_cmd_interfaces_.back());
-      cmd_interfaces.back().set_value(INITIAL_POS_JOINTS[i]);
+      cmd_interfaces.back().set_value(initial_pos_joints[i]);
       cmd_interfaces.emplace_back(vel_cmd_interfaces_.back());
       cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]);
       cmd_interfaces.emplace_back(acc_cmd_interfaces_.back());
       cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]);
       cmd_interfaces.emplace_back(eff_cmd_interfaces_.back());
       cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]);
-      joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
+      joint_state_pos_[i] = initial_pos_joints[i];
       joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
       joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
       state_interfaces.emplace_back(pos_state_interfaces_.back());
@@ -434,17 +436,51 @@ class TrajectoryControllerTest : public ::testing::Test
     return state_msg_;
   }
 
-  void expectHoldingPoint(std::vector<double> points)
+  void expectHoldingPoint(std::vector<double> point)
   {
     // it should be holding the given point
     // i.e., active but trivial trajectory (one point only)
     EXPECT_TRUE(traj_controller_->has_trivial_traj());
 
     if (traj_controller_->has_position_command_interface())
     {
-      EXPECT_NEAR(points.at(0), joint_pos_[0], COMMON_THRESHOLD);
-      EXPECT_NEAR(points.at(1), joint_pos_[1], COMMON_THRESHOLD);
-      EXPECT_NEAR(points.at(2), joint_pos_[2], COMMON_THRESHOLD);
+      EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD);
+      EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD);
+      EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD);
+    }
+
+    if (traj_controller_->has_velocity_command_interface())
+    {
+      EXPECT_EQ(0.0, joint_vel_[0]);
+      EXPECT_EQ(0.0, joint_vel_[1]);
+      EXPECT_EQ(0.0, joint_vel_[2]);
+    }
+
+    if (traj_controller_->has_acceleration_command_interface())
+    {
+      EXPECT_EQ(0.0, joint_acc_[0]);
+      EXPECT_EQ(0.0, joint_acc_[1]);
+      EXPECT_EQ(0.0, joint_acc_[2]);
+    }
+
+    if (traj_controller_->has_effort_command_interface())
+    {
+      EXPECT_EQ(0.0, joint_eff_[0]);
+      EXPECT_EQ(0.0, joint_eff_[1]);
+      EXPECT_EQ(0.0, joint_eff_[2]);
+    }
+  }
+
+  void expectHoldingPointDeactivated(std::vector<double> point)
+  {
+    // it should be holding the given point, but no active trajectory
+    EXPECT_FALSE(traj_controller_->has_active_traj());
+
+    if (traj_controller_->has_position_command_interface())
+    {
+      EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD);
+      EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD);
+      EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD);
     }
 
     if (traj_controller_->has_velocity_command_interface())