Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

get frequency from robot

Browse Source
This commit is contained in:
Felix Mauch
2019-05-07 14:30:45 +02:00
parent f38ec92389
commit 4106aa9fb8
10 changed files with 49 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
ur_controllers/include/ur_controllers/hardware_interface_adapter.h
View File

@@ -15,7 +15,6 @@
#ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
#define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
#include <joint_trajectory_controller/hardware_interface_adapter.h>
#include "ur_controllers/scaled_joint_command_interface.h"
@@ -43,20 +42,24 @@ template <class State>
class HardwareInterfaceAdapter<ur_controllers::ScaledPositionJointInterface, State>
{
public:
HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
HardwareInterfaceAdapter() : joint_handles_ptr_(0)
{
}
bool init(std::vector<ur_controllers::ScaledJointHandle>& 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<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
};
#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED

2
ur_controllers/include/ur_controllers/speed_scaling_state_controller.h
View File

@@ -36,7 +36,7 @@ public:
private:
std::vector<SpeedScalingHandle> sensors_;
//TODO: We should use a better datatype later on
// TODO: We should use a better datatype later on
typedef std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > RtPublisherPtr;
std::vector<RtPublisherPtr> realtime_pubs_;
std::vector<ros::Time> last_publish_times_;

1
ur_controllers/src/scaled_joint_trajectory_controller.cpp
View File

@@ -18,7 +18,6 @@
#include <pluginlib/class_list_macros.hpp>
#include <trajectory_interface/quintic_spline_segment.h>
namespace position_controllers
{
typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,

1
ur_controllers/src/speed_scaling_state_controller.cpp
View File

@@ -86,4 +86,3 @@ void SpeedScalingStateController ::stopping(const ros::Time& /*time*/)
} // namespace ur_controllers
PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerBase)

7
ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h
View File

@@ -57,12 +57,19 @@ public:
bool start();
bool getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package, std::chrono::milliseconds timeout);
double getMaxFrequency() const
{
return max_frequency_;
}
private:
comm::URStream<PackageHeader> stream_;
RTDEParser parser_;
comm::URProducer<PackageHeader> prod_;
comm::Pipeline<PackageHeader> pipeline_;
double max_frequency_;
constexpr static const double CB3_MAX_FREQUENCY = 125.0;
constexpr static const double URE_MAX_FREQUENCY = 500.0;

5
ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
View File

@@ -56,7 +56,7 @@ public:
*/
std::unique_ptr<rtde_interface::DataPackage> 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_interface::RTDEClient> rtde_client_;
std::unique_ptr<comm::ReverseInterface> reverse_interface_;

12
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -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<std::string>("robot_ip", "192.168.56.101");
ROS_INFO_STREAM("Initializing urdriver");

5
ur_rtde_driver/src/ros/hardware_interface_node.cpp
View File

@@ -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");
}
}

8
ur_rtde_driver/src/rtde/rtde_client.cpp
View File

@@ -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<rtde_interface::GetUrcontrolVersion*>(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
{

11
ur_rtde_driver/src/ur/ur_driver.cpp
View File

@@ -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<rtde_interface::PackageHeader> 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<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
@@ -134,7 +139,7 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
if (tmp != nullptr)
{
urpackage.release();
return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
return std::unique_ptr<rtde_interface::DataPackage>(tmp);
}
}
return nullptr;