unittest_canopen_braketest_adapter.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2019 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <functional>
#include <memory>

#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <ros/ros.h>
#include <canopen_chain_node/GetObject.h>
#include <canopen_chain_node/SetObject.h>

#include <prbt_hardware_support/canopen_braketest_adapter.h>
#include <prbt_hardware_support/canopen_braketest_adapter_exception.h>
#include <prbt_hardware_support/BrakeTest.h>
#include <prbt_hardware_support/canopen_chain_node_mock.h>

namespace canopen_braketest_adapter_test
{
using namespace prbt_hardware_support;
using namespace testing;

using canopen_chain_node::GetObjectRequest;
using canopen_chain_node::GetObjectResponse;
using canopen_chain_node::SetObjectRequest;
using canopen_chain_node::SetObjectResponse;

static const std::string BRAKE_TEST_SERVICE_NAME{ "/prbt/braketest_adapter_node/trigger_braketest" };

static const std::string BRAKE_TEST_DURATION_OBJECT_INDEX{ "2060sub1" };
static const std::string START_BRAKE_TEST_OBJECT_INDEX{ "2060sub2" };
static const std::string BRAKE_TEST_STATUS_OBJECT_INDEX{ "2060sub3" };

static const std::string NODE_NAMES_PARAMETER_NAME{ "/prbt/driver/nodes" };
static const std::string BRAKETEST_REQUIRED_NAME{ "braketest_required" };
static const std::string NODE_NAMES_PREFIX{ "prbt_joint_" };
static const std::vector<size_t> NODE_TEST_SET{ { 0, 2, 5 } };

#define DEFAULT_SETUP                                                                                                  \
  CANOpenChainNodeMock canopen_chain_node;                                                                             \
  ros::NodeHandle nh_adapter("/prbt/braketest_adapter_node");                                                          \
  prbt_hardware_support::CANOpenBrakeTestAdapter canopen_braketest_adapter(nh_adapter);                                \
  ros::ServiceClient brake_test_srv_client = nh_.serviceClient<BrakeTest>(BRAKE_TEST_SERVICE_NAME);                    \
                                                                                                                       \
  ASSERT_TRUE(brake_test_srv_client.exists()) << "Brake test service not available.";

class CanOpenBraketestAdapterTest : public Test
{
protected:
  ros::NodeHandle nh_;
};

TEST_F(CanOpenBraketestAdapterTest, testCANOpenBrakeTestAdapterExceptionDtor)
{
  {
    std::shared_ptr<CANOpenBrakeTestAdapterException> ex{ new CANOpenBrakeTestAdapterException("Test msg") };
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestServiceWithoutCANGetService)
{
  CANOpenChainNodeMock canopen_chain_node;
  canopen_chain_node.shutdownGetService();

  EXPECT_THROW(prbt_hardware_support::CANOpenBrakeTestAdapter canopen_braketest_adapter(nh_),
               CANOpenBrakeTestAdapterException);
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestServiceWithoutCANSetService)
{
  CANOpenChainNodeMock canopen_chain_node;
  canopen_chain_node.shutdownSetService();

  EXPECT_THROW(prbt_hardware_support::CANOpenBrakeTestAdapter canopen_braketest_adapter(nh_),
               CANOpenBrakeTestAdapterException);
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestServiceWithoutNodeParameters)
{
  DEFAULT_SETUP

  /**********
   * Step 1 *
   **********/
  XmlRpc::XmlRpcValue rpc;
  ASSERT_TRUE(nh_.getParam(NODE_NAMES_PARAMETER_NAME, rpc));
  nh_.deleteParam(NODE_NAMES_PARAMETER_NAME);

  /**********
   * Step 3 *
   **********/
  BrakeTest srv;
  EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
  EXPECT_FALSE(srv.response.success);
  EXPECT_EQ(BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE, srv.response.error_code.value);

  nh_.setParam(NODE_NAMES_PARAMETER_NAME, rpc);
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestServiceWithNodeParametersTypeError)
{
  DEFAULT_SETUP

  /**********
   * Step 1 *
   **********/
  XmlRpc::XmlRpcValue rpc;
  ASSERT_TRUE(nh_.getParam(NODE_NAMES_PARAMETER_NAME, rpc));
  auto param_modified_name = NODE_NAMES_PARAMETER_NAME + "/" + NODE_NAMES_PREFIX + "1/" + BRAKETEST_REQUIRED_NAME;
  nh_.setParam(param_modified_name, 99);

  /**********
   * Step 3 *
   **********/
  BrakeTest srv;
  EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
  EXPECT_FALSE(srv.response.success);
  EXPECT_EQ(BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE, srv.response.error_code.value);

  nh_.setParam(NODE_NAMES_PARAMETER_NAME, rpc);
}

TEST_F(CanOpenBraketestAdapterTest, testGetBrakeTestDurationServiceCallFailure)
{
  DEFAULT_SETUP

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_DURATION_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(Return(false));

    EXPECT_CALL(canopen_chain_node, set_obj(AllOf(Field(&SetObjectRequest::node, node),
                                                  Field(&SetObjectRequest::object, START_BRAKE_TEST_OBJECT_INDEX)),
                                            _))
        .Times(0);

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::GET_DURATION_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    ASSERT_TRUE(Mock::VerifyAndClearExpectations(&canopen_chain_node));
  }
}

TEST_F(CanOpenBraketestAdapterTest, testGetBrakeTestDurationServiceResponseFailure)
{
  DEFAULT_SETUP

  GetObjectResponse duration_resp;
  duration_resp.success = false;

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_DURATION_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(duration_resp), Return(true)));

    EXPECT_CALL(canopen_chain_node, set_obj(AllOf(Field(&SetObjectRequest::node, node),
                                                  Field(&SetObjectRequest::object, START_BRAKE_TEST_OBJECT_INDEX)),
                                            _))
        .Times(0);

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::GET_DURATION_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    ASSERT_TRUE(Mock::VerifyAndClearExpectations(&canopen_chain_node));
  }
}

TEST_F(CanOpenBraketestAdapterTest, testStartBrakeTestServiceCallSuccess)
{
  DEFAULT_SETUP

  /**********
   * Step 1 *
   **********/
  canopen_chain_node.setDefaultActions();

  /**********
   * Step 2 *
   **********/
  BrakeTest srv;
  EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
  EXPECT_TRUE(srv.response.success);
}

TEST_F(CanOpenBraketestAdapterTest, testStartBrakeTestServiceCallFailure)
{
  DEFAULT_SETUP

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_DURATION_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1));

    EXPECT_CALL(canopen_chain_node, set_obj(AllOf(Field(&SetObjectRequest::node, node),
                                                  Field(&SetObjectRequest::object, START_BRAKE_TEST_OBJECT_INDEX)),
                                            _))
        .WillOnce(Return(false));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testStartBrakeTestServiceResponseFailure)
{
  DEFAULT_SETUP

  SetObjectResponse start_resp;
  start_resp.success = false;

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_DURATION_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1));

    EXPECT_CALL(canopen_chain_node, set_obj(AllOf(Field(&SetObjectRequest::node, node),
                                                  Field(&SetObjectRequest::object, START_BRAKE_TEST_OBJECT_INDEX)),
                                            _))
        .WillOnce(DoAll(SetArgReferee<1>(start_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusServiceCallFailure)
{
  DEFAULT_SETUP

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(Return(false));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::GET_STATUS_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusServiceResponseFailure)
{
  DEFAULT_SETUP

  GetObjectResponse status_resp;
  status_resp.success = false;

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(status_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::GET_STATUS_FAILURE, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusUnknown)
{
  DEFAULT_SETUP

  GetObjectResponse status_resp;
  status_resp.success = true;
  status_resp.value = "\0";

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(status_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::STATUS_UNKNOWN, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusPerformed)
{
  DEFAULT_SETUP

  GetObjectResponse status_resp;
  status_resp.success = true;
  status_resp.value = "\x01";

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(status_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::STATUS_PERFORMING, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusNotSuccessful)
{
  DEFAULT_SETUP

  GetObjectResponse status_resp;
  status_resp.success = true;
  status_resp.value = "\x03";

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(status_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::STATUS_NO_SUCCESS, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

TEST_F(CanOpenBraketestAdapterTest, testBrakeTestStatusNotActivelyControlled)
{
  DEFAULT_SETUP

  GetObjectResponse status_resp;
  status_resp.success = true;
  status_resp.value = "\x04";

  for (size_t node_index : NODE_TEST_SET)
  {
    std::string node{ NODE_NAMES_PREFIX + std::to_string(node_index + 1) };

    /**********
     * Step 1 *
     **********/
    // Avoid unmatched expectations
    canopen_chain_node.expectAnything();

    EXPECT_CALL(canopen_chain_node, get_obj(AllOf(Field(&GetObjectRequest::node, node),
                                                  Field(&GetObjectRequest::object, BRAKE_TEST_STATUS_OBJECT_INDEX)),
                                            _))
        .Times(AtLeast(1))
        .WillRepeatedly(DoAll(SetArgReferee<1>(status_resp), Return(true)));

    /**********
     * Step 2 *
     **********/
    BrakeTest srv;
    EXPECT_TRUE(brake_test_srv_client.call(srv)) << "Failed to call brake test service.";
    EXPECT_FALSE(srv.response.success);
    EXPECT_EQ(BrakeTestErrorCodes::STATUS_NO_CONTROL, srv.response.error_code.value);

    /**********
     * Step 3 *
     **********/
    Mock::VerifyAndClearExpectations(&canopen_chain_node);
  }
}

}  // namespace canopen_braketest_adapter_test

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "unittest_brake_test_executor");
  ros::NodeHandle nh;

  ros::AsyncSpinner spinner{ 2 };
  spinner.start();

  testing::InitGoogleMock(&argc, argv);
  return RUN_ALL_TESTS();
}