moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
move_group_fixture.cpp
Go to the documentation of this file.
1// Copyright 2024 Intrinsic Innovation LLC.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
18
19#include <chrono>
20#include <thread>
21
22#include <gtest/gtest.h>
25#include <rclcpp/executors/single_threaded_executor.hpp>
26#include <rclcpp/rclcpp.hpp>
27
29
30namespace
31{
32
33// The trajectory cache calls MoveGroupInterface::getCurrentState() internally (e.g. to resolve
34// `is_diff` start states), which only waits one second for a complete robot state. On a loaded CI
35// machine, DDS discovery of /joint_states regularly takes longer than that, which made these tests
36// flake with "Failed to fetch current robot state". Prime the current state monitor here, with a
37// generous timeout, so those short internal waits always find an already-populated monitor.
38constexpr double CURRENT_STATE_WAIT_SECONDS = 10.0;
39
40// Bound how long the spin loop blocks while idle. Long enough to keep the thread asleep instead of
41// busy-waiting (which starves DDS discovery when several of these tests run in parallel), short
42// enough that TearDown() joins promptly.
43constexpr std::chrono::milliseconds SPIN_TIMEOUT(100);
44
45} // namespace
46
48 : node_(rclcpp::Node::make_shared("move_group_fixture")), move_group_name_("panda_arm")
49{
50 node_->declare_parameter<std::string>("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection");
51 move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, move_group_name_);
52}
53
55{
56 is_spinning_ = false;
57 if (spin_thread_.joinable())
58 {
59 spin_thread_.join();
60 }
61}
62
64{
65 is_spinning_ = true;
66 spin_thread_ = std::thread(&MoveGroupFixture::spinNode, this);
67
69 db_->setParams(":memory:", 1);
70 ASSERT_TRUE(db_->connect());
71
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.";
76}
77
79{
80 db_.reset();
81 is_spinning_ = false;
82 spin_thread_.join();
83}
84
85void MoveGroupFixture::spinNode()
86{
87 rclcpp::executors::SingleThreadedExecutor executor;
88 executor.add_node(node_);
89 while (is_spinning_ && rclcpp::ok())
90 {
91 executor.spin_once(SPIN_TIMEOUT);
92 }
93}
void TearDown() override
rclcpp::Node::SharedPtr node_
warehouse_ros::DatabaseConnection::Ptr db_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > move_group_
warehouse_ros::DatabaseConnection::Ptr loadDatabase(const rclcpp::Node::SharedPtr &node)
Load a database connection.