Skip to content

Commit

Permalink
lint
Browse files Browse the repository at this point in the history
  • Loading branch information
jhurliman committed Feb 27, 2024
1 parent 1f7a2fe commit 2310f75
Show file tree
Hide file tree
Showing 23 changed files with 1,900 additions and 1,958 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -759,7 +759,7 @@ inline void Server<ServerConfiguration>::handleBinaryMessage(ConnHandle hdl, Mes
length,
data};
_handlers.clientMessageHandler(clientMessage, hdl);
} catch (const ServiceError const& e) {
} catch (ServiceError const& e) {
sendStatusAndLogMsg(hdl, StatusLevel::Error, e.what());
} catch (std::exception const& e) {
sendStatusAndLogMsg(hdl, StatusLevel::Error,
Expand Down
169 changes: 83 additions & 86 deletions ros2_foxglove_bridge/include/ros2_babel_fish/babel_fish.hpp
Original file line number Diff line number Diff line change
@@ -1,9 +1,12 @@
// Copyright (c) 2021 Stefan Fabian. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.
// Licensed under the MIT license. See LICENSE file in the project root for full license
// information.

#ifndef ROS2_BABEL_FISH_BABEL_FISH_HPP
#define ROS2_BABEL_FISH_BABEL_FISH_HPP

#include <rclcpp/node.hpp>

#include "ros2_babel_fish/detail/babel_fish_action_client.hpp"
#include "ros2_babel_fish/detail/babel_fish_publisher.hpp"
#include "ros2_babel_fish/detail/babel_fish_service.hpp"
Expand All @@ -14,19 +17,14 @@
#include "ros2_babel_fish/messages/compound_message.hpp"
#include "ros2_babel_fish/messages/value_message.hpp"

#include <rclcpp/node.hpp>

namespace ros2_babel_fish
{
namespace ros2_babel_fish {

/*!
* Allows communication using message types that are not known at compile time.
*/
class BabelFish : public std::enable_shared_from_this<BabelFish>
{

class BabelFish : public std::enable_shared_from_this<BabelFish> {
public:
RCLCPP_SMART_PTR_DEFINITIONS( BabelFish )
RCLCPP_SMART_PTR_DEFINITIONS(BabelFish)

/*!
* Constructs an instance of BabelFish with a new instance of the default description provider.
Expand All @@ -35,111 +33,110 @@ class BabelFish : public std::enable_shared_from_this<BabelFish>
*/
BabelFish();

explicit BabelFish( std::vector<TypeSupportProvider::SharedPtr> type_support_providers );
explicit BabelFish(std::vector<TypeSupportProvider::SharedPtr> type_support_providers);

~BabelFish();

//! Wrapper for create_subscription without type.
template<typename CallbackT>
BabelFishSubscription::SharedPtr
create_subscription( rclcpp::Node &node, const std::string &topic, const rclcpp::QoS &qos,
CallbackT &&callback, rclcpp::CallbackGroup::SharedPtr group = nullptr,
rclcpp::SubscriptionOptions options = {},
std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) )
{
template <typename CallbackT>
BabelFishSubscription::SharedPtr create_subscription(
rclcpp::Node& node, const std::string& topic, const rclcpp::QoS& qos, CallbackT&& callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {},
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) {
#if RCLCPP_VERSION_MAJOR >= 9
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> any_callback;
#else
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> any_callback(
options.get_allocator() );
options.get_allocator());
#endif
any_callback.set( std::forward<CallbackT>( callback ) );
return create_subscription( node, topic, qos, any_callback, std::move( group ),
std::move( options ), timeout );
any_callback.set(std::forward<CallbackT>(callback));
return create_subscription(node, topic, qos, any_callback, std::move(group), std::move(options),
timeout);
}

/*!
* This method will wait for the given topic until timeout expired (if a timeout is set) or the topic becomes available.
* As soon as the topic is available, it will create a subscription for the topic with the passed options.
* @param qos The quality of service options. Can be a number which will be the queue size, i.e., number of messages
* to queue for processing before dropping messages if the processing can't keep up.
* @param timeout The maximum time this call will block before returning. Set to 0 to not block at all.
* @return A subscription if the topic became available before the timeout expired or a nullptr otherwise.
* This method will wait for the given topic until timeout expired (if a timeout is set) or the
* topic becomes available. As soon as the topic is available, it will create a subscription for
* the topic with the passed options.
* @param qos The quality of service options. Can be a number which will be the queue size, i.e.,
* number of messages to queue for processing before dropping messages if the processing can't
* keep up.
* @param timeout The maximum time this call will block before returning. Set to 0 to not block at
* all.
* @return A subscription if the topic became available before the timeout expired or a nullptr
* otherwise.
*
* @throws BabelFishException If the message type for the given topic could not be loaded or a subscription could not
* be created for any other reason.
* @throws BabelFishException If the message type for the given topic could not be loaded or a
* subscription could not be created for any other reason.
*/
BabelFishSubscription::SharedPtr create_subscription(
rclcpp::Node &node, const std::string &topic, const rclcpp::QoS &qos,
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {},
std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
rclcpp::Node& node, const std::string& topic, const rclcpp::QoS& qos,
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {},
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));

//! Wrapper for create_subscription using type.
template<typename CallbackT>
BabelFishSubscription::SharedPtr
create_subscription( rclcpp::Node &node, const std::string &topic, const std::string &type,
const rclcpp::QoS &qos, CallbackT &&callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr,
rclcpp::SubscriptionOptions options = {} )
{
template <typename CallbackT>
BabelFishSubscription::SharedPtr create_subscription(
rclcpp::Node& node, const std::string& topic, const std::string& type, const rclcpp::QoS& qos,
CallbackT&& callback, rclcpp::CallbackGroup::SharedPtr group = nullptr,
rclcpp::SubscriptionOptions options = {}) {
#if RCLCPP_VERSION_MAJOR >= 9
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> any_callback;
#else
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> any_callback(
options.get_allocator() );
options.get_allocator());
#endif
any_callback.set( std::forward<CallbackT>( callback ) );
return create_subscription( node, topic, type, qos, any_callback, std::move( group ),
std::move( options ) );
any_callback.set(std::forward<CallbackT>(callback));
return create_subscription(node, topic, type, qos, any_callback, std::move(group),
std::move(options));
}

/*!
* This method will create a subscription for the given topic using the given message type.
* Since the message type is provided, it will not wait for the topic to become available.
* @param type The message type name for the given topic. E.g.: geometry_msgs/msg/Pose
* @param qos The quality of service options. Can be a number which will be the queue size, i.e., number of messages
* to queue for processing before dropping messages if the processing can't keep up.´
* @param qos The quality of service options. Can be a number which will be the queue size, i.e.,
* number of messages to queue for processing before dropping messages if the processing can't
* keep up.´
* @return A subscription to the given topic with the given message type.
*
* @throws BabelFishException If the given message type could not be loaded or a subscription could not
* be created for any other reason.
* @throws BabelFishException If the given message type could not be loaded or a subscription
* could not be created for any other reason.
*/
BabelFishSubscription::SharedPtr create_subscription(
rclcpp::Node &node, const std::string &topic, const std::string &type, const rclcpp::QoS &qos,
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {} );

BabelFishPublisher::SharedPtr create_publisher( rclcpp::Node &node, const std::string &topic,
const std::string &type, const rclcpp::QoS &qos,
rclcpp::PublisherOptions options = {} );

template<typename CallbackT>
BabelFishService::SharedPtr
create_service( rclcpp::Node &node, const std::string &service_name, const std::string &type,
CallbackT &&callback,
const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr )
{
AnyServiceCallback any_callback( std::forward<CallbackT>( callback ) );
return create_service( node, service_name, type, any_callback, qos_profile, group );
rclcpp::Node& node, const std::string& topic, const std::string& type, const rclcpp::QoS& qos,
rclcpp::AnySubscriptionCallback<CompoundMessage, std::allocator<void>> callback,
rclcpp::CallbackGroup::SharedPtr group = nullptr, rclcpp::SubscriptionOptions options = {});

BabelFishPublisher::SharedPtr create_publisher(rclcpp::Node& node, const std::string& topic,
const std::string& type, const rclcpp::QoS& qos,
rclcpp::PublisherOptions options = {});

template <typename CallbackT>
BabelFishService::SharedPtr create_service(
rclcpp::Node& node, const std::string& service_name, const std::string& type,
CallbackT&& callback, const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr) {
AnyServiceCallback any_callback(std::forward<CallbackT>(callback));
return create_service(node, service_name, type, any_callback, qos_profile, group);
}

BabelFishService::SharedPtr
create_service( rclcpp::Node &node, const std::string &service_name, const std::string &type,
AnyServiceCallback callback,
const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr );
BabelFishService::SharedPtr create_service(
rclcpp::Node& node, const std::string& service_name, const std::string& type,
AnyServiceCallback callback,
const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr);

BabelFishServiceClient::SharedPtr
create_service_client( rclcpp::Node &node, const std::string &service_name, const std::string &type,
const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr );
BabelFishServiceClient::SharedPtr create_service_client(
rclcpp::Node& node, const std::string& service_name, const std::string& type,
const rmw_qos_profile_t& qos_profile = rmw_qos_profile_services_default,
rclcpp::CallbackGroup::SharedPtr group = nullptr);

BabelFishActionClient::SharedPtr create_action_client(
rclcpp::Node &node, const std::string &name, const std::string &type,
const rcl_action_client_options_t &options = rcl_action_client_get_default_options(),
rclcpp::CallbackGroup::SharedPtr group = nullptr );
rclcpp::Node& node, const std::string& name, const std::string& type,
const rcl_action_client_options_t& options = rcl_action_client_get_default_options(),
rclcpp::CallbackGroup::SharedPtr group = nullptr);

/*!
* Creates an empty message of the given type.
Expand All @@ -148,10 +145,10 @@ class BabelFish : public std::enable_shared_from_this<BabelFish>
*
* @throws BabelFishException If the message description was not found
*/
CompoundMessage create_message( const std::string &type ) const;
CompoundMessage create_message(const std::string& type) const;

//! @copydoc create_message
CompoundMessage::SharedPtr create_message_shared( const std::string &type ) const;
CompoundMessage::SharedPtr create_message_shared(const std::string& type) const;

/*!
* Creates a service request message for the given service type.
Expand All @@ -160,22 +157,22 @@ class BabelFish : public std::enable_shared_from_this<BabelFish>
*
* @throws BabelFishException If the service description was not found
*/
CompoundMessage create_service_request( const std::string &type ) const;
CompoundMessage create_service_request(const std::string& type) const;

//! @copydoc create_service_request
CompoundMessage::SharedPtr create_service_request_shared( const std::string &type ) const;
CompoundMessage::SharedPtr create_service_request_shared(const std::string& type) const;

MessageTypeSupport::ConstSharedPtr get_message_type_support( const std::string &type ) const;
MessageTypeSupport::ConstSharedPtr get_message_type_support(const std::string& type) const;

ServiceTypeSupport::ConstSharedPtr get_service_type_support( const std::string &type ) const;
ServiceTypeSupport::ConstSharedPtr get_service_type_support(const std::string& type) const;

ActionTypeSupport::ConstSharedPtr get_action_type_support( const std::string &type ) const;
ActionTypeSupport::ConstSharedPtr get_action_type_support(const std::string& type) const;

std::vector<TypeSupportProvider::SharedPtr> type_support_providers();

private:
std::vector<TypeSupportProvider::SharedPtr> type_support_providers_;
};
} // namespace ros2_babel_fish
} // namespace ros2_babel_fish

#endif // ROS2_BABEL_FISH_BABEL_FISH_HPP
#endif // ROS2_BABEL_FISH_BABEL_FISH_HPP
Loading

0 comments on commit 2310f75

Please sign in to comment.