Skip to content

Change test to work with Foxy and posterior action API #111

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
v-lopez wants to merge 1 commit into from
Closed

Change test to work with Foxy and posterior action API #111

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 21 additions & 34 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -203,7 +201,7 @@ class TestTrajectoryActions : public ::testing::Test
using GoalHandle = rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>;
using GoalOptions = rclcpp_action::Client<FollowJointTrajectoryMsg>::SendGoalOptions;

bool sendActionGoal(
std::shared_future<typename GoalHandle::SharedPtr> sendActionGoal(
const std::vector<JointTrajectoryPoint> & points,
double timeout,
const GoalOptions & opt)
Expand All @@ -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";
Expand All @@ -233,7 +230,6 @@ class TestTrajectoryActions : public ::testing::Test

rclcpp_action::Client<FollowJointTrajectoryMsg>::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;
Expand All @@ -246,21 +242,6 @@ class TestTrajectoryActions : public ::testing::Test
GoalOptions goal_options_;

public:
void common_goal_response(std::shared_future<GoalHandle::SharedPtr> 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(
Expand Down Expand Up @@ -288,6 +269,7 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal) {
SetUpExecutor();
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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());
Expand All @@ -327,6 +309,7 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal) {
feedback_recv = true;
};

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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);
Expand All @@ -370,6 +353,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) {
SetUpExecutor();
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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,
Expand Down Expand Up @@ -416,6 +400,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) {
feedback_recv = true;
};

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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,
Expand All @@ -463,6 +448,7 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail) {
SetUpExecutor();
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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,
Expand All @@ -490,6 +476,7 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position) {
SetUpExecutor();
SetUpControllerHardware();

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
Expand All @@ -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,
Expand Down