Skip to content

Commit

Permalink
adjust return type of checkCalibration, passing calibration_checksum …
Browse files Browse the repository at this point in the history
…in constructor is deprecated
  • Loading branch information
bergmannto committed Jan 15, 2024
1 parent 9ff97fb commit d83409a
Show file tree
Hide file tree
Showing 2 changed files with 66 additions and 6 deletions.
33 changes: 32 additions & 1 deletion include/ur_client_library/ur/ur_driver_low_bandwidth.h
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,34 @@ namespace urcl
class UrDriverLowBandwidth
{
public:
/*!
* \brief Constructs a new UrDriver object.
* \param robot_ip IP-address under which the robot is reachable.
* \param script_file URScript file that should be sent to the robot.
* \param output_recipe_file Filename where the output recipe is stored in.
* \param input_recipe_file Filename where the input recipe is stored in.
* \param handle_program_state Function handle to a callback on program state changes. For this to
* work, the URScript program will have to send keepalive signals to the \p reverse_port. I no
* keepalive signal can be read, program state will be false.
* \param headless_mode Parameter to control if the driver should be started in headless mode.
* \param tool_comm_setup Configuration for using the tool communication.
* \param reverse_port Port that will be opened by the driver to allow direct communication between the driver
* 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)
* \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
* \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
* \param servoj_time_waiting
* \param max_joint_difference
* \param max_velocity
*/
UrDriverLowBandwidth(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 uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002,
double servoj_time_waiting = 0.001, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false,
double max_joint_difference = 0.0001, double max_velocity = 10.0);

/*!
* \brief Constructs a new UrDriver object.
* Upon initialization this class will check the calibration checksum reported from the robot and
Expand Down Expand Up @@ -189,8 +217,11 @@ class UrDriverLowBandwidth
* \brief Checks if the kinematics information in the used model fits the actual robot.
*
* \param checksum Hash of the used kinematics information
* \returns True if the robot's calibration checksum matches the one given to the checker. False
* if it doesn't match or the check was not yet performed.
*/
void checkCalibration(const std::string& checksum);
bool checkCalibration(const std::string& checksum);

/*!
* \brief Getter for the RTDE writer used to write to the robot's RTDE interface.
Expand Down
39 changes: 34 additions & 5 deletions src/ur/ur_driver_low_bandwidth.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
urcl::UrDriverLowBandwidth::UrDriverLowBandwidth(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, const uint32_t script_sender_port, double servoj_time_waiting, int servoj_gain,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port,
const uint32_t script_sender_port, double servoj_time_waiting, int servoj_gain,
double servoj_lookahead_time, bool non_blocking_read,
double max_joint_difference, double max_velocity)
: servoj_time_(0.002)
Expand All @@ -79,8 +79,6 @@ urcl::UrDriverLowBandwidth::UrDriverLowBandwidth(const std::string& robot_ip, co
secondary_stream_.reset(
new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT));
secondary_stream_->connect();
URCL_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;
Expand Down Expand Up @@ -186,6 +184,36 @@ urcl::UrDriverLowBandwidth::UrDriverLowBandwidth(const std::string& robot_ip, co
URCL_LOG_DEBUG("Initialization done");
}

urcl::UrDriverLowBandwidth::UrDriverLowBandwidth(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, const uint32_t script_sender_port, double servoj_time_waiting, int servoj_gain,
double servoj_lookahead_time, bool non_blocking_read,
double max_joint_difference, double max_velocity)
: UrDriverLowBandwidth(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_time_waiting, servoj_gain, servoj_lookahead_time,
non_blocking_read, max_joint_difference, max_velocity)
{
URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been "
"deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This "
"notice is for application developers using this library. If you are only using an application using "
"this library, you can ignore this message.");
if (checkCalibration(calibration_checksum))
{
URCL_LOG_INFO("Calibration checked successfully.");
}
else
{
URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics "
"config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use "
"the ur_calibration tool to extract the correct calibration from the robot and pass that into the "
"description. See "
"[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] "
"for details.");
}
}

std::unique_ptr<rtde_interface::DataPackage> urcl::UrDriverLowBandwidth::getDataPackage()
{
// This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control
Expand Down Expand Up @@ -267,7 +295,7 @@ std::string UrDriverLowBandwidth::readScriptFile(const std::string& filename)
return content;
}

void UrDriverLowBandwidth::checkCalibration(const std::string& checksum)
bool UrDriverLowBandwidth::checkCalibration(const std::string& checksum)
{
if (primary_stream_ == nullptr)
{
Expand All @@ -289,6 +317,7 @@ void UrDriverLowBandwidth::checkCalibration(const std::string& checksum)
std::this_thread::sleep_for(std::chrono::seconds(1));
}
URCL_LOG_DEBUG("Got calibration information from robot.");
return consumer.checkSuccessful();
}

rtde_interface::RTDEWriter& UrDriverLowBandwidth::getRTDEWriter()
Expand Down

0 comments on commit d83409a

Please sign in to comment.