Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Error Handling for Hardware Interface #69

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

Error Handling for Hardware Interface #69

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
201 changes: 111 additions & 90 deletions franka_hardware/src/franka_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,13 @@ CallbackReturn FrankaHardwareInterface::on_activate(
CallbackReturn FrankaHardwareInterface::on_deactivate(
const rclcpp_lifecycle::State& /*previous_state*/) {
RCLCPP_INFO(getLogger(), "trying to Stop...");
robot_->stopRobot();
try {
robot_->stopRobot();
} catch (const franka::Exception& e) {
RCLCPP_ERROR(getLogger(), e.what());
return CallbackReturn::ERROR;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess when there is an error in this stage. HardwareInterface will go back to the on_configure() stage. Is it possible that the robot initializes itself after error is acknowledged. Or what exactly will happen when we are back in the on_configure() stage?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@BarisYazici Thank you, sorry for the late reply. I am not sure how or when the robot initializes itself. Does that happen in firmware, or in libfranka, or both? I do not know much about Franka internals or libfranka, so I am probably not best equipped to write code which manages Franka's internal state.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It was a question more on how you want to use this feature. Because if the robot goes in the reflex mode, by throwing an exception, the hardware interface will not be functioning. But the controllers will still be running without noticing that the hardware interface is in Error state.

It's mostly a question of how you wanna use this feature and how the ROS 2 hardware interface work. So with this change the hardware interface and the robot will not be updating but the controllers will continue to work. I don't think this is an expected behavior for the users. So unless we have a logic to recover the robot from the error state, I don't see the value of this feature. So maybe you can mention the use-case you have and we can try to suit the feature to your use-case. Maybe this involves going back to the configure stage or introducing a recovery state in the hardware interface. But in this current stage if we catch the exception we will not allow the exception to escalate, which might be misleading because they wouldn't understand why their controller stopped working.

}

RCLCPP_INFO(getLogger(), "Stopped");
return CallbackReturn::SUCCESS;
}
Expand Down Expand Up @@ -165,10 +171,15 @@ void FrankaHardwareInterface::initializePositionCommands(const franka::RobotStat

hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/) {
if (hw_franka_model_ptr_ == nullptr) {
hw_franka_model_ptr_ = robot_->getModel();
try {
if (hw_franka_model_ptr_ == nullptr) {
hw_franka_model_ptr_ = robot_->getModel();
}
hw_franka_robot_state_ = robot_->readOnce();
} catch (const franka::Exception& e) {
RCLCPP_ERROR(getLogger(), e.what());
return hardware_interface::return_type::ERROR;
}
hw_franka_robot_state_ = robot_->readOnce();

initializePositionCommands(hw_franka_robot_state_);

Expand All @@ -193,29 +204,34 @@ hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Tim
return hardware_interface::return_type::ERROR;
}

if (velocity_joint_interface_running_) {
robot_->writeOnce(hw_velocity_commands_);
} else if (effort_interface_running_) {
robot_->writeOnce(hw_effort_commands_);
} else if (position_joint_interface_running_ && !first_position_update_ &&
!initial_joint_position_update_) {
robot_->writeOnce(hw_position_commands_);
} else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ &&
!first_elbow_update_) {
// Wait until the first read pass after robot controller is activated to write the elbow
// command to the robot
robot_->writeOnce(hw_cartesian_velocities_, hw_elbow_command_);
} else if (pose_cartesian_interface_running_ && elbow_command_interface_running_ &&
!first_cartesian_pose_update_ && !first_elbow_update_) {
// Wait until the first read pass after robot controller is activated to write the elbow
// command to the robot
robot_->writeOnce(hw_cartesian_pose_, hw_elbow_command_);
} else if (pose_cartesian_interface_running_ && !first_cartesian_pose_update_) {
// Wait until the first read pass after robot controller is activated to write the cartesian
// pose
robot_->writeOnce(hw_cartesian_pose_);
} else if (velocity_cartesian_interface_running_ && !elbow_command_interface_running_) {
robot_->writeOnce(hw_cartesian_velocities_);
try {
if (velocity_joint_interface_running_) {
robot_->writeOnce(hw_velocity_commands_);
} else if (effort_interface_running_) {
robot_->writeOnce(hw_effort_commands_);
} else if (position_joint_interface_running_ && !first_position_update_ &&
!initial_joint_position_update_) {
robot_->writeOnce(hw_position_commands_);
} else if (velocity_cartesian_interface_running_ && elbow_command_interface_running_ &&
!first_elbow_update_) {
// Wait until the first read pass after robot controller is activated to write the elbow
// command to the robot
robot_->writeOnce(hw_cartesian_velocities_, hw_elbow_command_);
} else if (pose_cartesian_interface_running_ && elbow_command_interface_running_ &&
!first_cartesian_pose_update_ && !first_elbow_update_) {
// Wait until the first read pass after robot controller is activated to write the elbow
// command to the robot
robot_->writeOnce(hw_cartesian_pose_, hw_elbow_command_);
} else if (pose_cartesian_interface_running_ && !first_cartesian_pose_update_) {
// Wait until the first read pass after robot controller is activated to write the cartesian
// pose
robot_->writeOnce(hw_cartesian_pose_);
} else if (velocity_cartesian_interface_running_ && !elbow_command_interface_running_) {
robot_->writeOnce(hw_cartesian_velocities_);
}
} catch (const franka::Exception& e) {
RCLCPP_ERROR(getLogger(), e.what());
return hardware_interface::return_type::ERROR;
}

return hardware_interface::return_type::OK;
Expand Down Expand Up @@ -313,75 +329,80 @@ rclcpp::Logger FrankaHardwareInterface::getLogger() {
hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_switch(
const std::vector<std::string>& /*start_interfaces*/,
const std::vector<std::string>& /*stop_interfaces*/) {
if (!effort_interface_running_ && effort_interface_claimed_) {
hw_effort_commands_.fill(0);
robot_->stopRobot();
robot_->initializeTorqueInterface();
effort_interface_running_ = true;
} else if (effort_interface_running_ && !effort_interface_claimed_) {
robot_->stopRobot();
effort_interface_running_ = false;
}

if (!velocity_joint_interface_running_ && velocity_joint_interface_claimed_) {
hw_velocity_commands_.fill(0);
robot_->stopRobot();
robot_->initializeJointVelocityInterface();
velocity_joint_interface_running_ = true;
} else if (velocity_joint_interface_running_ && !velocity_joint_interface_claimed_) {
robot_->stopRobot();
velocity_joint_interface_running_ = false;
}

if (!position_joint_interface_running_ && position_joint_interface_claimed_) {
robot_->stopRobot();
robot_->initializeJointPositionInterface();
position_joint_interface_running_ = true;
first_position_update_ = true;
initial_joint_position_update_ = true;
} else if (position_joint_interface_running_ && !position_joint_interface_claimed_) {
robot_->stopRobot();
position_joint_interface_running_ = false;
}
try {
if (!effort_interface_running_ && effort_interface_claimed_) {
hw_effort_commands_.fill(0);
robot_->stopRobot();
robot_->initializeTorqueInterface();
effort_interface_running_ = true;
} else if (effort_interface_running_ && !effort_interface_claimed_) {
robot_->stopRobot();
effort_interface_running_ = false;
}

if (!velocity_cartesian_interface_running_ && velocity_cartesian_interface_claimed_) {
hw_cartesian_velocities_.fill(0);
robot_->stopRobot();
robot_->initializeCartesianVelocityInterface();
if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) {
elbow_command_interface_running_ = true;
first_elbow_update_ = true;
if (!velocity_joint_interface_running_ && velocity_joint_interface_claimed_) {
hw_velocity_commands_.fill(0);
robot_->stopRobot();
robot_->initializeJointVelocityInterface();
velocity_joint_interface_running_ = true;
} else if (velocity_joint_interface_running_ && !velocity_joint_interface_claimed_) {
robot_->stopRobot();
velocity_joint_interface_running_ = false;
}
velocity_cartesian_interface_running_ = true;
} else if (velocity_cartesian_interface_running_ && !velocity_cartesian_interface_claimed_) {
robot_->stopRobot();
// Elbow command interface can't be commanded without cartesian velocity or pose interface
if (elbow_command_interface_running_) {
elbow_command_interface_running_ = false;
elbow_command_interface_claimed_ = false;

if (!position_joint_interface_running_ && position_joint_interface_claimed_) {
robot_->stopRobot();
robot_->initializeJointPositionInterface();
position_joint_interface_running_ = true;
first_position_update_ = true;
initial_joint_position_update_ = true;
} else if (position_joint_interface_running_ && !position_joint_interface_claimed_) {
robot_->stopRobot();
position_joint_interface_running_ = false;
}
velocity_cartesian_interface_running_ = false;
}

if (!pose_cartesian_interface_running_ && pose_cartesian_interface_claimed_) {
robot_->stopRobot();
robot_->initializeCartesianPoseInterface();
if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) {
elbow_command_interface_running_ = true;
first_elbow_update_ = true;
initial_elbow_state_update_ = true;
if (!velocity_cartesian_interface_running_ && velocity_cartesian_interface_claimed_) {
hw_cartesian_velocities_.fill(0);
robot_->stopRobot();
robot_->initializeCartesianVelocityInterface();
if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) {
elbow_command_interface_running_ = true;
first_elbow_update_ = true;
}
velocity_cartesian_interface_running_ = true;
} else if (velocity_cartesian_interface_running_ && !velocity_cartesian_interface_claimed_) {
robot_->stopRobot();
// Elbow command interface can't be commanded without cartesian velocity or pose interface
if (elbow_command_interface_running_) {
elbow_command_interface_running_ = false;
elbow_command_interface_claimed_ = false;
}
velocity_cartesian_interface_running_ = false;
}
pose_cartesian_interface_running_ = true;
initial_robot_state_update_ = true;
first_cartesian_pose_update_ = true;
} else if (pose_cartesian_interface_running_ && !pose_cartesian_interface_claimed_) {
robot_->stopRobot();
// Elbow command interface can't be commanded without cartesian pose or pose interface
if (elbow_command_interface_running_) {
elbow_command_interface_running_ = false;
elbow_command_interface_claimed_ = false;

if (!pose_cartesian_interface_running_ && pose_cartesian_interface_claimed_) {
robot_->stopRobot();
robot_->initializeCartesianPoseInterface();
if (!elbow_command_interface_running_ && elbow_command_interface_claimed_) {
elbow_command_interface_running_ = true;
first_elbow_update_ = true;
initial_elbow_state_update_ = true;
}
pose_cartesian_interface_running_ = true;
initial_robot_state_update_ = true;
first_cartesian_pose_update_ = true;
} else if (pose_cartesian_interface_running_ && !pose_cartesian_interface_claimed_) {
robot_->stopRobot();
// Elbow command interface can't be commanded without cartesian pose or pose interface
if (elbow_command_interface_running_) {
elbow_command_interface_running_ = false;
elbow_command_interface_claimed_ = false;
}
pose_cartesian_interface_running_ = false;
}
pose_cartesian_interface_running_ = false;
} catch (const franka::Exception& e) {
RCLCPP_ERROR(getLogger(), e.what());
return hardware_interface::return_type::ERROR;
}

// check if the elbow command is activated without cartesian command interface
Expand Down