diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 89195279e0..65e756a4d2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -61,8 +61,6 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUp() { TrajectoryControllerTest::SetUp(); - 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; @@ -153,7 +151,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest 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) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; @@ -161,13 +159,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest 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); } 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; @@ -180,23 +176,6 @@ class TestTrajectoryActions : public TrajectoryControllerTest 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( @@ -225,6 +204,7 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -238,11 +218,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, joint_pos_[0]); @@ -262,6 +242,7 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -283,12 +264,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, joint_pos_[0], COMMON_THRESHOLD); @@ -307,6 +288,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) SetUpExecutor(params); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -319,11 +301,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, common_action_result_code_); @@ -351,6 +333,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr) { feedback_recv = true; }; + std::shared_future gh_future; // send goal with multiple points { std::vector points; @@ -372,12 +355,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, common_action_result_code_); @@ -399,6 +382,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) SetUpExecutor(params); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -411,11 +395,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, @@ -427,6 +411,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) SetUpExecutor(); SetUpControllerHardware(); + std::shared_future gh_future; // send goal { std::vector points; @@ -445,15 +430,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, common_action_result_code_);