Skip to content

Commit

Permalink
Workaround for ros-industrial-attic#58
Browse files Browse the repository at this point in the history
  • Loading branch information
Ruddick Lawrence committed Nov 8, 2017
1 parent 9397f47 commit 92c1fbc
Showing 1 changed file with 12 additions and 12 deletions.
24 changes: 12 additions & 12 deletions src/ur_hardware_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,13 +177,13 @@ bool UrHardwareInterface::canSwitch(
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
if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::VelocityJointInterface") {
if (velocity_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
controller_it->claimed_resources.at(0).hardware_interface.c_str());
return false;
}
if (position_interface_running_) {
Expand All @@ -192,7 +192,7 @@ bool UrHardwareInterface::canSwitch(
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
if (stop_controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::PositionJointInterface") {
error = false;
break;
Expand All @@ -202,17 +202,17 @@ bool UrHardwareInterface::canSwitch(
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
controller_it->claimed_resources.at(0).hardware_interface.c_str());
return false;
}
}
} else if (controller_it->hardware_interface
} else if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::PositionJointInterface") {
if (position_interface_running_) {
ROS_ERROR(
"%s: An interface of that type (%s) is already running",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
controller_it->claimed_resources.at(0).hardware_interface.c_str());
return false;
}
if (velocity_interface_running_) {
Expand All @@ -221,7 +221,7 @@ bool UrHardwareInterface::canSwitch(
stop_list.begin();
stop_controller_it != stop_list.end();
++stop_controller_it) {
if (stop_controller_it->hardware_interface
if (stop_controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::VelocityJointInterface") {
error = false;
break;
Expand All @@ -231,7 +231,7 @@ bool UrHardwareInterface::canSwitch(
ROS_ERROR(
"%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
controller_it->claimed_resources.at(0).hardware_interface.c_str());
return false;
}
}
Expand All @@ -248,12 +248,12 @@ void UrHardwareInterface::doSwitch(
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
if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface
if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = false;
std::vector<double> tmp;
Expand All @@ -264,12 +264,12 @@ void UrHardwareInterface::doSwitch(
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
if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::VelocityJointInterface") {
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface
if (controller_it->claimed_resources.at(0).hardware_interface
== "hardware_interface::PositionJointInterface") {
position_interface_running_ = true;
robot_->uploadProg();
Expand Down

0 comments on commit 92c1fbc

Please sign in to comment.