Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

Added a service to hand back control to the robot

Browse Source 
This way, Ros can  be a part of a larger program.
This commit is contained in:
Felix Mauch
2019-07-16 09:31:45 +02:00
parent 600dac1dd3
commit bc683903b7
5 changed files with 52 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -193,6 +193,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
return true;
@@ -287,17 +289,22 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
{
if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING))
if ((runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)) &&
robot_program_running_)
{
if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_);
}
else
else if (robot_program_running_)
{
ur_driver_->writeKeepalive();
}
else
{
ur_driver_->stopControl();
}
}
}
@@ -448,4 +455,20 @@ void HardwareInterface ::checkCalibration(const std::string& checksum)
}
ROS_DEBUG_STREAM("Got calibration information from robot.");
}
bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
{
if (isRobotProgramRunning())
{
robot_program_running_ = false;
res.success = true;
res.message = "Deactivated control";
}
else
{
res.success = true;
res.message = "No control active. Nothing to do.";
}
return true;
}
} // namespace ur_driver

10
ur_rtde_driver/src/ur/ur_driver.cpp
View File

@@ -153,6 +153,16 @@ bool UrDriver::writeKeepalive()
return false;
}
bool UrDriver::stopControl()
{
if (reverse_interface_active_)
{
vector6d_t* fake = nullptr;
return reverse_interface_->write(fake, 0);
}
return false;
}
void UrDriver::startWatchdog()
{
handle_program_state_(false);