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.

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

Updated for primary client improvement #576

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
11 changes: 11 additions & 0 deletions ur_bringup/launch/ur_control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ def launch_setup(context, *args, **kwargs):
activate_joint_controller = LaunchConfiguration("activate_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
headless_mode = LaunchConfiguration("headless_mode")
simulated_robot = LaunchConfiguration("simulated_robot")
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
use_tool_communication = LaunchConfiguration("use_tool_communication")
tool_parity = LaunchConfiguration("tool_parity")
Expand Down Expand Up @@ -145,6 +146,9 @@ def launch_setup(context, *args, **kwargs):
"headless_mode:=",
headless_mode,
" ",
"simulated_robot:=",
simulated_robot,
" ",
"use_tool_communication:=",
use_tool_communication,
" ",
Expand Down Expand Up @@ -449,6 +453,13 @@ def generate_launch_description():
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"simulated_robot",
default_value="false",
description="Determine whether robot is simulated or real",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
Expand Down
11 changes: 11 additions & 0 deletions ur_robot_driver/launch/ur_control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ def launch_setup(context, *args, **kwargs):
activate_joint_controller = LaunchConfiguration("activate_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
headless_mode = LaunchConfiguration("headless_mode")
simulated_robot = LaunchConfiguration("simulated_robot")
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
use_tool_communication = LaunchConfiguration("use_tool_communication")
tool_parity = LaunchConfiguration("tool_parity")
Expand Down Expand Up @@ -145,6 +146,9 @@ def launch_setup(context, *args, **kwargs):
"headless_mode:=",
headless_mode,
" ",
"simulated_robot:=",
simulated_robot,
" ",
"use_tool_communication:=",
use_tool_communication,
" ",
Expand Down Expand Up @@ -441,6 +445,13 @@ def generate_launch_description():
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"simulated_robot",
default_value="false",
description="Determine whether the robot is simulated or real",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
Expand Down
17 changes: 4 additions & 13 deletions ur_robot_driver/src/hardware_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -377,10 +377,13 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
registerUrclLogHandler();
try {
bool simulated_robot = false;
simulated_robot = (info_.hardware_parameters["simulated_robot"] == "true") ||
(info_.hardware_parameters["simulated_robot"] == "True");
ur_driver_ = std::make_unique<urcl::UrDriver>(
robot_ip, script_filename, output_recipe_filename, input_recipe_filename,
std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode,
std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
std::move(tool_comm_setup), calibration_checksum, simulated_robot, (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
servoj_lookahead_time, non_blocking_read_);
} catch (urcl::ToolCommNotAvailable& e) {
RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication");
Expand All @@ -394,18 +397,6 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
calibration_checksum.c_str());
// check calibration
// https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/c3378599d5fa73a261328b326392e847f312ab6b/ur_robot_driver/src/hardware_interface.cpp#L296-L309
if (ur_driver_->checkCalibration(calibration_checksum)) {
RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Calibration checked successfully.");
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("URPositionHardwareInterface"),

"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_ROS2_Driver/blob/main/ur_calibration/"
"README.md] for details.");
}

ur_driver_->startRTDECommunication();

Expand Down
1 change: 1 addition & 0 deletions ur_robot_driver/test/robot_driver.py
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,7 @@ def generate_test_description():
"launch_rviz": "false",
"initial_joint_controller": "scaled_joint_trajectory_controller",
"headless_mode": "true",
"simulated_robot": "true",
"launch_dashboard_client": "false",
"start_joint_controller": "false",
}.items(),
Expand Down