integrationtest_sequence_action.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
#include <chrono>
#include <condition_variable>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <iostream>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <thread>
 
#include <actionlib/client/simple_action_client.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/GetMotionPlan.h>
#include <moveit_msgs/JointConstraint.h>
#include <ros/ros.h>
 
#include <pilz_industrial_motion_planner_testutils/async_test.h>
 
#include <pilz_industrial_motion_planner_testutils/checks.h>
#include <pilz_industrial_motion_planner_testutils/sequence.h>
#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
 
#include <moveit_msgs/MoveGroupSequenceAction.h>
 
static constexpr int WAIT_FOR_ACTION_SERVER_TIME_OUT{ 10 };  // seconds
 
const std::string SEQUENCE_ACTION_NAME("/sequence_move_group");
 
// Parameters from the node
const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance");
 
// events for callback tests
const std::string GOAL_SUCCEEDED_EVENT = "GOAL_SUCCEEDED";
const std::string SERVER_IDLE_EVENT = "SERVER_IDLE";
 
const std::string TEST_DATA_FILE_NAME("testdata_file_name");
const std::string GROUP_NAME("group_name");
 
using namespace pilz_industrial_motion_planner_testutils;
 
class IntegrationTestSequenceAction : public testing::Test, public testing::AsyncTest
{
protected:
  void SetUp() override;
 
public:
  MOCK_METHOD0(active_callback, void());
  MOCK_METHOD1(feedback_callback, void(const moveit_msgs::msg::MoveGroupSequenceFeedbackConstPtr& feedback));
  MOCK_METHOD2(done_callback, void(const actionlib::SimpleClientGoalState& state,
                                   const moveit_msgs::msg::MoveGroupSequenceResultConstPtr& result));
 
protected:
  ros::NodeHandle ph_{ "~" };
  actionlib::SimpleActionClient<moveit_msgs::msg::MoveGroupSequenceAction> ac_{ ph_, SEQUENCE_ACTION_NAME, true };
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
 
  robot_model_loader::RobotModelLoader model_loader_;
  robot_model::RobotModelPtr robot_model_;
  double joint_position_tolerance_;
 
  std::string test_data_file_name_;
  std::string group_name_;
  TestdataLoaderUPtr data_loader_;
 
  JointConfiguration start_config;
};
 
void IntegrationTestSequenceAction::SetUp()
{
  // get necessary parameters
  ASSERT_TRUE(ph_.getParam(JOINT_POSITION_TOLERANCE, joint_position_tolerance_));
  ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));
  ph_.param<std::string>(GROUP_NAME, group_name_, "manipulator");
 
  robot_model_ = model_loader_.getModel();
 
  data_loader_ = std::make_unique<XmlTestdataLoader>(test_data_file_name_, robot_model_);
  ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider.";
 
  // wait for action server
  ASSERT_TRUE(ac_.waitForServer(ros::Duration(WAIT_FOR_ACTION_SERVER_TIME_OUT))) << "Action server is not active.";
 
  // move to default position
  start_config = data_loader_->getJoints("ZeroPose", group_name_);
  robot_state::RobotState robot_state{ start_config.toRobotState() };
 
  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(start_config.getGroupName());
  move_group_->setPlannerId("PTP");
  move_group_->setGoalTolerance(joint_position_tolerance_);
  move_group_->setJointValueTarget(robot_state);
  move_group_->move();
 
  ASSERT_TRUE(
      isAtExpectedPosition(robot_state, *(move_group_->getCurrentState()), joint_position_tolerance_, group_name_));
}
 
TEST_F(IntegrationTestSequenceAction, TestSendingOfEmptySequence)
{
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
      << "Execution of sequence failed.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestDifferingGroupNames)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  MotionCmd& cmd{ seq.getCmd(1) };
  cmd.setPlanningGroup("WrongGroupName");
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME)
      << "Incorrect error code.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadius)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  seq.setBlendRadius(0, -1.0);
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
 
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN)
      << "Incorrect error code.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestOverlappingBlendRadii)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  seq.setBlendRadius(0, 10 * seq.getBlendRadius(0));
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
 
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN)
      << "Incorrect error code";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestTooLargeBlendRadii)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  seq.erase(2, seq.size());
  seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2));
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
 
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::FAILURE) << "Incorrect error code";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestInvalidCmd)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  // Erase certain command to invalid command following the command in sequence.
  seq.erase(3, 4);
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_NE(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestInvalidLinkName)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  seq.setAllBlendRadiiToZero();
 
  // Invalidate link name
  CircInterimCart& circ{ seq.getCmd<CircInterimCart>(1) };
  circ.getGoalConfiguration().setLinkName("InvalidLinkName");
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_NE(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
//*******************************************************
//*** matcher for callback functions of action server ***
//*******************************************************
MATCHER_P(FeedbackStateEq, state, "")
{
  return arg->state == state;
}
MATCHER(IsResultSuccess, "")
{
  return arg->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
}
MATCHER(IsResultNotEmpty, "")
{
  return !arg->response.planned_trajectories.empty();
}
 
TEST_F(IntegrationTestSequenceAction, TestActionServerCallbacks)
{
  using ::testing::_;
  using ::testing::AllOf;
  using ::testing::AtLeast;
  using ::testing::InSequence;
 
  namespace ph = std::placeholders;
 
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  // We do not need the complete sequence, just two commands.
  seq.erase(2, seq.size());
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  // set expectations (no guarantee, that done callback is called before idle
  // feedback)
  EXPECT_CALL(*this, active_callback()).Times(1).RetiresOnSaturation();
 
  EXPECT_CALL(*this, done_callback(_, AllOf(IsResultSuccess(), IsResultNotEmpty())))
      .Times(1)
      .WillOnce(ACTION_OPEN_BARRIER_VOID(GOAL_SUCCEEDED_EVENT))
      .RetiresOnSaturation();
 
  // the feedbacks are expected in order
  {
    InSequence dummy;
 
    EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("PLANNING"))).Times(AtLeast(1));
    EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("MONITOR"))).Times(AtLeast(1));
    EXPECT_CALL(*this, feedback_callback(FeedbackStateEq("IDLE")))
        .Times(AtLeast(1))
        .WillOnce(ACTION_OPEN_BARRIER_VOID(SERVER_IDLE_EVENT))
        .RetiresOnSaturation();
  }
 
  // send goal using mocked callback methods
  ac_.sendGoal(
      seq_goal, [this](const auto& state, const auto& result) { done_callback(state, result); },
      [this] { active_callback(); }, [this](const auto& feedback) { feedback_callback(feedback); });
 
  // wait for the ecpected events
  BARRIER({ GOAL_SUCCEEDED_EVENT, SERVER_IDLE_EVENT });
}
 
TEST_F(IntegrationTestSequenceAction, TestPlanOnlyFlag)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  // We do not need the complete sequence, just two commands.
  seq.erase(2, seq.size());
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.planning_options.plan_only = true;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Sequence execution failed.";
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
 
  ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(),
                                   joint_position_tolerance_, group_name_))
      << "Robot did move although \"PlanOnly\" flag set.";
}
 
TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotStateForPlanOnly)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  // We do not need the complete sequence, just two commands.
  seq.erase(2, seq.size());
 
  // create request
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.planning_options.plan_only = true;
  seq_goal.request = seq.toRequest();
 
  seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
      << "Execution of sequence failed.";
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
 
  ASSERT_TRUE(isAtExpectedPosition(*(move_group_->getCurrentState()), start_config.toRobotState(),
                                   joint_position_tolerance_, group_name_))
      << "Robot did move although \"PlanOnly\" flag set.";
}
 
TEST_F(IntegrationTestSequenceAction, TestNegativeBlendRadiusForPlanOnly)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  seq.setBlendRadius(0, -1.0);
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
  seq_goal.planning_options.plan_only = true;
 
  ac_.sendGoalAndWait(seq_goal);
 
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN)
      << "Incorrect error code.";
  EXPECT_TRUE(res->response.planned_trajectories.empty());
}
 
TEST_F(IntegrationTestSequenceAction, TestIgnoreRobotState)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  // We do not need the complete sequence, just two commands.
  seq.erase(2, seq.size());
 
  // create request
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  seq_goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
      << "Execution of sequence failed.";
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
}
 
TEST_F(IntegrationTestSequenceAction, TestLargeRequest)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
  moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() };
  // Create large request by making copies of the original sequence commands
  // and adding them to the end of the original sequence.
  size_t n{ req.items.size() };
  for (size_t i = 0; i < n; ++i)
  {
    moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) };
    if (i == 0)
    {
      // Remove start state because only the first request
      // is allowed to have a start state in a sequence.
      item.req.start_state = moveit_msgs::msg::RobotState();
    }
    req.items.push_back(item);
  }
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = req;
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS) << "Incorrect error code.";
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
}
 
TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithoutBlending)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
 
  seq.setAllBlendRadiiToZero();
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
}
 
TEST_F(IntegrationTestSequenceAction, TestComplexSequenceWithBlending)
{
  Sequence seq{ data_loader_->getSequence("ComplexSequence") };
 
  moveit_msgs::msg::MoveGroupSequenceGoal seq_goal;
  seq_goal.request = seq.toRequest();
 
  ac_.sendGoalAndWait(seq_goal);
  moveit_msgs::msg::MoveGroupSequenceResultConstPtr res = ac_.getResult();
  EXPECT_EQ(res->response.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
  EXPECT_FALSE(res->response.planned_trajectories.empty()) << "Planned trajectory is empty";
}
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "integrationtest_sequence_action_capability");
  ros::NodeHandle nh;
 
  ros::AsyncSpinner spinner{ 1 };
  spinner.start();
 
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}