diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 286e4cf82a..eba26dc7ec 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -103,8 +103,6 @@ class TestTrajectoryActions : public ::testing::Test rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); traj_lifecycle_node_->set_parameter(stopped_velocity_parameters); - goal_options_.goal_response_callback = - std::bind(&TestTrajectoryActions::common_goal_response, this, _1); goal_options_.result_callback = std::bind(&TestTrajectoryActions::common_result_response, this, _1); goal_options_.feedback_callback = nullptr; @@ -203,7 +201,7 @@ class TestTrajectoryActions : public ::testing::Test using GoalHandle = rclcpp_action::ClientGoalHandle; using GoalOptions = rclcpp_action::Client::SendGoalOptions; - bool sendActionGoal( + std::shared_future sendActionGoal( const std::vector & points, double timeout, const GoalOptions & opt) @@ -213,8 +211,7 @@ class TestTrajectoryActions : public ::testing::Test goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; - auto goal_handle_future = action_client_->async_send_goal(goal_msg, opt); - return true; + return action_client_->async_send_goal(goal_msg, opt); } std::string controller_name_ = "test_joint_trajectory_actions"; @@ -233,7 +230,6 @@ class TestTrajectoryActions : public ::testing::Test rclcpp_action::Client::SharedPtr action_client_; rclcpp_action::ResultCode common_resultcode_ = rclcpp_action::ResultCode::UNKNOWN; - bool common_goal_accepted_ = false; int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; bool setup_executor_ = false; @@ -246,21 +242,6 @@ class TestTrajectoryActions : public ::testing::Test GoalOptions goal_options_; public: - void common_goal_response(std::shared_future future) - { - RCLCPP_DEBUG( - node_->get_logger(), "common_goal_response time: %f", - rclcpp::Clock().now().seconds()); - const auto goal_handle = future.get(); - if (!goal_handle) { - common_goal_accepted_ = false; - RCLCPP_DEBUG(node_->get_logger(), "Goal rejected"); - } else { - common_goal_accepted_ = true; - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted"); - } - } - void common_result_response(const GoalHandle::WrappedResult & result) { RCLCPP_DEBUG( @@ -288,6 +269,7 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -301,11 +283,11 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) { points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ(1.0, joint1_pos_handle_->get_value()); @@ -327,6 +309,7 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -348,12 +331,12 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) { point2.positions[2] = 9.0; points.push_back(point2); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); EXPECT_TRUE(feedback_recv); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_NEAR(7.0, joint1_pos_handle_->get_value(), COMMON_THRESHOLD); @@ -370,6 +353,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -382,11 +366,11 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) { point.positions[2] = 3.0; points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, @@ -416,6 +400,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -437,12 +422,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) { point2.positions[2] = 9.0; points.push_back(point2); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); EXPECT_TRUE(feedback_recv); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, @@ -463,6 +448,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) { SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -475,11 +461,11 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) { point.positions[2] = 6.0; points.push_back(point); - sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 1.0, goal_options_); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, @@ -490,6 +476,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) { SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -508,15 +495,15 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) { goal_msg.trajectory.points = points; // send and wait for half a second before cancel - const auto goal_handle_future = action_client_->async_send_goal(goal_msg, goal_options_); + gh_future = action_client_->async_send_goal(goal_msg, goal_options_); std::this_thread::sleep_for(std::chrono::milliseconds(500)); - const auto goal_handle = goal_handle_future.get(); + const auto goal_handle = gh_future.get(); action_client_->async_cancel_goal(goal_handle); } controller_hw_thread_.join(); - EXPECT_TRUE(common_goal_accepted_); + EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, common_resultcode_); EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL,