diff --git a/moveit2.repos b/moveit2.repos index b7c72382ca..25598580b7 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -31,3 +31,15 @@ repositories: type: git url: https://github.com/ros-planning/warehouse_ros_mongo version: ros2 + osrf_pycommon: + type: git + url: https://github.com/osrf/osrf_pycommon + version: master + launch: + type: git + url: https://github.com/ros2/launch + version: master + launch_ros: + type: git + url: https://github.com/ros2/launch_ros + version: master diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index a705950bd3..5c0a9a8400 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -189,71 +189,58 @@ if(BUILD_TESTING) ament_add_gtest(test_low_pass_filter test/test_low_pass_filter.cpp) target_link_libraries(test_low_pass_filter ${SERVO_LIB_NAME}) - # TODO(andyz): re-enable integration tests when they are less flakey. - # The issue is that the test completes successfully but a results file is not generated. - - # # Unit test for ServoCalcs + # TODO(vatanaksoytezer): re-enable servo calculation test once it is not flaky. + # Unit test for ServoCalcs # ament_add_gtest_executable( # unit_test_servo_calcs # test/unit_test_servo_calcs.cpp # ) # target_link_libraries(unit_test_servo_calcs ${SERVO_LIB_NAME}) # ament_target_dependencies(unit_test_servo_calcs ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) - # add_ros_test(test/launch/unit_test_servo_calcs.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # # Servo integration launch test - # ament_add_gtest_executable(test_servo_integration - # test/test_servo_interface.cpp - # test/servo_launch_test_common.hpp - # ) - # target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME}) - # ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # add_ros_test(test/launch/test_servo_integration.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # # Servo collision checking integration test - # ament_add_gtest_executable(test_servo_collision - # test/test_servo_collision.cpp - # test/servo_launch_test_common.hpp - # ) - # target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME}) - # ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # add_ros_test(test/launch/test_servo_collision.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # # Servo singularity checking integration test - # ament_add_gtest_executable(test_servo_singularity - # test/test_servo_singularity.cpp - # test/servo_launch_test_common.hpp - # ) - # target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME}) - # ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # add_ros_test(test/launch/test_servo_singularity.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # TODO(henningkayser): Port tests to ROS2 - # servo_cpp_interface - # add_rostest_gtest(servo_cpp_interface_test - # test/servo_cpp_interface_test.test - # test/servo_cpp_interface_test.cpp - # ) - # target_link_libraries(servo_cpp_interface_test - # ${SERVO_LIB_NAME} - # ${catkin_LIBRARIES} - # ) - - # # pose_tracking - # ament_add_gtest_executable(test_servo_pose_tracking - # test/pose_tracking_test.cpp - # ) - # ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING}) - # add_ros_test(test/launch/test_servo_pose_tracking.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # # basic_servo_tests - # ament_add_gtest_executable(basic_servo_tests - # test/basic_servo_tests.cpp - # ) - # ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # target_link_libraries(basic_servo_tests ${POSE_TRACKING}) - # add_ros_test(test/launch/test_basic_servo.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # add_ros_test(test/launch/unit_test_servo_calcs.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Servo integration launch test + ament_add_gtest_executable(test_servo_integration + test/test_servo_interface.cpp + test/servo_launch_test_common.hpp + ) + target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME}) + ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Servo collision checking integration test + ament_add_gtest_executable(test_servo_collision + test/test_servo_collision.cpp + test/servo_launch_test_common.hpp + ) + target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME}) + ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Servo singularity checking integration test + ament_add_gtest_executable(test_servo_singularity + test/test_servo_singularity.cpp + test/servo_launch_test_common.hpp + ) + target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME}) + ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/launch/test_servo_singularity.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Servo pose tracking test + ament_add_gtest_executable(test_servo_pose_tracking + test/pose_tracking_test.cpp + ) + ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) + target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING}) + add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Basic servo functionality test + ament_add_gtest_executable(basic_servo_tests + test/basic_servo_tests.cpp + ) + ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS}) + target_link_libraries(basic_servo_tests ${POSE_TRACKING}) + add_ros_test(test/launch/test_basic_servo.test.py TIMEOUT 300 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py b/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py index ca94473267..237afab573 100644 --- a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py +++ b/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py @@ -28,7 +28,6 @@ def generate_test_description(): ), parameters=[servo_params], output="screen", - # prefix="kitty gdb -e run --args" ) return ( diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp deleted file mode 100644 index 572e40f733..0000000000 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp +++ /dev/null @@ -1,269 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman and Tyler Weaver - Desc: Test for the C++ interface to moveit_servo -*/ - -// C++ -#include - -// ROS -#include - -// Testing -#include - -// Servo -#include -#include - -static const std::string LOGNAME = "servo_cpp_interface_test"; -static constexpr double LARGEST_ALLOWABLE_PANDA_VEL = 2.8710; // to test joint velocity limit enforcement - -namespace moveit_servo -{ -class ServoFixture : public ::testing::Test -{ -public: - void SetUp() override - { - // Wait for several key topics / parameters - ros::topic::waitForMessage("/joint_states"); - while (!nh_.hasParam("/robot_description") && ros::ok()) - { - ros::Duration(0.1).sleep(); - } - - // Load the planning scene monitor - planning_scene_monitor_ = std::make_shared("robot_description"); - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startStateMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - - // Create moveit_servo - servo_ = std::make_shared(nh_, planning_scene_monitor_); - } - void TearDown() override - { - } - - bool waitForFirstStatus() - { - auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(1)); - return static_cast(msg); - } - -protected: - ros::NodeHandle nh_{ "~" }; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - moveit_servo::ServoPtr servo_; -}; // class ServoFixture - -TEST_F(ServoFixture, StartStopTest) -{ - servo_->start(); - EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - servo_->setPaused(true); - - ros::Duration(1.0).sleep(); - - // Start and stop again - servo_->setPaused(false); - EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - servo_->setPaused(true); -} - -TEST_F(ServoFixture, SendTwistStampedTest) -{ - servo_->start(); - EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - - auto parameters = servo_->getParameters(); - - // count trajectory messages sent by servo - size_t received_count = 0; - boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; - auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); - - // Create publisher to send servo commands - auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); - - constexpr double test_duration = 1.0; - const double publish_period = parameters.publish_period; - const size_t num_commands = static_cast(test_duration / publish_period); - - ros::Rate publish_rate(1. / publish_period); - - // Send a few Cartesian velocity commands - for (size_t i = 0; i < num_commands && ros::ok(); ++i) - { - auto msg = moveit::util::make_shared_from_pool(); - msg->header.stamp = ros::Time::now(); - msg->header.frame_id = "panda_link0"; - msg->twist.angular.y = 1.0; - - // Send the message - twist_stamped_pub.publish(msg); - publish_rate.sleep(); - } - - EXPECT_GT(received_count, num_commands - 20); - EXPECT_GT(received_count, (unsigned)0); - EXPECT_LT(received_count, num_commands + 20); - servo_->setPaused(true); -} - -TEST_F(ServoFixture, SendJointServoTest) -{ - servo_->start(); - EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - - auto parameters = servo_->getParameters(); - - // count trajectory messages sent by servo - size_t received_count = 0; - boost::function traj_callback = - [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; }; - auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); - - // Create publisher to send servo commands - auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); - - constexpr double test_duration = 1.0; - const double publish_period = parameters.publish_period; - const size_t num_commands = static_cast(test_duration / publish_period); - - ros::Rate publish_rate(1. / publish_period); - - // Send a few joint velocity commands - for (size_t i = 0; i < num_commands && ros::ok(); ++i) - { - auto msg = moveit::util::make_shared_from_pool(); - msg->header.stamp = ros::Time::now(); - msg->header.frame_id = "panda_link3"; - msg->velocities.push_back(0.1); - - // Send the message - joint_servo_pub.publish(msg); - publish_rate.sleep(); - } - - EXPECT_GT(received_count, num_commands - 20); - EXPECT_LT(received_count, num_commands + 20); - servo_->setPaused(true); -} - -TEST_F(ServoFixture, JointVelocityEnforcementTest) -{ - servo_->start(); - EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; - - auto parameters = servo_->getParameters(); - double publish_period = parameters.publish_period; - - // count trajectory messages sent by servo - size_t received_count = 0; - trajectory_msgs::JointTrajectory joint_command_from_servo; - trajectory_msgs::JointTrajectory prev_joint_command_from_servo; - boost::function traj_callback = - [&](const trajectory_msgs::JointTrajectoryConstPtr& msg) { - ++received_count; - // Store a series of two commands so we can calculate velocities - // from positions - prev_joint_command_from_servo = joint_command_from_servo; - joint_command_from_servo = *msg; - - // Start running checks when we have at least two datapoints - if (received_count > 1) - { - // Need a sequence of two commands to calculate a velocity - EXPECT_GT(joint_command_from_servo.points.size(), (unsigned)0); - EXPECT_GT(prev_joint_command_from_servo.points.size(), (unsigned)0); - EXPECT_EQ(prev_joint_command_from_servo.points.size(), joint_command_from_servo.points.size()); - // No velocities larger than the largest allowable Panda velocity - for (size_t joint_index = 0; joint_index < joint_command_from_servo.points[0].positions.size(); ++joint_index) - { - double joint_velocity = (joint_command_from_servo.points[0].positions[joint_index] - - prev_joint_command_from_servo.points[0].positions[joint_index]) / - publish_period; - EXPECT_LE(joint_velocity, LARGEST_ALLOWABLE_PANDA_VEL); - } - } - }; - auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); - - // Create publisher to send servo commands - auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); - - constexpr double test_duration = 1.0; - const size_t num_commands = static_cast(test_duration / publish_period); - - ros::Rate publish_rate(1. / publish_period); - - // Send a few Cartesian commands with very high velocity - for (size_t i = 0; i < num_commands && ros::ok(); ++i) - { - auto msg = moveit::util::make_shared_from_pool(); - msg->header.stamp = ros::Time::now(); - msg->header.frame_id = "panda_link0"; - msg->twist.linear.x = 10.0; - msg->twist.angular.y = 5 * LARGEST_ALLOWABLE_PANDA_VEL; - - // Send the message - twist_stamped_pub.publish(msg); - publish_rate.sleep(); - } - - EXPECT_GT(received_count, num_commands - 20); - EXPECT_LT(received_count, num_commands + 20); - servo_->setPaused(true); -} -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - ros::init(argc, argv, LOGNAME); - testing::InitGoogleTest(&argc, argv); - - ros::AsyncSpinner spinner(8); - spinner.start(); - - int result = RUN_ALL_TESTS(); - return result; -} diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test deleted file mode 100644 index 94c6a2a7f4..0000000000 --- a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp b/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp index 63194c4e1c..882deb1694 100644 --- a/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp +++ b/moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp @@ -315,15 +315,17 @@ TEST_F(ServoCalcsTestFixture, TestEnforcePosLimits) servo_calcs_->original_joint_state_ = joint_state; servo_calcs_->current_state_->setVariableValues(joint_state); + trajectory_msgs::msg::JointTrajectory joint_trajectory; + servo_calcs_->composeJointTrajMessage(joint_state, joint_trajectory); // Test here, expecting to be violating joint position limits - EXPECT_FALSE(servo_calcs_->enforcePositionLimits()); + EXPECT_FALSE(servo_calcs_->enforcePositionLimits(joint_trajectory)); // At the upper limits with negative velocity, we should not be 'violating' velocity.assign(9, -1.0); joint_state = getJointState(position, velocity); servo_calcs_->original_joint_state_ = joint_state; servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_TRUE(servo_calcs_->enforcePositionLimits()); + EXPECT_TRUE(servo_calcs_->enforcePositionLimits(joint_trajectory)); // However, if we change 1 of the joints to the bottom limit and stay negative velocity // We expect to violate joint position limits again @@ -331,7 +333,7 @@ TEST_F(ServoCalcsTestFixture, TestEnforcePosLimits) joint_state = getJointState(position, velocity); servo_calcs_->original_joint_state_ = joint_state; servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_FALSE(servo_calcs_->enforcePositionLimits()); + EXPECT_FALSE(servo_calcs_->enforcePositionLimits(joint_trajectory)); // For completeness, we'll set the position to lower limits with positive vel and expect a pass std::vector lower_position{ 0, 0, -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973 }; @@ -339,7 +341,7 @@ TEST_F(ServoCalcsTestFixture, TestEnforcePosLimits) joint_state = getJointState(lower_position, velocity); servo_calcs_->original_joint_state_ = joint_state; servo_calcs_->current_state_->setVariableValues(joint_state); - EXPECT_TRUE(servo_calcs_->enforcePositionLimits()); + EXPECT_TRUE(servo_calcs_->enforcePositionLimits(joint_trajectory)); } TEST_F(ServoCalcsTestFixture, TestEnforceVelLimits) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index e19157cdce..54af802ff1 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -92,7 +92,7 @@ if(BUILD_TESTING) # target_link_libraries(move_group_ompl_constraints_test ${THIS_PACKAGE_LIBRARIES}) # ament_target_dependencies(move_group_ompl_constraints_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # add_ros_test(test/launch/move_group_ompl_constraints.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # add_ros_test(test/launch/move_group_ompl_constraints.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # end move group interface cpp ompl constrained planning integration test endif()