diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos index f7dd6bee1..632ca647c 100644 --- a/Universal_Robots_ROS2_Driver.jazzy.repos +++ b/Universal_Robots_ROS2_Driver.jazzy.repos @@ -5,8 +5,8 @@ repositories: version: master Universal_Robots_ROS2_Description: type: git - url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: rolling + url: https://github.com/fmauch/Universal_Robots_ROS2_Description.git + version: pose_broadcaster ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git @@ -17,8 +17,8 @@ repositories: version: master ros2_controllers: type: git - url: https://github.com/ros-controls/ros2_controllers - version: master + url: https://github.com/RobertWilbrandt/ros2_controllers + version: pose_broadcaster kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index f7dd6bee1..632ca647c 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -5,8 +5,8 @@ repositories: version: master Universal_Robots_ROS2_Description: type: git - url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: rolling + url: https://github.com/fmauch/Universal_Robots_ROS2_Description.git + version: pose_broadcaster ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git @@ -17,8 +17,8 @@ repositories: version: master ros2_controllers: type: git - url: https://github.com/ros-controls/ros2_controllers - version: master + url: https://github.com/RobertWilbrandt/ros2_controllers + version: pose_broadcaster kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..60cb6290c 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + tcp_pose_broadcaster: + type: pose_broadcaster/PoseBroadcaster speed_scaling_state_broadcaster: ros__parameters: @@ -124,3 +126,8 @@ forward_position_controller: - $(var tf_prefix)wrist_1_joint - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint + +tcp_pose_broadcaster: + ros__parameters: + frame_id: $(var tf_prefix)base + pose_name: $(var tf_prefix)tcp_pose diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 67e27a222..533c559ec 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -144,6 +144,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::vector6d_t urcl_joint_efforts_; urcl::vector6d_t urcl_ft_sensor_measurements_; urcl::vector6d_t urcl_tcp_pose_; + urcl::vector3d_t tcp_orientation_rpy_; + tf2::Quaternion tcp_rotation_quat_; bool packet_read_; @@ -172,7 +174,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // transform stuff tf2::Vector3 tcp_force_; tf2::Vector3 tcp_torque_; - geometry_msgs::msg::TransformStamped tcp_transform_; // asynchronous commands std::array standard_dig_out_bits_cmd_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c884086fa..e81a7fcfe 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -127,6 +127,7 @@ def launch_setup(): "force_torque_sensor_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", + "tcp_pose_broadcaster", ] }, ], @@ -162,6 +163,7 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "tcp_pose_broadcaster", ] controllers_inactive = ["forward_position_controller"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5826cf595..7d6705c5b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -169,9 +169,14 @@ std::vector URPositionHardwareInterface::exp &speed_scaling_combined_)); for (auto& sensor : info_.sensors) { - for (uint j = 0; j < sensor.state_interfaces.size(); ++j) { - state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name, - &urcl_ft_sensor_measurements_[j])); + if (sensor.name == tf_prefix + "tcp_fts_sensor") { + const std::vector fts_names = { + "force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z" + }; + for (uint j = 0; j < 6; ++j) { + state_interfaces.emplace_back( + hardware_interface::StateInterface(sensor.name, fts_names[j], &urcl_ft_sensor_measurements_[j])); + } } } @@ -231,6 +236,19 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.x", &urcl_tcp_pose_[0])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.y", &urcl_tcp_pose_[1])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.z", &urcl_tcp_pose_[2])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.r", &tcp_orientation_rpy_[0])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.p", &tcp_orientation_rpy_[1])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.y", &tcp_orientation_rpy_[2])); + return state_interfaces; } @@ -765,10 +783,8 @@ void URPositionHardwareInterface::transformForceTorque() tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4], urcl_ft_sensor_measurements_[5]); - tf2::Quaternion rotation_quat; - tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat); - tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_); - tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_); + tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_); + tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_); urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(), tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() }; @@ -781,17 +797,13 @@ void URPositionHardwareInterface::extractToolPose() std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2)); tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]); - tf2::Quaternion rotation; if (tcp_angle > 1e-16) { - rotation.setRotation(rotation_vec.normalized(), tcp_angle); + tcp_rotation_quat_.setRotation(rotation_vec.normalized(), tcp_angle); } else { - rotation.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid + tcp_rotation_quat_.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid } - tcp_transform_.transform.translation.x = urcl_tcp_pose_[0]; - tcp_transform_.transform.translation.y = urcl_tcp_pose_[1]; - tcp_transform_.transform.translation.z = urcl_tcp_pose_[2]; - - tcp_transform_.transform.rotation = tf2::toMsg(rotation); + auto rot_mat = tf2::Matrix3x3(tcp_rotation_quat_); + rot_mat.getRPY(tcp_orientation_rpy_[0], tcp_orientation_rpy_[1], tcp_orientation_rpy_[2]); } hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch(