From 3991f55ffc9c1f4928bbbad03b452f4202e96eea Mon Sep 17 00:00:00 2001 From: Florian Walch <florian.walch@franka.de> Date: Fri, 24 Aug 2018 14:30:34 +0200 Subject: [PATCH] Publish robot mode in franka_state_controller --- CHANGELOG.md | 6 ++++ .../src/franka_state_controller.cpp | 30 +++++++++++++++++++ franka_msgs/msg/FrankaState.msg | 8 +++++ 3 files changed, 44 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 764df26..bfbf7fe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,11 @@ # CHANGELOG +## 0.7.0 - UNRELEASED + +Requires `libfranka` >= 0.5.0 + + * Publish `robot_mode` in `franka_state_controller` + ## 0.6.0 - 2018-08-08 Requires `libfranka` >= 0.5.0 diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp index 2e1e510..e6cd440 100644 --- a/franka_control/src/franka_state_controller.cpp +++ b/franka_control/src/franka_state_controller.cpp @@ -330,6 +330,36 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) { publisher_franka_states_.msg_.last_motion_errors = errorsToMessage(robot_state_.last_motion_errors); + switch (robot_state_.robot_mode) { + case franka::RobotMode::kOther: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_OTHER; + break; + + case franka::RobotMode::kIdle: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_IDLE; + break; + + case franka::RobotMode::kMove: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_MOVE; + break; + + case franka::RobotMode::kGuiding: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_GUIDING; + break; + + case franka::RobotMode::kReflex: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_REFLEX; + break; + + case franka::RobotMode::kUserStopped: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_USER_STOPPED; + break; + + case franka::RobotMode::kAutomaticErrorRecovery: + publisher_franka_states_.msg_.robot_mode = franka_msgs::FrankaState::ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY; + break; + } + publisher_franka_states_.msg_.header.seq = sequence_number_; publisher_franka_states_.msg_.header.stamp = time; publisher_franka_states_.unlockAndPublish(); diff --git a/franka_msgs/msg/FrankaState.msg b/franka_msgs/msg/FrankaState.msg index 7f8f376..73b234a 100644 --- a/franka_msgs/msg/FrankaState.msg +++ b/franka_msgs/msg/FrankaState.msg @@ -31,5 +31,13 @@ float64[16] O_T_EE_d float64[16] F_T_EE float64[16] EE_T_K float64 time +uint8 ROBOT_MODE_OTHER=0 +uint8 ROBOT_MODE_IDLE=1 +uint8 ROBOT_MODE_MOVE=2 +uint8 ROBOT_MODE_GUIDING=3 +uint8 ROBOT_MODE_REFLEX=4 +uint8 ROBOT_MODE_USER_STOPPED=5 +uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6 +uint8 robot_mode franka_msgs/Errors current_errors franka_msgs/Errors last_motion_errors -- GitLab