diff --git a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp index 5ab21af36..081664508 100644 --- a/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp +++ b/franka_gazebo/test/franka_hw_sim_gazebo_test.cpp @@ -1,9 +1,17 @@ +#include #include +#include #include #include +#include +#include +#include +#include #include #include #include +#include +#include #include #include #include @@ -11,7 +19,13 @@ #include using namespace franka_gazebo; +using franka::RobotMode; namespace sml = boost::sml; + +// NOTE: Keep a global node handle in memory in order for all tests to be executed sequentially +// If each test or fixture declares its own node handle only the first test will be executed correctly +std::unique_ptr nh; + TEST( #if ROS_VERSION_MINIMUM(1, 15, 13) FrankaHWSim, /* noetic & newer */ @@ -277,8 +291,198 @@ TEST_F(StateMachineFixture, ASSERT_EQ(state.ddq_d, zeros); } +struct FrankaHWSimFixture : public ::testing::Test { + private: + double start_; + + public: + ros::ServiceClient user_stop; + std::unique_ptr> gripper; + std::unique_ptr> error_recovery; + + virtual void SetUp() { + start_ = ros::Time::now().toSec(); + user_stop = nh->serviceClient("set_user_stop"); + gripper = std::make_unique>( + "/franka_gripper/move"); + error_recovery = + std::make_unique>( + "/franka_control/error_recovery"); + } + + double now() { return ros::Time::now().toSec() - start_; } + + void openGripper() { + franka_gripper::MoveGoal goal; + goal.speed = 0.1; + goal.width = franka_gazebo::kMaxFingerWidth; + gripper->sendGoalAndWait(goal, ros::Duration(5)); + auto result = gripper->getResult(); + ASSERT_TRUE(result->success) << "Failed to open the gripper, error: " << result->error; + } + void closeGripper() { + franka_gripper::MoveGoal goal; + goal.speed = 0.1; + goal.width = 0.0; + gripper->sendGoalAndWait(goal, ros::Duration(5)); + auto result = gripper->getResult(); + ASSERT_TRUE(result->success) << "Failed to close the gripper, error: " << result->error; + } + + void errorRecovery() { + error_recovery->sendGoalAndWait(franka_msgs::ErrorRecoveryGoal(), ros::Duration(5)); + } + + std_srvs::SetBool::Response setUserStop(bool pressed) { + std_srvs::SetBool service; + service.request.data = pressed; + user_stop.call(service); + return service.response; + } + + virtual void TearDown() { + closeGripper(); + setUserStop(false); + errorRecovery(); + } +}; + +TEST_F(FrankaHWSimFixture, pressing_user_stop_stops_the_arm_joints) { + // Test procedure: + // Command a circular motion to the impedance controller to get the joints moving + // After some time (user_stop_time) press the user stop, let the joints stop as fast as possible + // After again some time (assert_start_time) measure the velocities of all joints + // After again some time (assert_end_time) verify if the average of the velocities is below some + // threshold. Have to filter because velocity signal is noisy + + double radius = 0.10; + double frequency = 0.5; + double user_stop_time = 3; + double assert_start_time = 3.5; + double assert_end_time = 4.5; + double stop_speed_tolerance = 1e-2; // [rad/s] + + ros::Duration timeout(5); + + geometry_msgs::PoseStamped target; + target.pose.position.z = 0.6; + target.pose.orientation.x = 1; + target.header.frame_id = "panda_link0"; + size_t samples = 0; + std::map velocities; + auto pub = nh->advertise( + "/cartesian_impedance_example_controller/equilibrium_pose", 1); + ros::Rate rate(30); + + bool user_stop_pressed = false; + while (ros::ok()) { + rate.sleep(); + double t = now(); + + target.pose.position.x = radius * std::sin(2 * M_PI * frequency * t) + 0.3; + target.pose.position.y = radius * std::cos(2 * M_PI * frequency * t); + pub.publish(target); + + if (t < user_stop_time) { + continue; + } + // After some time, we press the user stop + if (not user_stop_pressed) { + setUserStop(true); + user_stop_pressed = true; + } + if (t < assert_start_time) { + continue; + } + + auto state = ros::topic::waitForMessage( + "/franka_state_controller/franka_states", *nh, timeout); + ASSERT_TRUE(state != nullptr) + << "No message on /franka_state_controller/franka_states received within " + << timeout.toSec() << "s"; + EXPECT_EQ(static_cast(state->robot_mode), RobotMode::kUserStopped); + + // A little while later we check if the joints are still moving + auto msg = ros::topic::waitForMessage("/joint_states", *nh, timeout); + ASSERT_TRUE(msg != nullptr) << "No message on /joint_states received within " << timeout.toSec() + << "s"; + for (int i = 0; i < msg->name.size(); i++) { + auto name = msg->name.at(i); + if (contains(name, "finger_joint")) { + continue; + } + velocities.emplace(name, 0); // insert if not yet exist + velocities.at(name) += msg->velocity.at(i); + } + samples++; + + if (t < assert_end_time) { + continue; + } + + for (auto pair : velocities) { + auto name = pair.first; + auto velocity = pair.second / samples; + EXPECT_NEAR(velocity, 0, stop_speed_tolerance) << "Joint " << name << " is still moving!"; + } + + break; // finish test + } +} + +TEST_F(FrankaHWSimFixture, pressing_user_stop_lets_finger_joints_still_move) { + // Test Procedure: + // Arange by closing the gripper after (close_gripper_time) + // After some time (user_stop_time) the user stop is pressed + // Then after some time (open_gripper_time) make a move action call to open the fingers + // When the action is finished, check if the finger positions are actually where we commanded them + + double user_stop_time = 1; + double open_gripper_time = 1.5; + double position_tolerance = 5e-3; // [m] + + bool pressed_user_stop = false; + ros::Rate rate(30); + ros::Duration timeout(5); + + while (ros::ok()) { + rate.sleep(); + + double t = now(); + + if (t < user_stop_time) { + continue; + } + + if (not pressed_user_stop) { + pressed_user_stop = true; + setUserStop(true); + } + + if (t < open_gripper_time) { + continue; + } + openGripper(); + + auto msg = ros::topic::waitForMessage("/franka_gripper/joint_states", + *nh, timeout); + ASSERT_TRUE(msg != nullptr) << "No message on /franka_gripper/joint_states received within " + << timeout.toSec() << "s"; + for (int i = 0; i < msg->name.size(); i++) { + auto name = msg->name.at(i); + if (not contains(name, "finger_joint")) { + continue; + } + EXPECT_NEAR(msg->position.at(i), franka_gazebo::kMaxFingerWidth / 2., position_tolerance); + } + + break; // finish test + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "franka_hw_sim_test"); + nh = std::make_unique(); return RUN_ALL_TESTS(); } diff --git a/franka_gazebo/test/launch/franka_hw_sim_gazebo.test b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test index 7edc56c4f..b9b861028 100644 --- a/franka_gazebo/test/launch/franka_hw_sim_gazebo.test +++ b/franka_gazebo/test/launch/franka_hw_sim_gazebo.test @@ -3,6 +3,7 @@ +