Commit fa6a1e4c authored by carebare47's avatar carebare47
Browse files

add non-blocking-read for combined_robot_hw

parent 8cd1c9b2
......@@ -258,6 +258,9 @@ protected:
bool controller_reset_necessary_;
bool controllers_initialized_;
bool packet_read_;
bool non_blocking_read_;
std::string robot_ip_;
std::string tf_prefix_;
};
......
......@@ -62,11 +62,12 @@ public:
* and the robot controller.
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port. If
* the robot cannot connect to this port, `External Control` will stop immediately.
* \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002);
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, bool non_blocking_read = false);
/*!
* \brief Constructs a new UrDriver object.
*
......@@ -82,13 +83,14 @@ public:
* and the robot controller
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port.
* If the robot cannot connect to this port, `External Control` will stop immediately.
* \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
const uint32_t script_sender_port = 50002)
const uint32_t script_sender_port = 50002, bool non_blocking_read = false)
: UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port)
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, non_blocking_read)
{
}
......@@ -217,6 +219,9 @@ private:
bool in_headless_mode_;
std::string full_robot_program_;
int get_packet_timeout_;
bool non_blocking_read_;
VersionInformation robot_version_;
};
} // namespace ur_driver
......
......@@ -104,6 +104,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
return false;
}
// Enables non_blocking_read mode. Useful when used with combined_robot_hw
robot_hw_nh.param("non_blocking_read", non_blocking_read_, false);
// Whenever the runtime state of the "External Control" program node in the UR-program changes, a
// message gets published here. So this is equivalent to the information whether the robot accepts
// commands from ROS side.
......@@ -214,7 +217,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
headless_mode, std::move(tool_comm_setup), calibration_checksum,
(uint32_t)reverse_port, (uint32_t)script_sender_port));
(uint32_t)reverse_port, (uint32_t)script_sender_port, non_blocking_read_));
}
catch (ur_driver::ToolCommNotAvailable& e)
{
......@@ -361,6 +364,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
if (data_pkg)
{
packet_read_ = true;
readData(data_pkg, "actual_q", joint_positions_);
readData(data_pkg, "actual_qd", joint_velocities_);
readData(data_pkg, "target_speed_fraction", target_speed_fraction_);
......@@ -431,7 +435,9 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
}
else
{
ROS_ERROR("Could not get fresh data package from robot");
if (!non_blocking_read_){
ROS_ERROR("Could not get fresh data package from robot");
}
}
}
......@@ -439,7 +445,7 @@ 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)) &&
robot_program_running_)
robot_program_running_ && ((non_blocking_read_ && packet_read_) || (!non_blocking_read_)))
{
if (position_controller_running_)
{
......@@ -453,6 +459,7 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
{
ur_driver_->stopControl();
}
packet_read_ = false;
}
}
......
......@@ -51,7 +51,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
const std::string& output_recipe_file, const std::string& input_recipe_file,
std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
const uint32_t reverse_port, const uint32_t script_sender_port)
const uint32_t reverse_port, const uint32_t script_sender_port, bool non_blocking_read)
: servoj_time_(0.008)
, servoj_gain_(2000)
, servoj_lookahead_time_(0.03)
......@@ -72,6 +72,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
LOG_INFO("Checking if calibration data matches connected robot.");
checkCalibration(calibration_checksum);
non_blocking_read_ = non_blocking_read;
get_packet_timeout_ = non_blocking_read_ ? 0 : 100;
if (!rtde_client_->init())
{
throw UrException("Initialization of RTDE client went wrong.");
......@@ -141,8 +144,11 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
{
std::chrono::milliseconds timeout(100); // We deliberately have a quite large timeout here, as the robot itself
// should command the control loop's timing.
std::chrono::milliseconds timeout(get_packet_timeout_); // This can take one of two values, 0ms or 100ms. The large
// timeout is for when the robot is commanding the control
// loop's timing (read is blocking). The zero timeout is for
// when the robot is sharing a control loop with
// something else (combined_robot_hw)
return rtde_client_->getDataPackage(timeout);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment