diff --git a/ft_gui/ft_gui/ft_calibration_gui.py b/ft_gui/ft_gui/ft_calibration_gui.py index b4380f3..c829cea 100644 --- a/ft_gui/ft_gui/ft_calibration_gui.py +++ b/ft_gui/ft_gui/ft_calibration_gui.py @@ -136,7 +136,7 @@ def connect_ros_calib(self, state): "background-color: rgb(255,0,51);" "border-radius:5px;" ) - self.node.get_logger().warning(f"An error occurred:{error}") + self.node.get_logger().warning(f"An error occurred: {error}") self.node.get_logger().error('Failled to initialize ROS2 comms!') else: self.is_connected = False diff --git a/ft_tools/src/ft_calibration_node.cpp b/ft_tools/src/ft_calibration_node.cpp index a6c5b1a..535b35b 100644 --- a/ft_tools/src/ft_calibration_node.cpp +++ b/ft_tools/src/ft_calibration_node.cpp @@ -292,6 +292,15 @@ bool FtCalibrationNode::init_kinematics_monitoring() return false; } bool all_ok = true; + + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!this->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR(this->get_logger(), "parameter robot_description not set"); + return false; + } + auto robot_description = robot_param.as_string(); + // Load the differential IK plugin if (!parameters_.kinematics.plugin_name.empty()) { try { @@ -302,7 +311,7 @@ bool FtCalibrationNode::init_kinematics_monitoring() kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - this->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, this->get_node_parameters_interface(), "kinematics")) { all_ok &= false; } diff --git a/ft_tools/src/ft_estimation_node.cpp b/ft_tools/src/ft_estimation_node.cpp index 6faff56..b245430 100644 --- a/ft_tools/src/ft_estimation_node.cpp +++ b/ft_tools/src/ft_estimation_node.cpp @@ -279,6 +279,15 @@ bool FtEstimationNode::init_kinematics_monitoring() return false; } bool all_ok = true; + + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!this->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR(this->get_logger(), "parameter robot_description not set"); + return false; + } + auto robot_description = robot_param.as_string(); + // Load the differential IK plugin if (!parameters_.kinematics.plugin_name.empty()) { try { @@ -289,7 +298,7 @@ bool FtEstimationNode::init_kinematics_monitoring() kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - this->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, this->get_node_parameters_interface(), "kinematics")) { all_ok &= false; }