22#include <gtest/gtest.h>
25#include <rclcpp/executors/single_threaded_executor.hpp>
26#include <rclcpp/rclcpp.hpp>
38constexpr double CURRENT_STATE_WAIT_SECONDS = 10.0;
43constexpr std::chrono::milliseconds SPIN_TIMEOUT(100);
50 node_->declare_parameter<std::string>(
"warehouse_plugin",
"warehouse_ros_sqlite::DatabaseConnection");
57 if (spin_thread_.joinable())
66 spin_thread_ = std::thread(&MoveGroupFixture::spinNode,
this);
69 db_->setParams(
":memory:", 1);
70 ASSERT_TRUE(
db_->connect());
72 ASSERT_TRUE(
move_group_->getCurrentState(CURRENT_STATE_WAIT_SECONDS) !=
nullptr)
73 <<
"Timed out after " << CURRENT_STATE_WAIT_SECONDS
74 <<
"s waiting for a complete robot state on /joint_states. The trajectory cache cannot "
75 "resolve `is_diff` start states without one.";
85void MoveGroupFixture::spinNode()
87 rclcpp::executors::SingleThreadedExecutor executor;
88 executor.add_node(
node_);
89 while (is_spinning_ && rclcpp::ok())
91 executor.spin_once(SPIN_TIMEOUT);
~MoveGroupFixture() override
rclcpp::Node::SharedPtr node_
warehouse_ros::DatabaseConnection::Ptr db_
std::string move_group_name_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > move_group_
warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr &node)
Load a database connection.