From 4106aa9fb82bd7dd17540ad444dac772e742cbc5 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 7 May 2019 14:30:45 +0200 Subject: [PATCH] get frequency from robot --- .../hardware_interface_adapter.h | 28 +++++++++++-------- .../speed_scaling_state_controller.h | 2 +- .../scaled_joint_trajectory_controller.cpp | 1 - .../src/speed_scaling_state_controller.cpp | 1 - .../include/ur_rtde_driver/rtde/rtde_client.h | 7 +++++ .../include/ur_rtde_driver/ur/ur_driver.h | 5 ++-- ur_rtde_driver/src/ros/hardware_interface.cpp | 12 ++++---- .../src/ros/hardware_interface_node.cpp | 5 +++- ur_rtde_driver/src/rtde/rtde_client.cpp | 8 +++--- ur_rtde_driver/src/ur/ur_driver.cpp | 11 ++++++-- 10 files changed, 49 insertions(+), 31 deletions(-) diff --git a/ur_controllers/include/ur_controllers/hardware_interface_adapter.h b/ur_controllers/include/ur_controllers/hardware_interface_adapter.h index 286d81b..8fea250 100644 --- a/ur_controllers/include/ur_controllers/hardware_interface_adapter.h +++ b/ur_controllers/include/ur_controllers/hardware_interface_adapter.h @@ -15,7 +15,6 @@ #ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED #define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED - #include #include "ur_controllers/scaled_joint_command_interface.h" @@ -43,20 +42,24 @@ template class HardwareInterfaceAdapter { public: - HardwareInterfaceAdapter() : joint_handles_ptr_(0) {} + HardwareInterfaceAdapter() : joint_handles_ptr_(0) + { + } bool init(std::vector& joint_handles, ros::NodeHandle& /*controller_nh*/) { // Store pointer to joint handles joint_handles_ptr_ = &joint_handles; - return true; } void starting(const ros::Time& /*time*/) { - if (!joint_handles_ptr_) {return;} + if (!joint_handles_ptr_) + { + return; + } // Semantic zero for commands for (auto& jh : *joint_handles_ptr_) @@ -65,20 +68,23 @@ public: } } - void stopping(const ros::Time& /*time*/) {} + void stopping(const ros::Time& /*time*/) + { + } - void updateCommand(const ros::Time& /*time*/, - const ros::Duration& /*period*/, - const State& desired_state, - const State& /*state_error*/) + void updateCommand(const ros::Time& /*time*/, const ros::Duration& /*period*/, const State& desired_state, + const State& /*state_error*/) { // Forward desired position to command const unsigned int n_joints = joint_handles_ptr_->size(); - for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);} + for (unsigned int i = 0; i < n_joints; ++i) + { + (*joint_handles_ptr_)[i].setCommand(desired_state.position[i]); + } } private: std::vector* joint_handles_ptr_; }; -#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED +#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h b/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h index cd7ffd1..47d7c36 100644 --- a/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h +++ b/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h @@ -36,7 +36,7 @@ public: private: std::vector sensors_; - //TODO: We should use a better datatype later on + // TODO: We should use a better datatype later on typedef std::shared_ptr > RtPublisherPtr; std::vector realtime_pubs_; std::vector last_publish_times_; diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 523ae19..5c5cb35 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -18,7 +18,6 @@ #include #include - namespace position_controllers { typedef ur_controllers::ScaledJointTrajectoryController, diff --git a/ur_controllers/src/speed_scaling_state_controller.cpp b/ur_controllers/src/speed_scaling_state_controller.cpp index c3a555c..293f0f2 100644 --- a/ur_controllers/src/speed_scaling_state_controller.cpp +++ b/ur_controllers/src/speed_scaling_state_controller.cpp @@ -86,4 +86,3 @@ void SpeedScalingStateController ::stopping(const ros::Time& /*time*/) } // namespace ur_controllers PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerBase) - diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h index 98fb1ac..cc88850 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h @@ -57,12 +57,19 @@ public: bool start(); bool getDataPackage(std::unique_ptr>& data_package, std::chrono::milliseconds timeout); + double getMaxFrequency() const + { + return max_frequency_; + } + private: comm::URStream stream_; RTDEParser parser_; comm::URProducer prod_; comm::Pipeline pipeline_; + double max_frequency_; + constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index b548815..9fd58f8 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -56,7 +56,7 @@ public: */ std::unique_ptr getDataPackage(); - const uint32_t& getControlFrequency() const + uint32_t getControlFrequency() const { return rtde_frequency_; } @@ -64,8 +64,7 @@ public: bool writeJointCommand(const vector6d_t& values); private: - //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series. - uint32_t rtde_frequency_; + int rtde_frequency_; comm::INotifier notifier_; std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 557d59c..e7143da 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -30,10 +30,10 @@ namespace ur_driver { HardwareInterface::HardwareInterface() - : joint_position_command_{ 0, 0, 0, 0, 0, 0 } - , joint_positions_{ 0, 0, 0, 0, 0, 0 } - , joint_velocities_{ 0, 0, 0, 0, 0, 0 } - , joint_efforts_{ 0, 0, 0, 0, 0, 0 } + : joint_position_command_({ 0, 0, 0, 0, 0, 0 }) + , joint_positions_{ { 0, 0, 0, 0, 0, 0 } } + , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } } + , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } } , joint_names_(6) , position_controller_running_(false) @@ -42,8 +42,8 @@ HardwareInterface::HardwareInterface() bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - joint_velocities_ = { 0, 0, 0, 0, 0, 0 }; - joint_efforts_ = { 0, 0, 0, 0, 0, 0 }; + joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } }; + joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; std::string ROBOT_IP = robot_hw_nh.param("robot_ip", "192.168.56.101"); ROS_INFO_STREAM("Initializing urdriver"); diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp index d8dfa63..8944407 100644 --- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp @@ -48,7 +48,9 @@ int main(int argc, char** argv) auto stopwatch_now = stopwatch_last; hw_interface.init(nh, nh_priv); + ROS_INFO_STREAM("initialized hw interface"); controller_manager::ControllerManager cm(&hw_interface, nh); + ROS_INFO_STREAM("started controller manager"); // Get current time and elapsed time since last read timestamp = ros::Time::now(); @@ -76,7 +78,8 @@ int main(int argc, char** argv) if (!control_rate.sleep()) { - ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << "ms"); + // ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << + // "ms"); } } diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp index e5f5c65..302a1c9 100644 --- a/ur_rtde_driver/src/rtde/rtde_client.cpp +++ b/ur_rtde_driver/src/rtde/rtde_client.cpp @@ -36,6 +36,7 @@ RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier) , parser_(readRecipe()) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier) + , max_frequency_(URE_MAX_FREQUENCY) { } @@ -68,7 +69,6 @@ bool RTDEClient::init() } // determine maximum frequency from ur-control version - double max_frequency = URE_MAX_FREQUENCY; size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer); stream_.write(buffer, size, written); pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); @@ -76,14 +76,14 @@ bool RTDEClient::init() dynamic_cast(package.get()); if (tmp_control_version->major_ < 5) { - max_frequency = CB3_MAX_FREQUENCY; + max_frequency_ = CB3_MAX_FREQUENCY; } // sending output recipe - LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency); + LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency_); if (protocol_version == 2) { - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency, readRecipe()); + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency_, readRecipe()); } else { diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 485239b..e014a61 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -62,7 +62,7 @@ def myProg(): stopj(1.0) sync() elif state == SERVO_RUNNING: - servoj(q, t=0.008, lookahead_time=0.03, gain=300) + servoj(q, t=0.008, lookahead_time=0.03, gain=750) else: sync() end @@ -90,7 +90,7 @@ def myProg(): end )"; -ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125) // conservative CB3 default. +ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) { ROS_INFO_STREAM("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_)); @@ -100,6 +100,8 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125 throw std::runtime_error("initialization went wrong"); // TODO: be less harsh } + rtde_frequency_ = rtde_client_->getMaxFrequency(); + comm::URStream stream(ROBOT_IP, 30001); stream.connect(); @@ -119,7 +121,10 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125 uint32_t reverse_port = 50001; // TODO: Make this a parameter reverse_interface_.reset(new comm::ReverseInterface(reverse_port)); + ROS_INFO_STREAM("Created reverse interface"); + rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface) + ROS_INFO_STREAM("Initialization done"); } std::unique_ptr ur_driver::UrDriver::getDataPackage() @@ -134,7 +139,7 @@ std::unique_ptr ur_driver::UrDriver::getDataPackage if (tmp != nullptr) { urpackage.release(); - return std::move(std::unique_ptr(tmp)); + return std::unique_ptr(tmp); } } return nullptr;