Skip to content

Commit 3a89a83

Browse files
tz61tz61
authored
Merge pull request #16 from hanbinzheng/master
add armor tester controller
2 parents ddb77a3 + 66b3007 commit 3a89a83

File tree

15 files changed

+708
-19
lines changed

15 files changed

+708
-19
lines changed

‎.gitignore‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,4 +2,5 @@ build
22
install
33
log
44
__pycache__/
5-
.cache
5+
.cache
6+
.vscode

‎decision/armor_tester/CMakeLists.txt‎

Lines changed: 26 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,26 +1,44 @@
1-
cmake_minimum_required(VERSION 3.8)
2-
project(armor_tester)
1+
cmake_minimum_required(VERSION 3.8) # the mininal version
2+
project(armor_tester) # define the project name here, and it influence the variable ${PROJECT_NAME}
33

4+
# only acts for GCC and Clang
45
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
56
add_compile_options(-Wall -Wextra -Wpedantic)
67
endif()
78

89
# find dependencies
9-
find_package(ament_cmake REQUIRED)
10-
find_package(ament_cmake_auto REQUIRED)
11-
ament_auto_find_build_dependencies()
10+
find_package(ament_cmake REQUIRED) # ament_cmake is mandatory
11+
find_package(ament_cmake_auto REQUIRED) # ament_cmake_auto is also mandatory
12+
ament_auto_find_build_dependencies() # scan the package.xml to get the dependencies
1213

14+
# construct controller shared library to complile the .so lib
1315
ament_auto_add_library(
14-
${PROJECT_NAME} SHARED
16+
${PROJECT_NAME}
17+
SHARED
1518
src/armor_tester.cpp
16-
)
19+
)
1720

21+
# register a class in library as plugin, here, the ArmorTester
1822
rclcpp_components_register_node(
1923
${PROJECT_NAME}
2024
PLUGIN "ArmorTester"
2125
EXECUTABLE ${PROJECT_NAME}_node
22-
)
26+
)
27+
# rclcpp_components_register_node(
28+
# <library_target> # existing library name
29+
# PLUGIN "<class_name>" # existing class name
30+
# EXECUTABLE <executable_name> # self-designed
31+
# )
32+
33+
# add the include file
34+
target_include_directories(
35+
${PROJECT_NAME}
36+
PUBLIC
37+
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
38+
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
39+
)
2340

41+
# test for building
2442
if(BUILD_TESTING)
2543
find_package(ament_lint_auto REQUIRED)
2644
# the following line skips the linter which checks for copyrights

‎decision/armor_tester/package.xml‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,16 +4,25 @@
44
<name>armor_tester</name>
55
<version>0.0.0</version>
66
<description>TODO: Package description</description>
7-
<maintainer email="mcerztn@gmail.com">meta</maintainer>
7+
<maintainer email="hanbinzheng54@gmail.com">hanbinzheng</maintainer>
88
<license>TODO: License declaration</license>
99

1010
<buildtool_depend>ament_cmake</buildtool_depend>
1111
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1212

13+
<build_depend>generate_parameter_library</build_depend>
14+
1315
<depend>operation_interface</depend>
1416
<depend>geometry_msgs</depend>
1517
<depend>behavior_interface</depend>
18+
<depend>control_msgs</depend>
19+
<depend>control_toolbox</depend>
20+
<depend>controller_interface</depend>
21+
<depend>hardware_interface</depend>
22+
<depend>realtime_tools</depend>
23+
<depend>pluginlib</depend>
1624
<depend>rclcpp</depend>
25+
<depend>rclcpp_lifecycle</depend>
1726
<depend>rclcpp_components</depend>
1827

1928
<test_depend>ament_lint_auto</test_depend>

‎decision/armor_tester/src/armor_tester.cpp‎

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
#include "rclcpp/rclcpp.hpp"
22
#include <geometry_msgs/msg/twist.hpp>
33
#include <operation_interface/msg/dbus_control.hpp>
4+
#include "behavior_interface/msg/armor.hpp"
45
#include <std_msgs/msg/float64_multi_array.hpp>
56
#include <memory>
67

78
#define PUB_RATE 15 // ms
9+
#define PI 3.1415926
810

911
class ArmorTester : public rclcpp::Node
1012
{
@@ -14,7 +16,8 @@ class ArmorTester : public rclcpp::Node
1416
// reset
1517
ls_x = ls_y = rs_x = rs_y = wheel = 0;
1618
lsw = rsw = "";
17-
motor_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("/forward_position_controller/commands", 10);
19+
velocity_pub_ = this->create_publisher<behavior_interface::msg::Armor>(
20+
"/armor_tester_controller/reference", 10);
1821

1922
dbus_sub_ = this->create_subscription<operation_interface::msg::DbusControl>(
2023
"dbus_control", 10,
@@ -31,7 +34,7 @@ class ArmorTester : public rclcpp::Node
3134

3235
private:
3336
rclcpp::Subscription<operation_interface::msg::DbusControl>::SharedPtr dbus_sub_;
34-
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr motor_pub_;
37+
rclcpp::Publisher<behavior_interface::msg::Armor>::SharedPtr velocity_pub_;
3538
rclcpp::TimerBase::SharedPtr timer_;
3639
double ls_x, ls_y, rs_x, rs_y, wheel;
3740
std::string lsw, rsw;
@@ -52,14 +55,14 @@ class ArmorTester : public rclcpp::Node
5255
{
5356
if (lsw != "MID") return;
5457

55-
std_msgs::msg::Float64MultiArray tmp_motor_velocity;
56-
std::vector<double> motor_velocity;
57-
motor_velocity.push_back(ls_x*6.28*2);
58-
RCLCPP_INFO(this->get_logger(), "speed:%lf", ls_x*6.28*2);
59-
tmp_motor_velocity.data = motor_velocity;
58+
behavior_interface::msg::Armor armor_tester_velocity;
59+
armor_tester_velocity.unitree_vel = ls_y * 4 * PI;
60+
armor_tester_velocity.dji_vel = rs_y * 4 * PI;
6061

61-
motor_pub_->publish(tmp_motor_velocity);
62+
RCLCPP_INFO(this->get_logger(), "unitree_vel:%lf, dji_vel: %lf",
63+
ls_y * 4 * PI, rs_y * 4 * PI);
64+
velocity_pub_->publish(armor_tester_velocity);
6265
}
6366
};
6467
#include <rclcpp_components/register_node_macro.hpp>
65-
RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester)
68+
RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester)

‎decomposition/meta_utils_controller/CMakeLists.txt‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
cmake_minimum_required(VERSION 3.8) # the mininal version
2+
project(meta_utils_controller) # define the project name here, and it influence the variable ${PROJECT_NAME}
3+
4+
# only acts for GCC and Clang
5+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
6+
add_compile_options(-Wall -Wextra -Wpedantic)
7+
endif()
8+
9+
# find dependencies
10+
find_package(ament_cmake REQUIRED) # ament_cmake is mandatory
11+
find_package(ament_cmake_auto REQUIRED) # ament_cmake_auto is also mandatory
12+
ament_auto_find_build_dependencies() # scan the package.xml to get the dependencies
13+
14+
# generate parameter library for controller
15+
generate_parameter_library(
16+
armor_tester_controller_parameters
17+
src/armor_tester_controller.yaml
18+
)
19+
20+
# generate_parameter_lib (
21+
# name_of_target, -> to generate target_name.cpp
22+
# yaml_file, -> generate the parameter listeners based on which yaml file
23+
# )
24+
25+
ament_auto_add_library(
26+
armor_tester_controller
27+
SHARED
28+
src/armor_tester_controller.cpp
29+
)
30+
31+
# ament_auto_add_library (
32+
# target_package_name
33+
# SHARED/STATIC/OBJECT
34+
# sorce file
35+
# )
36+
37+
target_include_directories(
38+
armor_tester_controller
39+
PUBLIC
40+
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
41+
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
42+
)
43+
44+
# target_include_directories(
45+
# cmake-target
46+
# PUBLIC
47+
# <BUILD_INTERFACE:.....> # used during build
48+
# <INSTALL_INTERFACE:...> # used during install
49+
# )
50+
51+
# link the the parameter listeners
52+
target_link_libraries(
53+
armor_tester_controller
54+
armor_tester_controller_parameters
55+
)
56+
# target_link_libraries(
57+
# cmake-target-package
58+
# cmake-target that cmake-target-package depends on
59+
# )
60+
61+
# where to find the description of the controller
62+
pluginlib_export_plugin_description_file(
63+
controller_interface
64+
armor_tester_controller.xml
65+
)
66+
# pluginlib_export_plugin_description_file(
67+
# base-class
68+
# description file
69+
# )
70+
71+
# test for building
72+
if(BUILD_TESTING)
73+
find_package(ament_lint_auto REQUIRED)
74+
# the following line skips the linter which checks for copyrights
75+
# comment the line when a copyright and license is added to all source files
76+
set(ament_cmake_copyright_FOUND TRUE)
77+
# the following line skips cpplint (only works in a git repo)
78+
# comment the line when this package is in a git repo and when
79+
# a copyright and license is added to all source files
80+
set(ament_cmake_cpplint_FOUND TRUE)
81+
ament_lint_auto_find_test_dependencies()
82+
endif()
83+
84+
ament_auto_package()

‎decomposition/meta_utils_controller/armor_tester_controller.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<!--
2+
Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
http://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
16+
17+
Source of this file are templates in
18+
[RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
19+
-->
20+
21+
<library path="armor_tester_controller">
22+
<class name="armor_tester_controller/ArmorTesterController"
23+
type="armor_tester_controller::ArmorTesterController" base_class_type="controller_interface::ChainableControllerInterface">
24+
<description>
25+
An Armor Tester Controller.
26+
</description>
27+
</class>
28+
</library>

‎decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
#ifndef ARMOR_TESTER_CONTROLLER_HPP_
2+
#define ARMOR_TESTER_CONTROLLER_HPP_
3+
4+
#include <memory>
5+
#include <string>
6+
#include <vector>
7+
#include <control_toolbox/pid_ros.hpp>
8+
#include <rclcpp/subscription.hpp>
9+
10+
#include "controller_interface/controller_interface.hpp"
11+
#include "controller_interface/chainable_controller_interface.hpp"
12+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
13+
#include "rclcpp/duration.hpp"
14+
#include "rclcpp_lifecycle/state.hpp"
15+
#include "realtime_tools/realtime_buffer.hpp"
16+
#include "realtime_tools/realtime_publisher.hpp"
17+
18+
#include <meta_utils_controller/armor_tester_controller_parameters.hpp>
19+
#include "behavior_interface/msg/armor.hpp" // dji_vel, unitree_vel
20+
21+
namespace armor_tester_controller // namespace begin here
22+
{
23+
24+
class ArmorTesterController : public controller_interface::ChainableControllerInterface
25+
{
26+
public:
27+
ArmorTesterController() = default;
28+
29+
// ControllerInterfaceBase and ChainableControllerInterface, a little strange
30+
~ArmorTesterController() = default;
31+
32+
// override method from ControllerInterfaceBase (done?)
33+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
34+
35+
// override method from ControllerInterfaceBase (done?)
36+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
37+
38+
// override method from ControllerInterfaceBase (done)
39+
controller_interface::CallbackReturn on_init() override;
40+
41+
// override method from ControllerInterfaceBase (done)
42+
controller_interface::CallbackReturn on_configure(
43+
const rclcpp_lifecycle::State & previous_state) override;
44+
45+
// override method from ControllerInterfaceBase (done)
46+
controller_interface::CallbackReturn on_activate(
47+
const rclcpp_lifecycle::State & previous_state) override;
48+
49+
// override method from ControllerInterfaceBase (done)
50+
controller_interface::CallbackReturn on_deactivate(
51+
const rclcpp_lifecycle::State & previous_state) override;
52+
53+
// override method from ChainableControllerInterface
54+
controller_interface::return_type update_and_write_commands(
55+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
56+
57+
protected:
58+
// override method from ChainableControllerInterface
59+
#if RCLCPP_VERSION_MAJOR >= 28 // Ros2 Jazzy or latter
60+
controller_interface::return_type update_reference_from_subscribers(
61+
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
62+
#else
63+
controller_interface::return_type update_reference_from_subscribers() override;
64+
#endif
65+
66+
// parameters
67+
armor_tester_controller::Params params_;
68+
std::shared_ptr<armor_tester_controller::ParamListener> param_listener_;
69+
70+
// pid for dji motor
71+
std::shared_ptr<control_toolbox::PidROS> dji_pid_;
72+
73+
// for subscriber
74+
rclcpp::Subscription<behavior_interface::msg::Armor>::SharedPtr vel_subscriber_ = nullptr;
75+
realtime_tools::RealtimeBuffer<std::shared_ptr<behavior_interface::msg::Armor>> vel_buffer_;
76+
77+
// override method from ChainableControllerInterface (done)
78+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
79+
80+
private:
81+
// callback function for subscriber (done)
82+
void velocity_callback(const std::shared_ptr<behavior_interface::msg::Armor> msg);
83+
}; // class definition ends here
84+
85+
} // namespace ends here
86+
87+
#endif // ARMOR_TESTER_CONTROLLER_HPP_

‎decomposition/meta_utils_controller/package.xml‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
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>meta_utils_controller</name>
5+
<version>0.0.0</version>
6+
<description>Package for Meta Utils</description>
7+
<maintainer email="[email protected]">hanbinzheng</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
12+
13+
<build_depend>generate_parameter_library</build_depend>
14+
15+
<depend>operation_interface</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>behavior_interface</depend>
18+
<depend>control_msgs</depend>
19+
<depend>control_toolbox</depend>
20+
<depend>controller_interface</depend>
21+
<depend>hardware_interface</depend>
22+
<depend>realtime_tools</depend>
23+
<depend>pluginlib</depend>
24+
<depend>rclcpp</depend>
25+
<depend>rclcpp_lifecycle</depend>
26+
<depend>rclcpp_components</depend>
27+
28+
<test_depend>ament_lint_auto</test_depend>
29+
<test_depend>ament_lint_common</test_depend>
30+
31+
<export>
32+
<build_type>ament_cmake</build_type>
33+
</export>
34+
</package>

0 commit comments

Comments
 (0)