diff --git a/CMakeLists.txt b/CMakeLists.txt index 9fe8476..1de2464 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED std_srvs tf trajectory_msgs + ur_controllers ur_msgs ) find_package(Boost REQUIRED) @@ -40,6 +41,7 @@ catkin_package( roscpp sensor_msgs trajectory_msgs + ur_controllers ur_msgs DEPENDS Boost diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml index ed86df6..3166578 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -22,6 +22,11 @@ force_torque_sensor_controller: type: force_torque_sensor_controller/ForceTorqueSensorController publish_rate: 125 +# Publish speed_scaling factor +speed_scaling_state_controller: + type: ur_controllers/SpeedScalingStateController + publish_rate: 125 + # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller pos_based_pos_traj_controller: @@ -104,4 +109,4 @@ joint_group_vel_controller: - elbow_joint - wrist_1_joint - wrist_2_joint - - wrist_3_joint \ No newline at end of file + - wrist_3_joint diff --git a/include/ur_rtde_driver/ros/hardware_interface.h b/include/ur_rtde_driver/ros/hardware_interface.h index 1ef712e..48abb6f 100644 --- a/include/ur_rtde_driver/ros/hardware_interface.h +++ b/include/ur_rtde_driver/ros/hardware_interface.h @@ -33,6 +33,8 @@ #include #include +#include + #include "ur_rtde_driver/ur/ur_driver.h" namespace ur_driver @@ -57,6 +59,7 @@ protected: hardware_interface::JointStateInterface js_interface_; hardware_interface::PositionJointInterface pj_interface_; + ur_controllers::SpeedScalingInterface speedsc_interface_; // hardware_interface::VelocityJointInterface vj_interface_; vector6d_t joint_position_command_; @@ -64,6 +67,7 @@ protected: vector6d_t joint_positions_; vector6d_t joint_velocities_; vector6d_t joint_efforts_; + double speed_scaling_value_; std::vector joint_names_; bool position_controller_running_; diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index 5a67b19..445d971 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -16,7 +16,7 @@ - + diff --git a/package.xml b/package.xml index f722e9b..69b99c6 100644 --- a/package.xml +++ b/package.xml @@ -29,6 +29,7 @@ roscpp sensor_msgs trajectory_msgs + ur_controllers ur_msgs tf std_srvs diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index c2e1893..e2f94e4 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -69,9 +69,13 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); } + speedsc_interface_.registerHandle( + ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_)); + // Register interfaces registerInterface(&js_interface_); registerInterface(&pj_interface_); + registerInterface(&speedsc_interface_); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); @@ -93,6 +97,11 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period // This throwing should never happen unless misconfigured throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!"); } + if (!data_pkg->getData("target_speed_fraction", speed_scaling_value_)) + { + // This throwing should never happen unless misconfigured + throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!"); + } } else {