36#include <condition_variable>
37#include <gmock/gmock.h>
38#include <gtest/gtest.h>
45#include <actionlib/client/simple_action_client.h>
50#include <moveit_msgs/Constraints.h>
51#include <moveit_msgs/GetMotionPlan.h>
52#include <moveit_msgs/JointConstraint.h>
54#include <sensor_msgs/JointState.h>
62#include <moveit_msgs/MoveGroupSequenceAction.h>
64static constexpr double WAIT_FOR_RESULT_TIME_OUT{ 5. };
65static constexpr double TIME_BEFORE_CANCEL_GOAL{ 1.0 };
66static constexpr double WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10. };
89 MOCK_METHOD1(feedback_callback,
void(
const moveit_msgs::msg::MoveGroupSequenceFeedbackConstPtr& feedback));
90 MOCK_METHOD2(done_callback,
void(
const actionlib::SimpleClientGoalState& state,
91 const moveit_msgs::msg::MoveGroupSequenceResultConstPtr& result));
94 ros::NodeHandle
ph_{
"~" };
96 std::shared_ptr<moveit::planning_interface::MoveGroupInterface>
move_group_;
120 ASSERT_NE(
nullptr,
data_loader_) <<
"Failed to load test data by provider.";
123 ASSERT_TRUE(
ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) <<
"Action server is not active.";
153 Sequence seq{ data_loader_->getSequence(
"ComplexSequence") };
155 moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
158 ac_.sendGoal(seq_goal);
160 ros::Duration(TIME_BEFORE_CANCEL_GOAL).sleep();
163 ac_.waitForResult(ros::Duration(WAIT_FOR_RESULT_TIME_OUT));
165 moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
166 EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
167 <<
"Error code should be preempted.";
172 ros::init(argc, argv,
"integrationtest_sequence_action_preemption");
175 ros::AsyncSpinner spinner{ 1 };
178 testing::InitGoogleTest(&argc, argv);
179 return RUN_ALL_TESTS();
robot_model_loader::RobotModelLoader model_loader_
actionlib::SimpleActionClient< moveit_msgs::msg::MoveGroupSequenceAction > ac_
MOCK_METHOD1(feedback_callback, void(const moveit_msgs::msg::MoveGroupSequenceFeedbackConstPtr &feedback))
robot_model::RobotModelPtr robot_model_
JointConfiguration start_config
The configuration at which the robot stays at the beginning of each test.
MOCK_METHOD0(active_callback, void())
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > move_group_
double joint_position_tolerance_
std::string test_data_file_name_
TestdataLoaderUPtr data_loader_
MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState &state, const moveit_msgs::msg::MoveGroupSequenceResultConstPtr &result))
Class to define a robot configuration in space with the help of joint values.
const std::vector< double > getJoints() const
moveit::core::RobotState toRobotState() const
std::string getGroupName() const
Data class storing all information regarding a Sequence command.
moveit_msgs::msg::MotionSequenceRequest toRequest() const
const moveit::core::RobotModelPtr & getModel() const
Get the constructed planning_models::RobotModel.
Test class that allows the handling of asynchronous test objects.
const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance")
int main(int argc, char **argv)
const std::string GROUP_NAME("group_name")
const std::string SERVER_IDLE_EVENT
const std::string GOAL_SUCCEEDED_EVENT
const std::string TEST_DATA_FILE_NAME("testdata_file_name")
TEST_F(IntegrationTestSequenceAction, TestCancellingOfGoal)
Tests that goal can be cancelled.
const std::string SEQUENCE_ACTION_NAME("/sequence_move_group")
::testing::AssertionResult isAtExpectedPosition(const moveit::core::RobotState &expected, const moveit::core::RobotState &actual, const double epsilon, const std::string &group_name="")
std::unique_ptr< TestdataLoader > TestdataLoaderUPtr