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

Added control switching

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-24 13:50:53 +02:00
parent af601b9c32
commit 1e8f13f232
7 changed files with 73 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/ur_hardware_interface.cpp
View File

@@ -106,6 +106,7 @@ void UrHardwareInterface::init() {
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
}
void UrHardwareInterface::read() {
@@ -127,7 +128,51 @@ void UrHardwareInterface::read() {
}
void UrHardwareInterface::write() {
robot_->setSpeed(joint_velocity_command_[0],joint_velocity_command_[1],joint_velocity_command_[2],joint_velocity_command_[3],joint_velocity_command_[4],joint_velocity_command_[5],100);
if (velocity_interface_running_) {
robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1],
joint_velocity_command_[2], joint_velocity_command_[3],
joint_velocity_command_[4], joint_velocity_command_[5], 100);
}
}
void UrHardwareInterface::doSwitch(
const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) {
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
if (controller_it->hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
}
/*std::string outp;
outp = "doSwitch called - Start list:";
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
start_list.begin(); controller_it != start_list.end();
++controller_it) {
outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
}
outp += " - Stop list:";
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
stop_list.begin(); controller_it != stop_list.end();
++controller_it) {
outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
}
ROS_INFO(outp.c_str()); */
}
} // namespace

4
src/ur_ros_wrapper.cpp
View File

@@ -363,14 +363,12 @@ private:
last_time = current_time;
// Input
hardware_interface_->read();
robot_.rt_interface_->robot_state_->setControllerUpdated();
// Control
//controller_manager_->update(ros::Time::now(), elapsed_time);
controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time);
// Output
hardware_interface_->write();
robot_.rt_interface_->robot_state_->setControllerUpdated();
}
}