Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

action bridge rebased mojin #13

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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")

Expand Down
16 changes: 8 additions & 8 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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`
Expand All @@ -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
```
Expand Down
93 changes: 66 additions & 27 deletions include/ros1_bridge/action_factory.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -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
Expand All @@ -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;
}
};

Expand Down Expand Up @@ -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_;
};
Expand Down Expand Up @@ -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;
}

Expand All @@ -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
Expand Down Expand Up @@ -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>();
Expand All @@ -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 {
Expand All @@ -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_;
};
Expand Down
4 changes: 1 addition & 3 deletions resource/get_factory.cpp.em
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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
16 changes: 8 additions & 8 deletions ros1_bridge/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
3 changes: 0 additions & 3 deletions src/action_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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__
Expand Down Expand Up @@ -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());
}
Expand Down
8 changes: 2 additions & 6 deletions src/dynamic_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
Loading