Skip to content

Add free drive mode support #718

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

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+37 −1
Open

Add free drive mode support #718

Show file tree
Hide file tree
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
7 changes: 7 additions & 0 deletions ur_robot_driver/include/ur_robot_driver/hardware_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,6 +213,7 @@ class HardwareInterface : public hardware_interface::RobotHW
bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
void commandCallback(const std_msgs::StringConstPtr& msg);
void freeDriveModeCallback(const std_msgs::BoolConstPtr& msg);
bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& res);
bool activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res);

Expand Down Expand Up @@ -318,6 +319,12 @@ class HardwareInterface : public hardware_interface::RobotHW
ros::ServiceServer resend_robot_program_srv_;
ros::Subscriber command_sub_;

ros::Subscriber free_drive_mode_sub_;
double free_drive_mode_timeout_;
bool free_drive_mode_requested_;
ros::Time free_drive_mode_latest_request_;
urcl::control::FreedriveControlMessage free_drive_mode_control_msg_;

industrial_robot_status_interface::RobotStatus robot_status_resource_{};
industrial_robot_status_interface::IndustrialRobotStatusInterface robot_status_interface_{};

Expand Down
31 changes: 30 additions & 1 deletion ur_robot_driver/src/hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,13 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
// end
command_sub_ = robot_hw_nh.subscribe("script_command", 1, &HardwareInterface::commandCallback, this);

// Create free drive functionality
free_drive_mode_sub_ = robot_hw_nh.subscribe("free_drive_mode", 1, &HardwareInterface::freeDriveModeCallback, this);
// timeout in sec from last received free_drive msg until free drive mode is automatically disabled
free_drive_mode_timeout_ = robot_hw_nh.param("free_drive_mode_timeout", 1.0);
free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_STOP;
free_drive_mode_requested_ = false;

// Names of the joints. Usually, this is given in the controller config file.
if (!robot_hw_nh.getParam("joints", joint_names_))
{
Expand Down Expand Up @@ -686,7 +693,18 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PAUSING)) &&
robot_program_running_ && (!non_blocking_read_ || packet_read_))
{
if (position_controller_running_)
if (free_drive_mode_requested_ || free_drive_mode_control_msg_ != urcl::control::FreedriveControlMessage::FREEDRIVE_STOP) {
if (ros::Time::now() > (free_drive_mode_latest_request_ + ros::Duration(free_drive_mode_timeout_)) || !free_drive_mode_requested_) {
free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_STOP;
free_drive_mode_requested_ = false;
} else if (free_drive_mode_control_msg_ == urcl::control::FreedriveControlMessage::FREEDRIVE_STOP) {
free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_START;
} else {
free_drive_mode_control_msg_ = urcl::control::FreedriveControlMessage::FREEDRIVE_NOOP;
}
ur_driver_->writeFreedriveControlMessage(free_drive_mode_control_msg_);
}
else if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_, urcl::comm::ControlMode::MODE_SERVOJ);
}
Expand Down Expand Up @@ -1199,6 +1217,17 @@ void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg)
}
}

void HardwareInterface::freeDriveModeCallback(const std_msgs::BoolConstPtr& msg)
{
if (ur_driver_ == nullptr)
{
throw std::runtime_error("Trying to use the ur_driver_ member before it is initialized. This should not happen, "
"please contact the package maintainer.");
}
free_drive_mode_latest_request_ = ros::Time::now();
free_drive_mode_requested_ = msg->data;
}

bool HardwareInterface::activateSplineInterpolation(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res)
{
use_spline_interpolation_ = req.data;
Expand Down