Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-09 17:40:47 +02:00
Code Releases Activity

added standard analog IO

Browse Source
This commit is contained in:
Felix Mauch
2019-07-18 15:55:27 +02:00
parent 369f3594af
commit 5c1043205d
3 changed files with 35 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -127,6 +127,9 @@ protected:
vector6d_t tcp_pose_;
std::bitset<18> actual_dig_out_bits_;
std::bitset<18> actual_dig_in_bits_;
std::array<double, 2> standard_analog_input_;
std::array<double, 2> standard_analog_output_;
std::bitset<4> analog_io_types_;
tf2::Vector3 tcp_force_;
tf2::Vector3 tcp_torque_;
geometry_msgs::TransformStamped tcp_transform_;

5
ur_rtde_driver/resources/rtde_recipe.txt
View File

@@ -8,3 +8,8 @@ actual_TCP_force
actual_TCP_pose
actual_digital_input_bits
actual_digital_output_bits
standard_analog_input0
standard_analog_input1
standard_analog_output0
standard_analog_output1
analog_io_types

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

@@ -38,6 +38,8 @@ HardwareInterface::HardwareInterface()
, joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
, joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
, joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
, standard_analog_input_{ { 0, 0 } }
, standard_analog_output_{ { 0, 0 } }
, joint_names_(6)
, runtime_state_(static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::STOPPED))
, position_controller_running_(false)
@@ -188,6 +190,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
io_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::IOStates>(robot_hw_nh, "io_states", 1));
io_pub_->msg_.digital_in_states.resize(actual_dig_in_bits_.size());
io_pub_->msg_.digital_out_states.resize(actual_dig_out_bits_.size());
io_pub_->msg_.analog_in_states.resize(standard_analog_input_.size());
io_pub_->msg_.analog_out_states.resize(standard_analog_output_.size());
for (size_t i = 0; i < actual_dig_in_bits_.size(); ++i)
{
io_pub_->msg_.digital_in_states[i].pin = i;
@@ -196,6 +200,14 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
{
io_pub_->msg_.digital_out_states[i].pin = i;
}
for (size_t i = 0; i < standard_analog_input_.size(); ++i)
{
io_pub_->msg_.analog_in_states[i].pin = i;
}
for (size_t i = 0; i < standard_analog_output_.size(); ++i)
{
io_pub_->msg_.analog_out_states[i].pin = i;
}
deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
@@ -240,8 +252,13 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
readData(data_pkg, "runtime_state", runtime_state_);
readData(data_pkg, "actual_TCP_force", fts_measurements_);
readData(data_pkg, "actual_TCP_pose", tcp_pose_);
readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]);
readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]);
readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]);
readData(data_pkg, "standard_analog_output1", standard_analog_output_[1]);
readBitsetData<uint64_t>(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_);
readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
publishIOData();
@@ -453,6 +470,16 @@ void HardwareInterface::publishIOData()
{
io_pub_->msg_.digital_out_states[i].state = actual_dig_out_bits_[i];
}
for (size_t i = 0; i < standard_analog_input_.size(); ++i)
{
io_pub_->msg_.analog_in_states[i].state = standard_analog_input_[i];
}
for (size_t i = 0; i < standard_analog_output_.size(); ++i)
{
io_pub_->msg_.analog_out_states[i].state = standard_analog_output_[i];
}
// TODO: Handle analog domain
// TODO: Handle tool IO
io_pub_->unlockAndPublish();
}
}