Skip to content

Commit

Permalink
trajectory execution manager: reactivate tests (#3177)
Browse files Browse the repository at this point in the history
  • Loading branch information
@cambel
cambel committed Jul 31, 2022
1 parent 24c18f6 commit a63580e
Show file tree
Hide file tree
Showing 4 changed files with 92 additions and 80 deletions.
15 changes: 9 additions & 6 deletions moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,18 @@ install(TARGETS ${MOVEIT_LIB_NAME}
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})

if(CATKIN_ENABLE_TESTING)
find_package(moveit_resources_panda_moveit_config REQUIRED)
find_package(rostest REQUIRED)

add_rostest_gtest(test_execution_manager
test/test_execution_manager.test
test/test_execution_manager.cpp)
target_link_libraries(test_execution_manager moveit_cpp ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})


## This needs further cleanup before it can run
# add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp)
# set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
# target_link_libraries(test_controller_manager_plugin ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#
# find_package(rostest REQUIRED)
# add_rostest_gtest(test_execution_manager
# test/test_execution_manager.test
# test/test_execution_manager.cpp)
# target_link_libraries(test_execution_manager ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})

endif(CATKIN_ENABLE_TESTING)
5 changes: 5 additions & 0 deletions moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -989,6 +989,11 @@ void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callba
void TrajectoryExecutionManager::execute(const ExecutionCompleteCallback& callback,
const PathSegmentCompleteCallback& part_callback, bool auto_clear)
{
// skip execution if no trajectory have been pushed
// it crashes otherwise
if (trajectories_.empty())
return;

stopExecution(false);

// check whether first trajectory starts at current robot state
Expand Down
132 changes: 73 additions & 59 deletions moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,76 +32,90 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */
/* Author: Ioan Sucan, Cristian C. Beltran
Desc: Test the TrajectoryExecutionManager with MoveitCpp
*/

#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
// ROS
#include <ros/ros.h>

int main(int argc, char** argv)
{
ros::init(argc, argv, "test_trajectory_execution_manager");

ros::AsyncSpinner spinner(1);
spinner.start();
// Testing
#include <gtest/gtest.h>

robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader);
planning_scene_monitor::PlanningSceneMonitor psm(rml);
trajectory_execution_manager::TrajectoryExecutionManager tem(rml->getModel(), psm.getStateMonitor(), true);
// Main class
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
// Msgs
#include <geometry_msgs/PointStamped.h>

std::cout << "1:\n";
if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "basej")))
ROS_ERROR("Fail!");

std::cout << "2:\n";
if (!tem.ensureActiveController("arms"))
ROS_ERROR("Fail!");

std::cout << "3:\n";
if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "rj2")))
ROS_ERROR("Fail!");
namespace moveit_cpp
{
class MoveItCppTest : public ::testing::Test
{
public:
void SetUp() override
{
nh_ = ros::NodeHandle();
moveit_cpp_ptr = std::make_shared<MoveItCpp>(nh_);
trajectory_execution_manager_ptr = moveit_cpp_ptr->getTrajectoryExecutionManager();

traj1.joint_trajectory.joint_names.push_back("panda_joint1");
traj1.joint_trajectory.points.resize(1);
traj1.joint_trajectory.points[0].positions.push_back(0.0);

traj2 = traj1;
traj2.joint_trajectory.joint_names.push_back("panda_joint2");
traj2.joint_trajectory.points[0].positions.push_back(1.0);
traj2.multi_dof_joint_trajectory.joint_names.push_back("panda_joint3");
traj2.multi_dof_joint_trajectory.points.resize(1);
traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1);
}

protected:
ros::NodeHandle nh_;
MoveItCppPtr moveit_cpp_ptr;
PlanningComponentPtr planning_component_ptr;
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_ptr;
moveit_msgs::RobotTrajectory traj1;
moveit_msgs::RobotTrajectory traj2;
};

std::cout << "4:\n";
if (!tem.ensureActiveControllersForJoints(std::vector<std::string>(1, "lj1")))
ROS_ERROR("Fail!");
TEST_F(MoveItCppTest, EnsureActiveControllersForJointsTest)
{
ASSERT_TRUE(trajectory_execution_manager_ptr->ensureActiveControllersForJoints({ "panda_joint1" }));
}

std::cout << "5:\n";
if (!tem.ensureActiveController("left_arm_head"))
ROS_ERROR("Fail!");
std::cout << "6:\n";
if (!tem.ensureActiveController("arms"))
ROS_ERROR("Fail!");
TEST_F(MoveItCppTest, ensureActiveControllerTest)
{
ASSERT_TRUE(trajectory_execution_manager_ptr->ensureActiveController("fake_panda_arm_controller"));
}

TEST_F(MoveItCppTest, ExecuteEmptySetOfTrajectoriesTest)
{
// execute with empty set of trajectories
tem.execute();
if (!tem.waitForExecution())
ROS_ERROR("Fail!");

moveit_msgs::RobotTrajectory traj1;
traj1.joint_trajectory.joint_names.push_back("rj1");
traj1.joint_trajectory.points.resize(1);
traj1.joint_trajectory.points[0].positions.push_back(0.0);
if (!tem.push(traj1))
ROS_ERROR("Fail!");

moveit_msgs::RobotTrajectory traj2 = traj1;
traj2.joint_trajectory.joint_names.push_back("lj2");
traj2.joint_trajectory.points[0].positions.push_back(1.0);
traj2.multi_dof_joint_trajectory.joint_names.push_back("basej");
traj2.multi_dof_joint_trajectory.points.resize(1);
traj2.multi_dof_joint_trajectory.points[0].transforms.resize(1);

if (!tem.push(traj2))
ROS_ERROR("Fail!");
trajectory_execution_manager_ptr->execute();
auto last_execution_status = trajectory_execution_manager_ptr->waitForExecution();
ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED);
}

TEST_F(MoveItCppTest, PushExecuteAndWaitTest)
{
ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj1));
ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj2));
traj1.multi_dof_joint_trajectory = traj2.multi_dof_joint_trajectory;
if (!tem.push(traj1))
ROS_ERROR("Fail!");
ASSERT_TRUE(trajectory_execution_manager_ptr->push(traj1));
auto last_execution_status = trajectory_execution_manager_ptr->executeAndWait();
ASSERT_EQ(last_execution_status, moveit_controller_manager::ExecutionStatus::SUCCEEDED);
}

} // namespace moveit_cpp

if (!tem.executeAndWait())
ROS_ERROR("Fail!");
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_execution_manager");

ros::waitForShutdown();
int result = RUN_ALL_TESTS();

return 0;
return result;
}
20 changes: 5 additions & 15 deletions moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.test
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,10 @@
<?xml version="1.0"?>
<launch>
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find moveit_resources)/pr2_moveit_config/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch"/>

<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_footprint" />
<!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
<include ns="test_execution_manager" file="$(find moveit_resources_panda_moveit_config)/launch/fake_moveit_controller_manager.launch.xml" />

<!-- Send fake joint values -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>

<!-- Start robot state publisher -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

<test test-name="execution_manager" pkg="moveit_ros_planning" type="test_execution_manager">
<param name="moveit_controller_manager" value=""/>
</test>
<test pkg="moveit_ros_planning" type="test_execution_manager" test-name="test_execution_manager" time-limit="300" args="" />
</launch>

0 comments on commit a63580e

Please sign in to comment.