Skip to content

Commit

Permalink
Merge remote-tracking branch 'upstream/melodic-devel' into melodic-devel
Browse files Browse the repository at this point in the history 
Signed-off-by: AbhinavSingh <singhabhinav9051571833@gmail.com>
  • Loading branch information
@suab321321
suab321321 committed Jan 25, 2020
2 parents 6cc5418 + 85b01dd commit 2337059
Show file tree
Hide file tree
Showing 26 changed files with 210 additions and 240 deletions.
47 changes: 24 additions & 23 deletions combined_robot_hw/src/combined_robot_hw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,11 @@ namespace combined_robot_hw
ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << robot_hw_nh.getNamespace() << ").");
return false;
}
std::vector<std::string>::iterator it;
for(it = robots.begin();it != robots.end();it++){
if (!loadRobotHW(*it)){

for (const auto& robot : robots)
{
if (!loadRobotHW(robot))
{
return false;
}
}
Expand All @@ -59,7 +61,7 @@ namespace combined_robot_hw
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
// Call the prepareSwitch method of the single RobotHW objects.
for (auto const & robot_hw : robot_hw_list_)
for (const auto& robot_hw : robot_hw_list_)
{
std::list<hardware_interface::ControllerInfo> filtered_start_list;
std::list<hardware_interface::ControllerInfo> filtered_stop_list;
Expand All @@ -68,7 +70,7 @@ namespace combined_robot_hw
filterControllerList(start_list, filtered_start_list, robot_hw);
filterControllerList(stop_list, filtered_stop_list, robot_hw);

if (!(robot_hw)->prepareSwitch(filtered_start_list, filtered_stop_list))
if (!robot_hw->prepareSwitch(filtered_start_list, filtered_stop_list))
return false;
}
return true;
Expand All @@ -78,7 +80,7 @@ namespace combined_robot_hw
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
// Call the doSwitch method of the single RobotHW objects.
for (auto const & robot_hw : robot_hw_list_)
for (const auto& robot_hw : robot_hw_list_)
{
std::list<hardware_interface::ControllerInfo> filtered_start_list;
std::list<hardware_interface::ControllerInfo> filtered_stop_list;
Expand All @@ -87,7 +89,7 @@ namespace combined_robot_hw
filterControllerList(start_list, filtered_start_list, robot_hw);
filterControllerList(stop_list, filtered_stop_list, robot_hw);

(robot_hw)->doSwitch(filtered_start_list, filtered_stop_list);
robot_hw->doSwitch(filtered_start_list, filtered_stop_list);
}
}

Expand Down Expand Up @@ -119,10 +121,9 @@ namespace combined_robot_hw
ROS_DEBUG("Constructing robot HW '%s' of type '%s'", name.c_str(), type.c_str());
try
{
std::vector<std::string> cur_types = robot_hw_loader_.getDeclaredClasses();
for(size_t i=0; i < cur_types.size(); i++)
for (const auto& cur_type : robot_hw_loader_.getDeclaredClasses())
{
if (type == cur_types[i])
if (type == cur_type)
{
robot_hw = robot_hw_loader_.createUniqueInstance(type);
}
Expand Down Expand Up @@ -182,19 +183,19 @@ namespace combined_robot_hw
void CombinedRobotHW::read(const ros::Time& time, const ros::Duration& period)
{
// Call the read method of the single RobotHW objects.
for (auto const & robot_hw : robot_hw_list_)
for (const auto& robot_hw : robot_hw_list_)
{
(robot_hw)->read(time, period);
robot_hw->read(time, period);
}
}


void CombinedRobotHW::write(const ros::Time& time, const ros::Duration& period)
{
// Call the write method of the single RobotHW objects.
for (auto const & robot_hw : robot_hw_list_)
for (const auto& robot_hw : robot_hw_list_)
{
(robot_hw)->write(time, period);
robot_hw->write(time, period);
}
}

Expand All @@ -203,21 +204,21 @@ namespace combined_robot_hw
hardware_interface::RobotHWSharedPtr robot_hw)
{
filtered_list.clear();
for (auto const & it : list)
for (const auto& controller : list)
{
hardware_interface::ControllerInfo filtered_controller;
filtered_controller.name = it.name;
filtered_controller.type = it.type;
filtered_controller.name = controller.name;
filtered_controller.type = controller.type;

if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
filtered_list.push_back(filtered_controller);
continue;
}
for (auto const& res_it : it.claimed_resources)
for (const auto& claimed_resource : controller.claimed_resources)
{
hardware_interface::InterfaceResources filtered_iface_resources;
filtered_iface_resources.hardware_interface = res_it.hardware_interface;
filtered_iface_resources.hardware_interface = claimed_resource.hardware_interface;
std::vector<std::string> r_hw_ifaces = robot_hw->getNames();

auto const if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), filtered_iface_resources.hardware_interface);
Expand All @@ -228,12 +229,12 @@ namespace combined_robot_hw

std::vector<std::string> r_hw_iface_resources = robot_hw->getInterfaceResources(filtered_iface_resources.hardware_interface);
std::set<std::string> filtered_resources;
for (auto const& ctrl_res : res_it.resources)
for (const auto& resource : claimed_resource.resources)
{
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), ctrl_res);
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), resource);
if (res_name != r_hw_iface_resources.end())
{
filtered_resources.insert(ctrl_res);
filtered_resources.insert(resource);
}
}
filtered_iface_resources.resources = filtered_resources;
Expand Down
34 changes: 17 additions & 17 deletions combined_robot_hw_tests/src/my_robot_hw_1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,17 +104,17 @@ void MyRobotHW1::write(const ros::Time& /*time*/, const ros::Duration& /*period*
bool MyRobotHW1::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& /*stop_list*/)
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
for (auto const& res_it : it.claimed_resources)
for (const auto& res_it : controller.claimed_resources)
{
std::vector<std::string> r_hw_ifaces = this->getNames();

auto const& if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
{
ROS_ERROR_STREAM("Bad interface: " << res_it.hardware_interface);
Expand All @@ -123,13 +123,13 @@ bool MyRobotHW1::prepareSwitch(const std::list<hardware_interface::ControllerInf
}

std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it.hardware_interface);
for (auto const& ctrl_res : res_it.resources)
for (const auto& resource : res_it.resources)
{
auto const& res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), ctrl_res);
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), resource);
if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
{
ROS_ERROR_STREAM("Bad resource: " << (ctrl_res));
std::cout << (ctrl_res);
ROS_ERROR_STREAM("Bad resource: " << resource);
std::cout << resource;
return false;
}
}
Expand All @@ -141,29 +141,29 @@ bool MyRobotHW1::prepareSwitch(const std::list<hardware_interface::ControllerInf
void MyRobotHW1::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& /*stop_list*/)
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
for (auto const& res_it : it.claimed_resources)
for (const auto& claimed_resource : controller.claimed_resources)
{
std::vector<std::string> r_hw_ifaces = this->getNames();

auto const& if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), claimed_resource.hardware_interface);
if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
{
throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it.hardware_interface + " is not registered");
throw hardware_interface::HardwareInterfaceException("Hardware_interface " + claimed_resource.hardware_interface + " is not registered");
}

std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it.hardware_interface);
for (auto const& ctrl_res : res_it.resources)
std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(claimed_resource.hardware_interface);
for (const auto& resource : claimed_resource.resources)
{
auto const& res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), ctrl_res);
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), resource);
if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
{
throw hardware_interface::HardwareInterfaceException("Resource " + ctrl_res + " is not registered");
throw hardware_interface::HardwareInterfaceException("Resource " + resource + " is not registered");
}
}
}
Expand Down
32 changes: 16 additions & 16 deletions combined_robot_hw_tests/src/my_robot_hw_2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,30 +86,30 @@ void MyRobotHW2::write(const ros::Time& /*time*/, const ros::Duration& /*period*
bool MyRobotHW2::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& /*stop_list*/)
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
for (auto const& res_it : it.claimed_resources)
for (const auto& res_it : controller.claimed_resources)
{
std::vector<std::string> r_hw_ifaces = this->getNames();

auto const& if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
{
ROS_ERROR_STREAM("Bad interface: " << res_it.hardware_interface);
return false;
}

std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it.hardware_interface);
for (auto const& ctrl_res : res_it.resources)
for (const auto& resource : res_it.resources)
{
auto const& res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), ctrl_res);
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), resource);
if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
{
ROS_ERROR_STREAM("Bad resource: " << ctrl_res);
ROS_ERROR_STREAM("Bad resource: " << resource);
return false;
}
}
Expand All @@ -121,29 +121,29 @@ bool MyRobotHW2::prepareSwitch(const std::list<hardware_interface::ControllerInf
void MyRobotHW2::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& /*stop_list*/)
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
for (auto const& res_it : it.claimed_resources)
for (const auto& claimed_resource : controller.claimed_resources)
{
std::vector<std::string> r_hw_ifaces = this->getNames();

auto const& if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it.hardware_interface);
std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), claimed_resource.hardware_interface);
if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
{
throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it.hardware_interface + " is not registered");
throw hardware_interface::HardwareInterfaceException("Hardware_interface " + claimed_resource.hardware_interface + " is not registered");
}

std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it.hardware_interface);
for (auto const& ctrl_res : res_it.resources)
std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(claimed_resource.hardware_interface);
for (const auto& resource : claimed_resource.resources)
{
auto const& res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), ctrl_res);
std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), resource);
if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
{
throw hardware_interface::HardwareInterfaceException("Resource " + ctrl_res + " is not registered");
throw hardware_interface::HardwareInterfaceException("Resource " + resource + " is not registered");
}
}
}
Expand Down
9 changes: 4 additions & 5 deletions combined_robot_hw_tests/src/my_robot_hw_4.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,9 @@ bool MyRobotHW4::prepareSwitch(const std::list<hardware_interface::ControllerInf
// To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail
if (!start_list.empty() || !stop_list.empty())
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
Expand All @@ -93,9 +93,9 @@ void MyRobotHW4::doSwitch(const std::list<hardware_interface::ControllerInfo>& s
// To easily test a failure case, any controller that claims resources on MyRobotHW4 will fail
if (!start_list.empty() || !stop_list.empty())
{
for (auto const& it : start_list)
for (const auto& controller : start_list)
{
if (it.claimed_resources.empty())
if (controller.claimed_resources.empty())
{
continue;
}
Expand All @@ -110,4 +110,3 @@ void MyRobotHW4::doSwitch(const std::list<hardware_interface::ControllerInfo>& s
}

PLUGINLIB_EXPORT_CLASS( combined_robot_hw_tests::MyRobotHW4, hardware_interface::RobotHW)

Loading

0 comments on commit 2337059

Please sign in to comment.