unittest_system_info.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 <string>
#include <vector>
#include <thread>
#include <atomic>
#include <memory>

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

#include <ros/ros.h>
#include <canopen_chain_node/GetObject.h>
#include <sensor_msgs/JointState.h>

#include <pilz_utils/wait_for_message.h>
#include <pilz_utils/wait_for_service.h>
#include <pilz_testutils/async_test.h>
#include <prbt_hardware_support/system_info.h>
#include <prbt_hardware_support/system_info_exception.h>

namespace system_info_tests
{

using canopen_chain_node::GetObject;
using namespace prbt_hardware_support;
using namespace testing;

static const std::string GET_OBJECT_TOPIC_NAME{"get_object"};
static const std::string JOINT_STATES_TOPIC_NAME{"joint_states"};
static constexpr unsigned int JOINT_STATES_TOPIC_QUEUE_SIZE{1};

class SystemInfoTests : public testing::Test, public testing::AsyncTest
{
public:
  virtual void SetUp() override;
  virtual void TearDown() override;

  void publishJointState();

protected:
  MOCK_METHOD2(executeGetObject,  bool(GetObject::Request&, GetObject::Response&));

  void advertiseGetObjectService();

protected:
  // Needed so that service callback are proecessed. Otherwise test in which
  // service are calls will hang.
  ros::AsyncSpinner spinner_ {2};

  const std::vector<std::string> joint_names_ {"joint1", "joint2"};
  ros::ServiceServer get_obj_service_;
  std::thread publisher_thread_;
  std::atomic_bool terminate_ {false};

};

void SystemInfoTests::advertiseGetObjectService()
{
  ros::NodeHandle driver_nh {"/prbt/driver/"};
  get_obj_service_ = driver_nh.advertiseService(GET_OBJECT_TOPIC_NAME,
                                                &SystemInfoTests::executeGetObject,
                                                this);
}

void SystemInfoTests::SetUp()
{
  spinner_.start();

  // Set joint names on parameter server
  ros::NodeHandle driver_nh {"/prbt/driver/"};
  for(unsigned int i = 0; i < joint_names_.size(); ++i)
  {
    driver_nh.setParam("nodes/" + joint_names_.at(i) + "/id", static_cast<int>(i));
  }

  publisher_thread_ = std::thread(&SystemInfoTests::publishJointState, this);
  advertiseGetObjectService();
}

void SystemInfoTests::TearDown()
{
  if (publisher_thread_.joinable())
  {
    terminate_ = true;
    publisher_thread_.join();
  }
}

void SystemInfoTests::publishJointState()
{
  ros::NodeHandle global_nh {"/"};
  ros::Publisher joint_state_pub = global_nh.advertise<sensor_msgs::JointState>(
        JOINT_STATES_TOPIC_NAME,
        JOINT_STATES_TOPIC_QUEUE_SIZE);

  while(!terminate_ && ros::ok())
  {
    joint_state_pub.publish(sensor_msgs::JointState());
    ros::Duration(0.5).sleep();
  }
}

TEST_F(SystemInfoTests, TestExceptions)
{
  const std::string exp_msg {"Test-Msg"};
  std::shared_ptr<SystemInfoException> ex {new SystemInfoException(exp_msg)};
  EXPECT_TRUE(std::string(ex->what()) == exp_msg);
}

TEST_F(SystemInfoTests, testNodeNamesMissing)
{
  ros::NodeHandle nh{"~"};
  nh.deleteParam("/prbt/driver/nodes");
  EXPECT_THROW(SystemInfo{nh}, SystemInfoException);
}

TEST_F(SystemInfoTests, testCANUpAndRunning)
{
  // Make sure that no JointStates are published
  terminate_ = true;
  publisher_thread_.join();

  // Variable stating if the constructor is finished or not.
  std::atomic_bool ctor_called {false};
  // Start thread which allows us to asynchronously call the constructor
  std::thread async_ctor_call_thread = std::thread([this, &ctor_called] () {
    ros::NodeHandle nh{"~"};
    SystemInfo info(nh);
    ctor_called = true;
    this->triggerClearEvent("ctor_called");
  });

  // Wait a while and then check if constructor is still waiting for the topic.
  ros::Duration(5.0 * pilz_utils::DEFAULT_RETRY_TIMEOUT).sleep();
  ASSERT_FALSE(ctor_called) << "Ctor already finished although \"/joint_states\" topic not published yet";

  // Activate publishing of "/joint_states"
  terminate_ = false;
  publisher_thread_ = std::thread(&SystemInfoTests::publishJointState, this);
  pilz_utils::waitForMessage<sensor_msgs::JointState>("/joint_states");
  // Wait till constructor is finished
  BARRIER("ctor_called");

  if (async_ctor_call_thread.joinable())
  {
    async_ctor_call_thread.join();
  }
}

TEST_F(SystemInfoTests, testCANServiceUpAndRunning)
{
  // Make sure that no ros_canopen service "get_object" is advertised.
  get_obj_service_.shutdown();

  // Variable stating if the constructor is finished or not.
  std::atomic_bool ctor_called {false};
  // Start thread which allows us to asynchronously call the constructor
  std::thread async_ctor_call_thread = std::thread([this, &ctor_called] () {
    ros::NodeHandle nh{"~"};
    SystemInfo info(nh);
    ctor_called = true;
    this->triggerClearEvent("ctor_called");
  });

  // Wait a while and then check if constructor is still waiting for the topic.
  ros::Duration(5.0 * pilz_utils::DEFAULT_RETRY_TIMEOUT).sleep();
  ASSERT_FALSE(ctor_called) << "Ctor already finished although \"get_object\" service not advertised yet";

  advertiseGetObjectService();
  pilz_utils::waitForService("/prbt/driver/get_object");
  // Wait till constructor is finished
  BARRIER("ctor_called");

  if (async_ctor_call_thread.joinable())
  {
    async_ctor_call_thread.join();
  }
}

TEST_F(SystemInfoTests, testServiceResponseFalse)
{
  EXPECT_CALL(*this, executeGetObject(_,_))
      .Times(1)
      .WillRepeatedly(testing::Invoke(
                        [](GetObject::Request&, GetObject::Response& res){
                        res.success = false;
                        return true;
                      }
                      ));

  ros::NodeHandle nh{"~"};
  SystemInfo info {nh};
  EXPECT_THROW(info.getFirmwareVersions(), SystemInfoException);
}

TEST_F(SystemInfoTests, testServiceFail)
{
  EXPECT_CALL(*this, executeGetObject(_,_))
      .Times(1)
      .WillRepeatedly(testing::Invoke(
                        [](GetObject::Request&, GetObject::Response&){
                        return false;
                      }
                      ));

  ros::NodeHandle nh{"~"};
  SystemInfo info {nh};
  EXPECT_THROW(info.getFirmwareVersions(), SystemInfoException);
}

TEST_F(SystemInfoTests, testGetFirmwareVersions)
{
  ASSERT_EQ(2u, joint_names_.size()) << "Number of joints in test set-up have changed => Change expected version container in test accordingly.";

  const FirmwareCont exp_versions { {joint_names_.at(0), "100 Build:11158 Date:2018-06-07 16:49:55"},
                                    {joint_names_.at(1), "101 Build:11158 Date:2018-06-07 16:49:55"}};

  EXPECT_CALL(*this, executeGetObject(_,_))
      .Times(2)
      .WillRepeatedly(testing::Invoke(
                        [exp_versions](GetObject::Request& req, GetObject::Response& res){
                        res.value = exp_versions.at(req.node);
                        res.success = true;
                        return true;
                      }
                      ));

  // Check that returned versions from service are correct
  ros::NodeHandle nh{"~"};
  SystemInfo info {nh};
  FirmwareCont actual_version_cont {info.getFirmwareVersions()};
  for(const auto& joint : joint_names_)
  {
    EXPECT_TRUE(actual_version_cont.find(joint) != actual_version_cont.cend()) << "No version for joint found";
    EXPECT_EQ(exp_versions.at(joint), actual_version_cont.at(joint)) << "Expected and actual firmware version do not match";
  }
}

TEST_F(SystemInfoTests, testGetFirmwareVersionsResizeTo40Chars)
{
  ASSERT_EQ(2u, joint_names_.size()) << "Number of joints in test set-up have changed => Change expected version container in test accordingly.";

  const FirmwareCont exp_versions { {joint_names_.at(0), "100 Build:11158 Date:2018-06-07 16:49:55"},
                                    {joint_names_.at(1), "101 Build:11158 Date:2018-06-07 16:49:55"}};

  EXPECT_CALL(*this, executeGetObject(_,_))
      .Times(2)
      .WillRepeatedly(testing::Invoke(
                        [exp_versions](GetObject::Request& req, GetObject::Response& res){
                        res.value = exp_versions.at(req.node) + "AdditionalStuffWhichNeedsToBeRemoved";
                        res.success = true;
                        return true;
                      }
                      ));

  // Check that returned versions from service are correct
  ros::NodeHandle nh{"~"};
  SystemInfo info {nh};
  FirmwareCont actual_version_cont {info.getFirmwareVersions()};
  for(const auto& joint : joint_names_)
  {
    EXPECT_TRUE(actual_version_cont.find(joint) != actual_version_cont.cend()) << "No version for joint found";
    EXPECT_EQ(exp_versions.at(joint), actual_version_cont.at(joint)) << "Expected and actual firmware version do not match";
  }
}


}  // namespace system_info_tests


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

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