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.

Sign up for GitHub

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

ServiceClient use callback group #2216

Merged
merged 8 commits into from
Mar 9, 2021
Merged

ServiceClient use callback group #2216

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
729c637
change timeout default value
Mar 4, 2021
b859845
create and use callback group
Mar 4, 2021
41412a1
remove generate_internal_node option
Mar 4, 2021
c9cda89
unit test to do a service call in a topic callback
Mar 4, 2021
d56aa18
Merge remote-tracking branch 'ros-planning/main' into service_client_…
Mar 8, 2021
4c29ac9
fix unit test
Mar 8, 2021
edde26f
Merge remote-tracking branch 'ros-planning/main' into service_client_…
Mar 9, 2021
9b1b2dd
set default value to -1
Mar 9, 2021
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
1 change: 1 addition & 0 deletions nav2_util/include/nav2_util/lifecycle_service_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "nav2_util/service_client.hpp"
#include "nav2_util/node_utils.hpp"

namespace nav2_util
{
Expand Down
34 changes: 15 additions & 19 deletions nav2_util/include/nav2_util/service_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@

#include <string>

#include "nav2_util/node_utils.hpp"
#include "rclcpp/rclcpp.hpp"

namespace nav2_util
Expand All @@ -29,30 +28,25 @@ class ServiceClient
public:
explicit ServiceClient(
const std::string & service_name,
const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr())
: service_name_(service_name)
const rclcpp::Node::SharedPtr & provided_node)
: service_name_(service_name), node_(provided_node)
{
if (provided_node) {
node_ = provided_node;
} else {
node_ = generate_internal_node(service_name + "_Node");
}
client_ = node_->create_client<ServiceT>(service_name);
}

ServiceClient(const std::string & service_name, const std::string & parent_name)
: service_name_(service_name)
{
node_ = generate_internal_node(parent_name + std::string("_") + service_name + "_client");
client_ = node_->create_client<ServiceT>(service_name);
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);
Copy link
Member

@SteveMacenski SteveMacenski Mar 6, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is me asking a question because I haven't used callback groups like this before (previously in the context of priorities only):

I see the second argument means whether to add it to the node's executor or not. Why do you need to run it against a separate executor if its in another callback group? Wouldn't the MutuallyExclusive callback group type make it so that it could be processed at the same time as other callbacks in the same executor?

Basing this question on this comment: https://github.com/ros2/rclcpp/blob/master/rclcpp/include/rclcpp/callback_group.hpp#L69

Copy link
Member

@SteveMacenski SteveMacenski Mar 6, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've done some digging, is the reason that doesn't work because the executor being run right now is single threaded? So you're using a second single threaded executor to execute this callback group separately due to the lack of multithreaded executor?

Edit: I've found that the default callback group when not set is MutuallyExclusive single threaded executor, so if you added this to the existing one, then it wouldn't work because it was already blocked. It seems odd to me to have an internal executor - but I think this is OK from that research. Any significan toverhead associated with just an orphaned executor over here for just this object?

Copy link
Contributor Author

@BriceRenaudeau BriceRenaudeau Mar 8, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The second argument will automatically add the callback group to the node executor.
Hence, you need to change the way you spin your nodes.

executor.add_node(node);
executor.spin();

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using MutuallyExclusive, only the callbacks in the same callback group are mutually exclusive. If you create a callback group per client, there are not blocking each other.

callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
client_ = node_->create_client<ServiceT>(
service_name,
rmw_qos_profile_services_default,
callback_group_);
}

using RequestType = typename ServiceT::Request;
using ResponseType = typename ServiceT::Response;

typename ResponseType::SharedPtr invoke(
typename RequestType::SharedPtr & request,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1))
{
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
Expand All @@ -69,7 +63,7 @@ class ServiceClient
service_name_.c_str());
auto future_result = client_->async_send_request(request);

if (rclcpp::spin_until_future_complete(node_, future_result, timeout) !=
if (callback_group_executor_.spin_until_future_complete(future_result, timeout) !=
rclcpp::FutureReturnCode::SUCCESS)
{
throw std::runtime_error(service_name_ + " service client: async_send_request failed");
Expand Down Expand Up @@ -97,7 +91,7 @@ class ServiceClient
service_name_.c_str());
auto future_result = client_->async_send_request(request);

if (rclcpp::spin_until_future_complete(node_, future_result) !=
if (callback_group_executor_.spin_until_future_complete(future_result) !=
rclcpp::FutureReturnCode::SUCCESS)
{
return false;
Expand All @@ -122,6 +116,8 @@ class ServiceClient
protected:
std::string service_name_;
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
typename rclcpp::Client<ServiceT>::SharedPtr client_;
};

Expand Down
46 changes: 38 additions & 8 deletions nav2_util/test/test_service_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <string>
#include "nav2_util/service_client.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_msgs/msg/empty.hpp"
#include "gtest/gtest.h"

using nav2_util::ServiceClient;
Expand All @@ -41,18 +43,46 @@ class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
const rclcpp::Node::SharedPtr & getNode() {return node_;}
};

TEST(ServiceClient, are_non_alphanumerics_removed)
{
TestServiceClient t("/foo/bar");
string adjustedPrefix = "_foo_bar_Node_";
ASSERT_EQ(t.name().length(), adjustedPrefix.length() + 8);
ASSERT_EQ(0, t.name().compare(0, adjustedPrefix.length(), adjustedPrefix));
}

TEST(ServiceClient, can_ServiceClient_use_passed_in_node)
{
auto node = rclcpp::Node::make_shared("test_node");
TestServiceClient t("bar", node);
ASSERT_EQ(t.getNode(), node);
ASSERT_EQ(t.name(), "test_node");
}

TEST(ServiceClient, can_ServiceClient_invoke_in_callback)
{
int a = 0;
auto service_node = rclcpp::Node::make_shared("service_node");
auto service = service_node->create_service<std_srvs::srv::Empty>(
"empty_srv",
[&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) {
a = 1;
});
auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);});

auto pub_node = rclcpp::Node::make_shared("pub_node");
auto pub = pub_node->create_publisher<std_msgs::msg::Empty>(
"empty_topic",
rclcpp::QoS(1).transient_local());
auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);});

auto sub_node = rclcpp::Node::make_shared("sub_node");
ServiceClient<std_srvs::srv::Empty> client("empty_srv", sub_node);
auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(
"empty_topic",
rclcpp::QoS(1),
[&client](std_msgs::msg::Empty::SharedPtr) {
auto req = std::make_shared<std_srvs::srv::Empty::Request>();
auto res = client.invoke(req);
});

pub->publish(std_msgs::msg::Empty());
rclcpp::spin_some(sub_node);

rclcpp::shutdown();
srv_thread.join();
pub_thread.join();
ASSERT_EQ(a, 1);
}