test_utils.h

Go to the documentation of this file.

#pragma once
#include <chrono>
#include <functional>
#include <parsernode/common.h>
#include <tf/transform_datatypes.h>

namespace test_utils {
template <bool done> struct WaitUntilResult {
  bool operator()(std::function<bool(void)> const &input_function,
                  std::chrono::seconds timeout) {
    auto start = std::chrono::system_clock::now();
    std::chrono::seconds duration(0);
    while (input_function() == !done && duration.count() < timeout.count()) {
      ros::spinOnce();
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      duration = std::chrono::duration_cast<std::chrono::seconds>(
          std::chrono::system_clock::now() - start);
    }
    return input_function();
  }
};
struct BaseTestPubSubs {
  BaseTestPubSubs() : nh_send_("~common"), nh_receive_status_("~common") {
    event_pub_ = nh_send_.advertise<std_msgs::String>("event_manager", 1);
    pose_pub_ =
        nh_send_.advertise<geometry_msgs::PoseStamped>("goal_pose_command", 1);
    status_subscriber_ = nh_receive_status_.subscribe(
        "system_status", 1, &BaseTestPubSubs::statusCallback, this);
  }
  void publishEvent(std::string event) {
    std_msgs::String event_msg;
    event_msg.data = event;
    event_pub_.publish(event_msg);
  }

  void publishPoseCommand(PositionYaw position_yaw) {
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.pose.position.x = position_yaw.x;
    pose_msg.pose.position.y = position_yaw.y;
    pose_msg.pose.position.z = position_yaw.z;
    pose_msg.pose.orientation =
        tf::createQuaternionMsgFromYaw(position_yaw.yaw);
    pose_pub_.publish(pose_msg);
  }

  PositionYaw getPositionYaw(parsernode::common::quaddata quad_data) {
    return PositionYaw(quad_data.localpos.x, quad_data.localpos.y,
                       quad_data.localpos.z, quad_data.rpydata.z);
  }

  bool isStatusConnected() { return status_subscriber_.getNumPublishers() > 0; }

protected:
  std::string status_;
  ros::NodeHandle nh_send_;           
  ros::NodeHandle nh_receive_status_; 
  ros::Publisher event_pub_;          
  ros::Publisher pose_pub_;           
  ros::Subscriber status_subscriber_; 

  void statusCallback(std_msgs::String status) { status_ = status.data; }
};
using waitUntilTrue = WaitUntilResult<true>;
using waitUntilFalse = WaitUntilResult<false>;
};