Skip to content

Commit 5504363

Browse files
lukicdarkoorenan028
lukicdarkoo
and
renan028
authored
Implement a plugin system in C++ and integrate ros2_control (#221)
* create webots launch without controller * add webots_ros2_cpp package with lidar only * add camera device cpp * add gps, imu, joint states * Initi * Improve name * Begin plugin design * Add plugin interface * Add plugin interface * Fix distane sensor plugin compilation * Register plugins * Add lidar * Add robot description * Parametrise lidar * Simplify * Fix step * Add robot tag * Add PointCloud2 * Cache messages * Add FLU fix * Add camera * Fix update rate * Add recognition * Fix timestep * Add gps * Simplify * Add warnings * Fix naming * Fix GPS * Prepare ros control * Fix * Plugin system works with pluginlib * Improve plugin interface * Update * Add ros2_control controller_manager * Fix * Fix * Add joint details * Start controller_manager thread * Add range finder * WIP: Add distance sensor * Add distance sensor * Add assert * Add light sensor * WIP: Add IMU * Fix * Improve * Stamped vel * WIP: Add TurtleBot * Basic TurtleBot integration * Include IMU * Add LED * Integrate ros2_control * Include robot_state_publisher * Add footprint transform * Add clock publisher * Add webots lib * Fix include * Fix deps * Fix * Prevent pluginlib from using boost * Fix * Fix * Change path * Rename * Delete submodule * Fix * Fix description * Mac and Windows * Delete unused * Single threaded executor is faster * Add changelog * Delete package attribute * Update WebotsNode.hpp * Update Driver.cpp * Rename * Update .gitmodules * Add unordered_map * Protected Co-authored-by: Renan Freitas <[email protected]>
1 parent ad78a04 commit 5504363

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+2818
-67
lines changed

‎.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,6 @@
11
[submodule "webots_ros2_importer/webots_ros2_importer/urdf2webots"]
22
path = webots_ros2_importer/webots_ros2_importer/urdf2webots
33
url = https://github.com/cyberbotics/urdf2webots.git
4+
[submodule "webots_ros2_driver/webots"]
5+
path = webots_ros2_driver/webots
6+
url = https://github.com/cyberbotics/webots-libcontroller.git

‎webots_ros2/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3-
<package format="2">
3+
<package format="3">
44
<name>webots_ros2</name>
55
<version>1.0.6</version>
66
<description>Interface between Webots and ROS2</description>

‎webots_ros2_control/CHANGELOG.rst‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package webots_ros2_control
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
1.0.6 (2021-05-21)
6+
------------------
7+
* Initial version

‎webots_ros2_control/CMakeLists.txt‎

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(webots_ros2_control)
3+
4+
# Default to C99
5+
if(NOT CMAKE_C_STANDARD)
6+
set(CMAKE_C_STANDARD 99)
7+
endif()
8+
9+
# Default to C++14
10+
if(NOT CMAKE_CXX_STANDARD)
11+
set(CMAKE_CXX_STANDARD 14)
12+
endif()
13+
14+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
15+
add_compile_options(-Wall -Wextra -Wpedantic)
16+
endif()
17+
18+
# Dependencies
19+
find_package(ament_cmake REQUIRED)
20+
find_package(hardware_interface REQUIRED)
21+
find_package(controller_manager REQUIRED)
22+
find_package(pluginlib REQUIRED)
23+
find_package(rclcpp REQUIRED)
24+
find_package(webots_ros2_driver REQUIRED)
25+
26+
list(GET webots_ros2_driver_INCLUDE_DIRS 0 webots_ros2_driver_INCLUDE)
27+
include_directories(
28+
${webots_ros2_driver_INCLUDE}/webots/cpp
29+
)
30+
31+
# webots_ros2 Plugin
32+
add_library(
33+
${PROJECT_NAME}
34+
SHARED
35+
src/Ros2Control.cpp
36+
)
37+
target_include_directories(
38+
${PROJECT_NAME}
39+
PRIVATE
40+
include
41+
)
42+
ament_target_dependencies(
43+
${PROJECT_NAME}
44+
hardware_interface
45+
controller_manager
46+
pluginlib
47+
rclcpp
48+
webots_ros2_driver
49+
)
50+
51+
# Prevent pluginlib from using boost
52+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
53+
pluginlib_export_plugin_description_file(webots_ros2_driver webots_ros2_control.xml)
54+
55+
# ros2_control System
56+
add_library(
57+
${PROJECT_NAME}_system
58+
SHARED
59+
src/Ros2ControlSystem.cpp
60+
)
61+
target_include_directories(
62+
${PROJECT_NAME}_system
63+
PRIVATE
64+
include
65+
)
66+
ament_target_dependencies(
67+
${PROJECT_NAME}_system
68+
hardware_interface
69+
pluginlib
70+
rclcpp
71+
webots_ros2_driver
72+
)
73+
74+
# Prevent pluginlib from using boost
75+
target_compile_definitions(${PROJECT_NAME}_system PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
76+
pluginlib_export_plugin_description_file(${PROJECT_NAME} webots_ros2_control_system.xml)
77+
78+
# Common
79+
install(TARGETS
80+
${PROJECT_NAME}
81+
${PROJECT_NAME}_system
82+
ARCHIVE DESTINATION lib
83+
LIBRARY DESTINATION lib
84+
RUNTIME DESTINATION bin
85+
)
86+
ament_export_include_directories(
87+
include
88+
)
89+
90+
if(BUILD_TESTING)
91+
find_package(ament_cmake_gtest REQUIRED)
92+
endif()
93+
94+
ament_export_dependencies(
95+
hardware_interface
96+
pluginlib
97+
rclcpp
98+
)
99+
ament_export_libraries(
100+
${PROJECT_NAME}
101+
${PROJECT_NAME}_system
102+
)
103+
104+
ament_package()

‎webots_ros2_control/README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# ros2_control plugin for Webots

‎webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
// Copyright 1996-2021 Cyberbotics Ltd.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef ROS2_CONTROL_HPP
16+
#define ROS2_CONTROL_HPP
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
#include <thread>
22+
23+
#include "hardware_interface/base_interface.hpp"
24+
#include "hardware_interface/system_interface.hpp"
25+
#include "hardware_interface/handle.hpp"
26+
#include "hardware_interface/hardware_info.hpp"
27+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
28+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
29+
#include "controller_manager/controller_manager.hpp"
30+
#include "rclcpp/macros.hpp"
31+
#include "webots_ros2_driver/PluginInterface.hpp"
32+
#include "webots_ros2_driver/WebotsNode.hpp"
33+
#include "webots_ros2_control/Ros2ControlSystemInterface.hpp"
34+
35+
namespace webots_ros2_control
36+
{
37+
class Ros2Control : public webots_ros2_driver::PluginInterface
38+
{
39+
public:
40+
void step() override;
41+
void init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters) override;
42+
43+
private:
44+
webots_ros2_driver::WebotsNode *mNode;
45+
std::shared_ptr<pluginlib::ClassLoader<Ros2ControlSystemInterface>> mHardwareLoader;
46+
std::shared_ptr<controller_manager::ControllerManager> mControllerManager;
47+
48+
std::thread mThreadExecutor;
49+
rclcpp::executors::MultiThreadedExecutor::SharedPtr mExecutor;
50+
};
51+
}
52+
53+
#endif

‎webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright 1996-2021 Cyberbotics Ltd.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef ROS2_CONTROL_HPP
16+
#define ROS2_CONTROL_HPP
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "hardware_interface/base_interface.hpp"
23+
#include "hardware_interface/system_interface.hpp"
24+
#include "hardware_interface/handle.hpp"
25+
#include "hardware_interface/hardware_info.hpp"
26+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
27+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
28+
#include "rclcpp/macros.hpp"
29+
#include "webots_ros2_driver/PluginInterface.hpp"
30+
#include "webots_ros2_driver/WebotsNode.hpp"
31+
32+
#include "webots_ros2_control/Ros2ControlSystemInterface.hpp"
33+
#include "webots/Motor.hpp"
34+
#include "webots/PositionSensor.hpp"
35+
36+
namespace webots_ros2_control
37+
{
38+
struct Joint {
39+
double positionCommand;
40+
double position;
41+
double velocityCommand;
42+
double velocity;
43+
double effortCommand;
44+
double effort;
45+
bool controlPosition;
46+
bool controlVelocity;
47+
bool controlEffort;
48+
std::string name;
49+
webots::Motor* motor;
50+
webots::PositionSensor* sensor;
51+
};
52+
53+
class Ros2ControlSystem : public Ros2ControlSystemInterface
54+
{
55+
public:
56+
void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override;
57+
58+
hardware_interface::return_type configure(const hardware_interface::HardwareInfo &info) override;
59+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
60+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
61+
hardware_interface::return_type start() override;
62+
hardware_interface::return_type stop() override;
63+
hardware_interface::return_type read() override;
64+
hardware_interface::return_type write() override;
65+
66+
private:
67+
webots_ros2_driver::WebotsNode *mNode;
68+
std::vector<Joint> mJoints;
69+
};
70+
}
71+
72+
#endif

‎webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
// Copyright 1996-2021 Cyberbotics Ltd.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef ROS2_CONTROL_INTERFACE_HPP
16+
#define ROS2_CONTROL_INTERFACE_HPP
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "hardware_interface/base_interface.hpp"
23+
#include "hardware_interface/system_interface.hpp"
24+
#include "hardware_interface/handle.hpp"
25+
#include "hardware_interface/hardware_info.hpp"
26+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
27+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
28+
#include "webots_ros2_driver/PluginInterface.hpp"
29+
#include <webots/Supervisor.hpp>
30+
#include "webots_ros2_driver/WebotsNode.hpp"
31+
32+
namespace webots_ros2_control
33+
{
34+
class Ros2ControlSystemInterface : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
35+
{
36+
public:
37+
virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0;
38+
};
39+
}
40+
41+
#endif

‎webots_ros2_control/package.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>webots_ros2_control</name>
5+
<version>1.0.6</version>
6+
<description>ros2_control plugin for Webots</description>
7+
<maintainer email="[email protected]">Cyberbotics</maintainer>
8+
<url type="website">http://wiki.ros.org/webots_ros2</url>
9+
<url type="bugtracker">https://github.com/cyberbotics/webots_ros2/issues</url>
10+
<url type="repository">https://github.com/cyberbotics/webots_ros2</url>
11+
<license>Apache License 2.0</license>
12+
13+
<depend>hardware_interface</depend>
14+
<depend>controller_manager</depend>
15+
<depend>pluginlib</depend>
16+
<depend>rclcpp</depend>
17+
<depend>webots_ros2_driver</depend>
18+
19+
<buildtool_depend>ament_cmake</buildtool_depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>ament_lint_common</test_depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

0 commit comments

Comments
 (0)