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

Extend drake_ros_core package. #6

Merged
merged 15 commits into from
Dec 6, 2021
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
2 changes: 2 additions & 0 deletions .github/workflows/main.yml
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ jobs:
- uses: ros-tooling/[email protected]
with:
required-ros-distributions: ${{ matrix.ros_distribution }}
- name: Cope with Python 2 pollution
run: apt-get update && apt-get install -y python-is-python3
- name: Build and test all packages
uses: ros-tooling/[email protected]
with:
Expand Down
47 changes: 39 additions & 8 deletions drake_ros_core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,19 @@ endif()

find_package(ament_cmake_ros REQUIRED)
find_package(drake REQUIRED)
# Must use Drake's fork of Pybind11
find_package(pybind11 REQUIRED HINTS "${drake_DIR}/../pybind11" NO_DEFAULT_PATH)
find_package(rclcpp REQUIRED)
find_package(rosidl_runtime_c REQUIRED)
find_package(rosidl_typesupport_cpp REQUIRED)

add_library(drake_ros_core
src/drake_ros.cpp
src/publisher.cpp
src/ros_interface_system.cpp
src/ros_publisher_system.cpp
src/ros_subscriber_system.cpp
src/subscription.cpp
src/drake_ros.cc
src/publisher.cc
src/ros_interface_system.cc
src/ros_publisher_system.cc
src/ros_subscriber_system.cc
src/subscription.cc
)

target_link_libraries(drake_ros_core PUBLIC
Expand Down Expand Up @@ -59,25 +61,54 @@ install(
DESTINATION include
)

ament_python_install_package(
drake_ros_core PACKAGE_DIR src/drake_ros_core)

###
# Python bindings
###
pybind11_add_module(drake_ros_core_py SHARED
src/python_bindings/drake_ros_core.cc
)
target_link_libraries(drake_ros_core_py PRIVATE drake_ros_core)
set_target_properties(drake_ros_core_py PROPERTIES OUTPUT_NAME "_drake_ros_core")
target_include_directories(drake_ros_core_py
PRIVATE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/python_bindings>"
)

ament_get_python_install_dir(python_install_dir)

install(
TARGETS drake_ros_core_py
DESTINATION "${python_install_dir}"
)
### End Python bindings

if(BUILD_TESTING)
find_package(ament_cmake_clang_format REQUIRED)
find_package(ament_cmake_cpplint REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_pytest REQUIRED)
find_package(ament_cmake_pycodestyle REQUIRED)
find_package(test_msgs REQUIRED)

ament_clang_format(CONFIG_FILE .clang-format)
ament_cpplint()

ament_add_gtest(test_pub_sub test/test_pub_sub.cpp)
ament_pycodestyle(--config pycodestyle.ini)

ament_add_gtest(test_pub_sub test/test_pub_sub.cc)

target_link_libraries(test_pub_sub
drake::drake
drake_ros_core
${test_msgs_TARGETS}
)

ament_add_gtest(test_drake_ros test/test_drake_ros.cpp)
ament_add_pytest_test(test_pub_sub_py test/test_pub_sub.py)

ament_add_gtest(test_drake_ros test/test_drake_ros.cc)
target_link_libraries(test_drake_ros
drake_ros_core
)
Expand Down
6 changes: 6 additions & 0 deletions drake_ros_core/CPPLINT.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,12 @@ filter=+build/pragma_once
# Disable cpplint's include order. We have our own via //tools:drakelint.
filter=-build/include_order

# TODO(hidmic): uncomment when ament_cpplint updates its vendored cpplint
# version to support the following filters. Also remove corresponding NOLINT
# codetags in sources.
## Allow private, sibling include files.
#filter=-build/include_subdir

# We do not care about the whitespace details of a TODO comment. It is not
# relevant for easy grepping, and the GSG does not specify any particular
# whitespace style. (We *do* care what the "TODO(username)" itself looks like
Expand Down
81 changes: 81 additions & 0 deletions drake_ros_core/include/drake_ros_core/drake_ros.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
// Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once

#include <memory>
#include <string>

#include <rclcpp/node.hpp>
#include <rclcpp/node_options.hpp>

namespace drake_ros_core {

/** A Drake ROS interface that wraps a live ROS node.

This interface manages both ROS node construction and scheduling
(using a `rclcpp::executors::SingleThreadedExecutor` instance).
See `rclcpp::Node` and `rclcpp::Executor` documentation for further
reference on expected behavior.
*/
class DrakeRos final {
public:
/** A constructor that wraps a `node_name` ROS node with `node_options`.
See `rclcpp::Node::Node` documentation for further reference on arguments.
*/
DrakeRos(const std::string& node_name,
rclcpp::NodeOptions node_options = rclcpp::NodeOptions{});

~DrakeRos();

/** Returns a constant reference to the underlying ROS node. */
const rclcpp::Node& get_node() const;

/** Returns a mutable reference to the underlying ROS node. */
rclcpp::Node* get_mutable_node() const;

/** Spins the underlying ROS node, dispatching all available work if any.

In this context, work refers to subscription callbacks, timer callbacks,
service request and reply callbacks, etc., that are registered with the
underlying `rclcpp::Node` instance. Availability implies these are ready
to be serviced by the underlying `rclcpp::Executor` instance.

This method's behavior has been modeled after that of the
`drake::lcm::DrakeLcm::HandleSubscriptions()` method (to a partial extent).

@param[in] timeout_millis Timeout, in milliseconds, when fetching work.
Negative timeout values are not allowed. If timeout is 0, the call will
not wait for any new work. If timeout is larger than 0, the call will
continue fetching work up to the given timeout or until no work is
available.
@throws std::runtime_error if timeout is negative.
*/
void Spin(int timeout_millis = 0);

private:
struct Impl;
std::unique_ptr<Impl> impl_;
};

/**Initialize Drake ROS's global context.
This function decorates a `rclcpp::init` invocation.
*/
void init(int argc = 0, const char** argv = nullptr);

/**Shutdown Drake ROS's global context.
This function decorates a `rclcpp::shutdown` invocation.
*/
bool shutdown();

} // namespace drake_ros_core
60 changes: 0 additions & 60 deletions drake_ros_core/include/drake_ros_core/drake_ros.hpp

This file was deleted.

47 changes: 0 additions & 47 deletions drake_ros_core/include/drake_ros_core/drake_ros_interface.hpp

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -11,35 +11,33 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_
#define DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_
#pragma once

#include <memory>

#include <drake/systems/framework/leaf_system.h>

#include "drake_ros_core/drake_ros_interface.hpp"
#include "drake_ros_core/drake_ros.h"

namespace drake_ros_core {
// PIMPL forward declaration
class RosInterfaceSystemPrivate;

/// System that takes care of calling spin() in Drake's systems framework
/** A system that manages a Drake ROS interface. */
class RosInterfaceSystem : public drake::systems::LeafSystem<double> {
public:
explicit RosInterfaceSystem(std::unique_ptr<DrakeRosInterface> ros);
virtual ~RosInterfaceSystem();
/** A constructor that takes ownership of the `ros` interface. */
explicit RosInterfaceSystem(std::unique_ptr<DrakeRos> ros);

~RosInterfaceSystem() override;

/// Return a handle for interacting with ROS
std::shared_ptr<DrakeRosInterface> get_ros_interface() const;
/** Returns a mutable reference to the underlying ROS interface. */
DrakeRos* get_ros_interface() const;

protected:
/// Override as a place to call rclcpp::spin()
void DoCalcNextUpdateTime(const drake::systems::Context<double>&,
drake::systems::CompositeEventCollection<double>*,
double*) const override;

std::unique_ptr<RosInterfaceSystemPrivate> impl_;
private:
struct Impl;
std::unique_ptr<Impl> impl_;
};
} // namespace drake_ros_core
#endif // DRAKE_ROS_CORE__ROS_INTERFACE_SYSTEM_HPP_
Loading