diff --git a/include/ros1_bridge/action_factory.hpp b/include/ros1_bridge/action_factory.hpp index 06bbaf9c..bce04f4a 100644 --- a/include/ros1_bridge/action_factory.hpp +++ b/include/ros1_bridge/action_factory.hpp @@ -78,7 +78,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface client_ = rclcpp_action::create_client(ros2_node, action_name); } - virtual void shutdown() { + virtual void shutdown() + { std::lock_guard lock(mutex_); for (auto goal : goals_) { std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); @@ -147,7 +148,8 @@ class ActionFactory_1_2 : public ActionFactoryInterface std::shared_future gh2_future; // Changes as per Dashing auto send_goal_ops = ROS2SendGoalOptions(); - send_goal_ops.goal_response_callback = [this, &gh2_future](std::shared_future gh2) mutable { + send_goal_ops.goal_response_callback = + [this, &gh2_future](std::shared_future gh2) mutable { auto goal_handle = gh2_future.get(); if (!goal_handle) { gh1_.setRejected(); // goal was not accepted by remote server @@ -253,7 +255,8 @@ class ActionFactory_2_1 : public ActionFactoryInterface std::bind(&ActionFactory_2_1::handle_accepted, this, std::placeholders::_1)); } - virtual void shutdown() { + virtual void shutdown() + { std::lock_guard lock(mutex_); for (auto goal : goals_) { std::thread([handler = goal.second]() mutable {handler->cancel();}).detach(); @@ -391,7 +394,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface std::size_t get_goal_id_hash(const rclcpp_action::GoalUUID & uuid) { - return std::hash{} (uuid); + return std::hash{}(uuid); } ros::NodeHandle ros1_node_; diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index dbfd9238..6404e6d1 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -26,8 +26,8 @@ from io import StringIO # Python 3.x import genmsg -import genmsg.msg_loader from genmsg.base import COMMENTCHAR, IODELIM +import genmsg.msg_loader import rosidl_adapter.parser from rosidl_cmake import expand_template @@ -423,7 +423,7 @@ def get_ros1_actions(rospack=None): # actual function: rosmsg.iterate_packages(rospack, rosmsg.MODE_MSG)) pkgs = sorted(x for x in iterate_action_packages(rospack)) for package_name, path in pkgs: - for action_name in rosmsg._list_types(path, 'msg', ".action"): + for action_name in rosmsg._list_types(path, 'msg', '.action'): actions.append(Message(package_name, action_name, path)) return actions @@ -462,7 +462,8 @@ def get_ros2_actions(): file=sys.stderr) continue for data in content: - if all(n not in data for n in ('ros1_message_name', 'ros2_message_name', 'ros1_service_name', 'ros2_service_name')): + if (all(n not in data for n in ('ros1_message_name', 'ros2_message_name', + 'ros1_service_name', 'ros2_service_name'))): try: rules.append(ActionMappingRule(data, package_name)) except Exception as e: @@ -899,7 +900,9 @@ def determine_common_actions( # the check for 'builtin_interfaces' should be removed # once merged with __init__.py # It seems to handle it already - if (ros1_type, ros2_type) not in message_string_pairs and not ros2_type.startswith("builtin_interfaces") and "GripperCommand" not in ros2_type: + if ((ros1_type, ros2_type) not in message_string_pairs and + not ros2_type.startswith('builtin_interfaces') and + 'GripperCommand' not in ros2_type): match = False break output[direction].append({ @@ -1104,6 +1107,7 @@ def load_ros1_service(ros1_srv): # genmsg/actions.py + class ActionSpec(object): def __init__(self, goal, result, feedback, text, full_name='', short_name='', package=''): @@ -1139,12 +1143,11 @@ def __ne__(self, other): return not self.__eq__(other) def __repr__(self): - return "ActionSpec[%s, %s, %s]" % (repr(self.goal), repr(self.result), repr(self.feedback)) + return 'ActionSpec[%s, %s, %s]' % (repr(self.goal), repr(self.result), repr(self.feedback)) # genmsg.msg_loader - def load_action_from_string(msg_context, text, full_name): """ Load :class:`ActionSpec` from the .action file. @@ -1162,15 +1165,15 @@ def load_action_from_string(msg_context, text, full_name): count = 0 accum = text_goal for l in text.split('\n'): - l = l.split(COMMENTCHAR)[0].strip() # strip comments - if l.startswith(IODELIM): # lenient, by request + s = l.split(COMMENTCHAR)[0].strip() # strip comments + if s.startswith(IODELIM): # lenient, by request if count == 0: accum = text_result count = 1 else: accum = text_feedback else: - accum.write(l+'\n') + accum.write(s+'\n') # create separate MsgSpec objects for each half of file msg_goal = genmsg.msg_loader.load_msg_from_string( @@ -1186,8 +1189,8 @@ def load_action_from_string(msg_context, text, full_name): def load_action_from_file(msg_context, file_path, full_name): """ Convert the .action representation in the file to a :class:`MsgSpec` instance. - NOTE: this will register the message in the *msg_context*. + NOTE: this will register the message in the *msg_context*. :param file_path: path of file to load from, ``str`` :returns: :class:`MsgSpec` instance :raises: :exc:`InvalidMsgSpec`: if syntax errors or other problems are detected in file @@ -1207,7 +1210,8 @@ def load_ros1_action(ros1_action): ros1_action.prefix_path, ros1_action.message_name + '.action') try: spec = load_action_from_file( - msg_context, message_path, '%s/%s' % (ros1_action.package_name, ros1_action.message_name)) + msg_context, message_path, '%s/%s' % + (ros1_action.package_name, ros1_action.message_name)) except genmsg.InvalidMsgSpec: return None return spec @@ -1275,7 +1279,7 @@ def load_ros2_action(ros2_action): spec = rosidl_adapter.parser.parse_action_file( ros2_action.package_name, actiom_path) except rosidl_adapter.parser.InvalidSpecification: - print("Invalid spec") + print('Invalid spec') return None return spec diff --git a/src/dynamic_bridge.cpp b/src/dynamic_bridge.cpp index 4773a78f..90255d1e 100644 --- a/src/dynamic_bridge.cpp +++ b/src/dynamic_bridge.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include // NOLINT // include ROS 1 #ifdef __clang__ @@ -151,8 +151,10 @@ void update_bridge( std::map & bridges_2to1, std::map & service_bridges_1_to_2, std::map & service_bridges_2_to_1, - std::map> & action_bridges_1_to_2, - std::map> & action_bridges_2_to_1, + std::map> & action_bridges_1_to_2, + std::map> & action_bridges_2_to_1, bool bridge_all_1to2_topics, bool bridge_all_2to1_topics) { std::lock_guard lock(g_bridge_mutex); @@ -574,9 +576,12 @@ inline bool is_action_topic( // action type is 'Fibonacci' if (!type.empty() && is_action_type) { std::string pkg_name = type.substr(0, type.find("/")); - std::string action_type = type.substr(type.find_last_of("/") + 1, type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); + std::string action_type = + type.substr( + type.find_last_of("/") + 1, + type.length() - (type.find_last_of("/") + type_ends_with.length() + 1)); actions[name]["package"] = pkg_name; - if(is_ros2) { + if (is_ros2) { actions[name]["type"] = "action/" + action_type; } else { actions[name]["type"] = action_type; @@ -610,31 +615,34 @@ void get_active_ros1_actions( for (it = publishers.begin(); it != publishers.end(); it++) { // check for action client - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/cancel", it->second.c_str(), "/GoalID")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, true, it->first.c_str(), "/goal", it->second.c_str(), "ActionGoal")) { continue; - } - // check for action server - else if (is_action_topic( + } else if ( // NOLINT // check for action server + is_action_topic( active_ros1_action_servers, action_server_nums, true, it->first.c_str(), "/feedback", it->second.c_str(), "ActionFeedback")) { continue; } - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/result", it->second.c_str(), "ActionResult")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/status", it->second.c_str(), "GoalStatusArray")) @@ -646,29 +654,32 @@ void get_active_ros1_actions( // subscribers do not report their types, but use it to confirm action for (it = subscribers.begin(); it != subscribers.end(); it++) { // check for action server - if (is_action_topic( + if ( + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/cancel", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_servers, action_server_nums, false, it->first.c_str(), "/goal", it->second.c_str(), "")) { continue; - } - // check for action client - else if (is_action_topic( + } else if ( // NOLINT // check for action client + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/feedback", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/result", it->second.c_str(), "")) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros1_action_clients, action_client_nums, false, it->first.c_str(), "/status", it->second.c_str(), "")) { @@ -701,12 +712,14 @@ void get_active_ros2_actions( std::map::iterator it_num; std::map action_server_nums, action_client_nums; for (it = active_ros2_publishers.begin(); it != active_ros2_publishers.end(); it++) { - if (is_action_topic( + if ( + is_action_topic( active_ros2_action_servers, action_server_nums, true, it->first.c_str(), "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros2_action_servers, action_server_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), "GoalStatusArray"), true) @@ -715,13 +728,15 @@ void get_active_ros2_actions( } } for (it = active_ros2_subscribers.begin(); it != active_ros2_subscribers.end(); it++) { - if (is_action_topic( + if ( + is_action_topic( active_ros2_action_clients, action_client_nums, true, it->first.c_str(), "/_action/feedback", it->second.c_str(), "_FeedbackMessage", true)) { continue; - } else if (is_action_topic( + } else if ( // NOLINT + is_action_topic( active_ros2_action_clients, action_client_nums, false, it->first.c_str(), "/_action/status", it->second.c_str(), "GoalStatusArray", true)) @@ -739,7 +754,6 @@ void get_active_ros2_actions( active_ros2_action_servers.erase(it_num->first); } } - } int main(int argc, char * argv[]) @@ -894,7 +908,8 @@ int main(int argc, char * argv[]) } // check actions - std::map> active_ros1_action_servers, active_ros1_action_clients; + std::map> active_ros1_action_servers, + active_ros1_action_clients; get_active_ros1_actions( current_ros1_publishers, current_ros1_subscribers, active_ros1_action_servers, active_ros1_action_clients); @@ -1071,7 +1086,8 @@ int main(int argc, char * argv[]) } } - std::map> active_ros2_action_servers, active_ros2_action_clients; + std::map> active_ros2_action_servers, + active_ros2_action_clients; get_active_ros2_actions( current_ros2_publishers, current_ros2_subscribers, active_ros2_action_servers, active_ros2_action_clients);