action bridge rebased mojin

CMakeLists.txt

@@ -191,7 +191,8 @@ add_library(${PROJECT_NAME} SHARED
 ament_target_dependencies(${PROJECT_NAME}
   ${prefixed_ros1_message_packages}
   ${ros2_interface_packages}
-  "rclcpp" "rclcpp_action"
+  "rclcpp"
+  "rclcpp_action"
   "ros1_roscpp"
   "ros1_std_msgs")
 

README.md

@@ -505,9 +505,9 @@ Note that the `qos` section can be omitted entirely and options not set are left
 
 # Action bridge
 
-This bridge extends the `ros1_bridge` to action interface. The bridge works in both directions, meaning an action goal can be sent from ROS1 client to ROS2 server, or from ROS2 client to ROS1 server.
+This bridge extends the `ros1_bridge` to support actions. The bridge works in both directions, meaning an action goal can be sent from ROS 1 client to ROS 2 server, or from ROS 2 client to ROS 1 server.
 
-The arguments for `action_bridge` node are:  
+The arguments for the `action_bridge` node are:  
 `direction`: from client (`ros1` or `ros2`)
 e.g.:
 - `ROS1` client to `ROS2` server --> `direction` = `ros1`
@@ -517,34 +517,34 @@ e.g.:
 `type`: action interface type of `ROS1`  
 `name`: action name
 
-For sending goals from ROS2 action client to ROS1 action server
+For sending goals from a ROS 2 action client to a ROS 1 action server
 ```
 # Terminal 1 -- action bridge
 # Make sure roscore is already running
 source <ros1_bridge-install-dir>/setup.bash
 ros2 run ros1_bridge action_bridge ros1 actionlib_tutorials Fibonacci fibonacci
 
-# Terminal 2 -- ROS1 action server
+# Terminal 2 -- ROS 1 action server
 source <ros1-install-dir>/setup.bash
 rosrun actionlib_tutorials fibonacci_server
 
-# Terminal 3 -- ROS2 action client
+# Terminal 3 -- ROS 2 action client
 source <ros2-install-dir>/setup.bash
 ros2 run action_tutorials_cpp fibonacci_action_client 20
 ```
 
-For sending goals from ROS1 action client to ROS2 action server
+For sending goals from a ROS 1 action client to a ROS 2 action server
 ```
 # Terminal 1 -- action bridge
 # Make sure roscore is already running
 source <ros1_bridge-install-dir>/setup.bash
 ros2 run ros1_bridge action_bridge ros2 action_tutorials_interfaces action/Fibonacci fibonacci
 
-# Terminal 2 -- ROS2 action server
+# Terminal 2 -- ROS 2 action server
 source <ros2-install-dir>/setup.bash
 ros2 run action_tutorials_cpp fibonacci_action_server
 
-# Terminal 3 -- ROS1 action client
+# Terminal 3 -- ROS 1 action client
 source <ros1-install-dir>/setup.bash
 rosrun actionlib_tutorials fibonacci_client 20
 ```

include/ros1_bridge/action_factory.hpp

@@ -27,7 +27,6 @@
 
 #ifdef __clang__
 #pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Wunused-parameter"
 #endif
 #include <actionlib/client/action_client.h>
 #include <actionlib/client/simple_action_client.h>  // Need this for the goal state. Need a better solution
@@ -91,6 +90,7 @@ class ActionFactory_1_2 : public ActionFactoryInterface
   {
     // try to find goal and cancel it
     std::lock_guard<std::mutex> lock(mutex_);
+    RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::cancel_cb");
     auto it = goals_.find(gh1.getGoalID().id);
     if (it != goals_.end()) {
       std::thread([handler = it->second]() mutable {handler->cancel();}).detach();
@@ -103,11 +103,11 @@ class ActionFactory_1_2 : public ActionFactoryInterface
 
     // create a new handler for the goal
     std::shared_ptr<GoalHandler> handler;
-    handler.reset(new GoalHandler(gh1, client_));
+    handler = std::make_shared<GoalHandler>(gh1, client_, ros2_node_->get_logger());
     std::lock_guard<std::mutex> lock(mutex_);
     goals_.insert(std::make_pair(goal_id, handler));
 
-    RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal");
+    RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_1_2::goal_cb Sending goal");
     std::thread(
       [handler, goal_id, this]() mutable {
         // execute the goal remotely
@@ -131,40 +131,68 @@ class ActionFactory_1_2 : public ActionFactoryInterface
         auto fut = client_->async_cancel_goal(gh2_);
       }
     }
+
     void handle()
     {
       auto goal1 = gh1_.getGoal();
       ROS2Goal goal2;
       translate_goal_1_to_2(*gh1_.getGoal(), goal2);
 
       if (!client_->wait_for_action_server(std::chrono::seconds(1))) {
-        std::cout << "Action server not available after waiting" << std::endl;
+        RCLCPP_INFO(this->logger_, "Action server not available after waiting");
         gh1_.setRejected();
         return;
       }
 
-      // goal_response_callback signature changed after foxy, this implementation
-      // works with both
       std::shared_future<ROS2ClientGoalHandle> gh2_future;
-      // Changes as per Dashing
       auto send_goal_ops = ROS2SendGoalOptions();
       send_goal_ops.goal_response_callback =
         [this, &gh2_future](std::shared_future<ROS2GoalHandle> gh2) mutable {
-          auto goal_handle = gh2_future.get();
-          if (!goal_handle) {
-            gh1_.setRejected();          // goal was not accepted by remote server
-            return;
-          }
+          try {
+            // this is a workaround for the underlying actionlib implementation bug
+            // this callback function should never get called the the shared_future
+            // is not ready/valid
+            int counter = 0;
+            while (!gh2_future.valid()) {
+              if (counter >= 10) {
+                RCLCPP_ERROR(
+                  this->logger_,
+                  "std::shared_future not valid for more than 1.0 seconds. "
+                  "Rejecting action goal.");
+                gh1_.setRejected();
+                return;
+              }
+              RCLCPP_WARN(
+                this->logger_,
+                "std::shared_future not valid. "
+                "This indicates a bug in the actionlib implementation. "
+                "goal_reponse_callback should only get called when the future is valid. "
+                "Waiting and retrying...");
+              rclcpp::sleep_for(std::chrono::milliseconds(100));
+              counter++;
+            }
+            auto goal_handle = gh2_future.get();
+            if (!goal_handle) {
+              gh1_.setRejected();          // goal was not accepted by remote server
+              return;
+            }
 
-          gh1_.setAccepted();
+            gh1_.setAccepted();
 
-          {
-            std::lock_guard<std::mutex> lock(mutex_);
-            gh2_ = goal_handle;
+            {
+              std::lock_guard<std::mutex> lock(mutex_);
+              gh2_ = goal_handle;
 
-            if (canceled_) {          // cancel was called in between
-              auto fut = client_->async_cancel_goal(gh2_);
+              if (canceled_) {          // cancel was called in between
+                auto fut = client_->async_cancel_goal(gh2_);
+              }
             }
+          } catch (const std::future_error & e) {
+            RCLCPP_ERROR_STREAM(
+              this->logger_,
+              "Caught a future_error with code '" << e.code() <<
+                "' Message: '" << e.what() << "'");
+            throw;
           }
         };
 
@@ -193,13 +221,14 @@ class ActionFactory_1_2 : public ActionFactoryInterface
       }
     }
 
-    GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client)
-    : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {}
+    GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client, rclcpp::Logger logger)
+    : gh1_(gh1), gh2_(nullptr), client_(client), logger_(logger), canceled_(false) {}
 
 private:
     ROS1GoalHandle gh1_;
     ROS2ClientGoalHandle gh2_;
     ROS2ClientSharedPtr client_;
+    rclcpp::Logger logger_;
     bool canceled_;      // cancel was called
     std::mutex mutex_;
   };
@@ -272,7 +301,7 @@ class ActionFactory_2_1 : public ActionFactoryInterface
     (void)uuid;
     (void)goal;
     if (!client_->waitForActionServerToStart(ros::Duration(1))) {
-      std::cout << "Action server not available after waiting" << std::endl;
+      RCLCPP_INFO(ros2_node_->get_logger(), "Action server not available after waiting");
       return rclcpp_action::GoalResponse::REJECT;
     }
 
@@ -295,11 +324,11 @@ class ActionFactory_2_1 : public ActionFactoryInterface
   {
     std::size_t goal_id = get_goal_id_hash(gh2->get_goal_id());
     std::shared_ptr<GoalHandler> handler;
-    handler.reset(new GoalHandler(gh2, client_));
+    handler = std::make_shared<GoalHandler>(gh2, client_, ros2_node_->get_logger());
     std::lock_guard<std::mutex> lock(mutex_);
     goals_.insert(std::make_pair(goal_id, handler));
 
-    RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal");
+    RCLCPP_INFO(ros2_node_->get_logger(), "ActionFactory_2_1::handle_accepted Sending goal");
     std::thread(
       [handler, goal_id, this]() mutable {
         // execute the goal remotely
@@ -344,7 +373,10 @@ class ActionFactory_2_1 : public ActionFactoryInterface
         [this, &result_ready,
         &cond_result](ROS1ClientGoalHandle goal_handle) mutable           // transition_cb
         {
-          ROS_INFO("Goal [%s]", goal_handle.getCommState().toString().c_str());
+          RCLCPP_INFO(
+            this->logger_,
+            "ActionFactory_2_1::handle Goal [%s]",
+            goal_handle.getCommState().toString().c_str());
           if (goal_handle.getCommState() == actionlib::CommState::RECALLING) {
             // cancelled before being processed
             auto result2 = std::make_shared<ROS2Result>();
@@ -357,7 +389,10 @@ class ActionFactory_2_1 : public ActionFactoryInterface
             auto result2 = std::make_shared<ROS2Result>();
             auto result1 = goal_handle.getResult();
             translate_result_1_to_2(*result2, *result1);
-            ROS_INFO("Goal [%s]", goal_handle.getTerminalState().toString().c_str());
+            RCLCPP_INFO(
+              this->logger_,
+              "ActionFactory_2_1::handle Goal [%s]",
+              goal_handle.getTerminalState().toString().c_str());
             if (goal_handle.getTerminalState() == actionlib::TerminalState::SUCCEEDED) {
               gh2_->succeed(result2);
             } else {
@@ -381,13 +416,17 @@ class ActionFactory_2_1 : public ActionFactoryInterface
       cond_result.wait(lck, [&result_ready] {return result_ready.load();});
     }
 
-    GoalHandler(std::shared_ptr<ROS2ServerGoalHandle> & gh2, std::shared_ptr<ROS1Client> & client)
-    : gh2_(gh2), client_(client), canceled_(false) {}
+    GoalHandler(
+      std::shared_ptr<ROS2ServerGoalHandle> & gh2,
+      std::shared_ptr<ROS1Client> & client,
+      rclcpp::Logger logger)
+    : gh2_(gh2), client_(client), logger_(logger), canceled_(false) {}
 
 private:
     std::shared_ptr<ROS1ClientGoalHandle> gh1_;
     std::shared_ptr<ROS2ServerGoalHandle> gh2_;
     std::shared_ptr<ROS1Client> client_;
+    rclcpp::Logger logger_;
     bool canceled_;      // cancel was called
     std::mutex mutex_;
   };

resource/get_factory.cpp.em

@@ -65,7 +65,6 @@ std::unique_ptr<ServiceFactoryInterface> get_service_factory(const std::string &
     return factory;
   }
 @[end for]@
-  // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data());
   return factory;
 }
 
@@ -84,8 +83,7 @@ std::unique_ptr<ActionFactoryInterface> get_action_factory(const std::string & r
     return factory;
   }
 @[end for]@
-  // fprintf(stderr, "No template specialization for the service %s:%s/%s\n", ros_id.data(), package_name.data(), service_name.data());
-  return factory;
+  return nullptr;
 }
 
 }  // namespace ros1_bridge

ros1_bridge/__init__.py

@@ -814,10 +814,10 @@ def determine_common_services(
                 ros2_name = ros2_fields[direction][i].name
                 if ros1_name != ros2_name:
                     # if the names do not match, check first if the types are builtin
-                    ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type))
-                    ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type()
+                    ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type))
+                    ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type()
                     # Check if types are primitive types
-                    if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type:
+                    if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type:
                         # if the message types have a custom mapping their names
                         # might not be equal, therefore check the message pairs
                         if ((ros1_type, ros2_type) not in message_string_pairs and
@@ -910,11 +910,11 @@ def determine_common_actions(
                     # if the message types have a custom mapping their names
                     # might not be equal, therefore check the message pairs
 
-                    ros1_is_buildin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type))
-                    ros2_is_buildin = ros2_fields[direction][i].type.is_primitive_type()
+                    ros1_is_builtin = genmsg.msgs.is_builtin(genmsg.msgs.bare_msg_type(ros1_type))
+                    ros2_is_builtin = ros2_fields[direction][i].type.is_primitive_type()
 
                     # Check if types are primitive types
-                    if not ros1_is_buildin or not ros2_is_buildin or ros1_type != ros2_type:
+                    if not ros1_is_builtin or not ros2_is_builtin or ros1_type != ros2_type:
                         # the check for 'builtin_interfaces' should be removed
                         # once merged with __init__.py
                         # It seems to handle it already
@@ -1296,12 +1296,12 @@ def load_ros2_service(ros2_srv):
 
 
 def load_ros2_action(ros2_action):
-    actiom_path = os.path.join(
+    action_path = os.path.join(
         ros2_action.prefix_path, 'share', ros2_action.package_name, 'action',
         ros2_action.message_name + '.action')
     try:
         spec = rosidl_adapter.parser.parse_action_file(
-            ros2_action.package_name, actiom_path)
+            ros2_action.package_name, action_path)
     except rosidl_adapter.parser.InvalidSpecification:
         print('Invalid spec')
         return None

src/action_bridge.cpp

@@ -17,7 +17,6 @@
 // include ROS 1
 #ifdef __clang__
 #pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Wunused-parameter"
 #endif
 #include "ros/ros.h"
 #ifdef __clang__
@@ -46,14 +45,12 @@ int main(int argc, char * argv[])
 
   std::cout << dir << " " << package << " " << type << " " << name << std::endl;
 
-  // ros1 example_tutorials Fibonacci fibonacci
 
   auto factory = ros1_bridge::get_action_factory(dir, package, type);
   if (factory) {
     printf("created action factory\n");
     try {
       factory->create_server_client(ros1_node, ros2_node, name);
-      // printf("Created 2 to 1 bridge for service %s\n", name.data());
     } catch (std::runtime_error & e) {
       fprintf(stderr, "Failed to created a bridge: %s\n", e.what());
     }

src/dynamic_bridge.cpp

@@ -109,9 +109,7 @@ void update_bridge(
   const std::map<std::string, std::map<std::string, std::string>> & ros1_services,
   const std::map<std::string, std::map<std::string, std::string>> & ros2_services,
   const std::map<std::string, std::map<std::string, std::string>> & ros1_action_servers,
-  const std::map<std::string, std::map<std::string, std::string>> & ros1_action_clients,
   const std::map<std::string, std::map<std::string, std::string>> & ros2_action_servers,
-  const std::map<std::string, std::map<std::string, std::string>> & ros2_action_clients,
   std::map<std::string, ros1_bridge::Bridge1to2HandlesAndMessageTypes> & bridges_1to2,
   std::map<std::string, ros1_bridge::Bridge2to1HandlesAndMessageTypes> & bridges_2to1,
   std::map<std::string, ros1_bridge::ServiceBridge1to2> & service_bridges_1_to_2,
@@ -763,8 +761,7 @@ int main(int argc, char * argv[])
         ros1_publishers, ros1_subscribers,
         ros2_publishers, ros2_subscribers,
         ros1_services, ros2_services,
-        ros1_action_servers, ros1_action_clients,
-        ros2_action_servers, ros2_action_clients,
+        ros1_action_servers, ros2_action_servers,
         bridges_1to2, bridges_2to1,
         service_bridges_1_to_2, service_bridges_2_to_1,
         action_bridges_1_to_2, action_bridges_2_to_1,
@@ -830,8 +827,7 @@ int main(int argc, char * argv[])
         ros1_publishers, ros1_subscribers,
         ros2_publishers, ros2_subscribers,
         ros1_services, ros2_services,
-        ros1_action_servers, ros1_action_clients,
-        ros2_action_servers, ros2_action_clients,
+        ros1_action_servers, ros2_action_servers,
         bridges_1to2, bridges_2to1,
         service_bridges_1_to_2, service_bridges_2_to_1,
         action_bridges_1_to_2, action_bridges_2_to_1,