diff --git a/.gitignore b/.gitignore index e129ef2..fce3960 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ build install log __pycache__/ -.cache \ No newline at end of file +.cache +.vscode diff --git a/decision/armor_tester/CMakeLists.txt b/decision/armor_tester/CMakeLists.txt index 24826c3..f49deb3 100644 --- a/decision/armor_tester/CMakeLists.txt +++ b/decision/armor_tester/CMakeLists.txt @@ -1,26 +1,44 @@ -cmake_minimum_required(VERSION 3.8) -project(armor_tester) +cmake_minimum_required(VERSION 3.8) # the mininal version +project(armor_tester) # define the project name here, and it influence the variable ${PROJECT_NAME} +# only acts for GCC and Clang if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(ament_cmake REQUIRED) # ament_cmake is mandatory +find_package(ament_cmake_auto REQUIRED) # ament_cmake_auto is also mandatory +ament_auto_find_build_dependencies() # scan the package.xml to get the dependencies +# construct controller shared library to complile the .so lib ament_auto_add_library( - ${PROJECT_NAME} SHARED + ${PROJECT_NAME} + SHARED src/armor_tester.cpp - ) +) +# register a class in library as plugin, here, the ArmorTester rclcpp_components_register_node( ${PROJECT_NAME} PLUGIN "ArmorTester" EXECUTABLE ${PROJECT_NAME}_node - ) +) +# rclcpp_components_register_node( +# # existing library name +# PLUGIN "" # existing class name +# EXECUTABLE # self-designed +# ) + +# add the include file +target_include_directories( + ${PROJECT_NAME} + PUBLIC + "$" + "$" +) +# test for building if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/decision/armor_tester/package.xml b/decision/armor_tester/package.xml index ba5c9be..f8e8cf4 100644 --- a/decision/armor_tester/package.xml +++ b/decision/armor_tester/package.xml @@ -4,16 +4,25 @@ armor_tester 0.0.0 TODO: Package description - meta + hanbinzheng TODO: License declaration ament_cmake ament_cmake_auto + generate_parameter_library + operation_interface geometry_msgs behavior_interface + control_msgs + control_toolbox + controller_interface + hardware_interface + realtime_tools + pluginlib rclcpp + rclcpp_lifecycle rclcpp_components ament_lint_auto diff --git a/decision/armor_tester/src/armor_tester.cpp b/decision/armor_tester/src/armor_tester.cpp index 92bbad6..e934630 100644 --- a/decision/armor_tester/src/armor_tester.cpp +++ b/decision/armor_tester/src/armor_tester.cpp @@ -1,10 +1,12 @@ #include "rclcpp/rclcpp.hpp" #include #include +#include "behavior_interface/msg/armor.hpp" #include #include #define PUB_RATE 15 // ms +#define PI 3.1415926 class ArmorTester : public rclcpp::Node { @@ -14,7 +16,8 @@ class ArmorTester : public rclcpp::Node // reset ls_x = ls_y = rs_x = rs_y = wheel = 0; lsw = rsw = ""; - motor_pub_ = this->create_publisher("/forward_position_controller/commands", 10); + velocity_pub_ = this->create_publisher( + "/armor_tester_controller/reference", 10); dbus_sub_ = this->create_subscription( "dbus_control", 10, @@ -31,7 +34,7 @@ class ArmorTester : public rclcpp::Node private: rclcpp::Subscription::SharedPtr dbus_sub_; - rclcpp::Publisher::SharedPtr motor_pub_; + rclcpp::Publisher::SharedPtr velocity_pub_; rclcpp::TimerBase::SharedPtr timer_; double ls_x, ls_y, rs_x, rs_y, wheel; std::string lsw, rsw; @@ -52,14 +55,14 @@ class ArmorTester : public rclcpp::Node { if (lsw != "MID") return; - std_msgs::msg::Float64MultiArray tmp_motor_velocity; - std::vector motor_velocity; - motor_velocity.push_back(ls_x*6.28*2); - RCLCPP_INFO(this->get_logger(), "speed:%lf", ls_x*6.28*2); - tmp_motor_velocity.data = motor_velocity; + behavior_interface::msg::Armor armor_tester_velocity; + armor_tester_velocity.unitree_vel = ls_y * 4 * PI; + armor_tester_velocity.dji_vel = rs_y * 4 * PI; - motor_pub_->publish(tmp_motor_velocity); + RCLCPP_INFO(this->get_logger(), "unitree_vel:%lf, dji_vel: %lf", + ls_y * 4 * PI, rs_y * 4 * PI); + velocity_pub_->publish(armor_tester_velocity); } }; #include -RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester) diff --git a/decomposition/meta_utils_controller/CMakeLists.txt b/decomposition/meta_utils_controller/CMakeLists.txt new file mode 100644 index 0000000..8586c60 --- /dev/null +++ b/decomposition/meta_utils_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.8) # the mininal version +project(meta_utils_controller) # define the project name here, and it influence the variable ${PROJECT_NAME} + +# only acts for GCC and Clang +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) # ament_cmake is mandatory +find_package(ament_cmake_auto REQUIRED) # ament_cmake_auto is also mandatory +ament_auto_find_build_dependencies() # scan the package.xml to get the dependencies + +# generate parameter library for controller +generate_parameter_library( + armor_tester_controller_parameters + src/armor_tester_controller.yaml +) + +# generate_parameter_lib ( +# name_of_target, -> to generate target_name.cpp +# yaml_file, -> generate the parameter listeners based on which yaml file +# ) + +ament_auto_add_library( + armor_tester_controller + SHARED + src/armor_tester_controller.cpp +) + +# ament_auto_add_library ( +# target_package_name +# SHARED/STATIC/OBJECT +# sorce file +# ) + +target_include_directories( + armor_tester_controller + PUBLIC + "$" + "$" +) + +# target_include_directories( +# cmake-target +# PUBLIC +# # used during build +# # used during install +# ) + +# link the the parameter listeners +target_link_libraries( + armor_tester_controller + armor_tester_controller_parameters +) +# target_link_libraries( +# cmake-target-package +# cmake-target that cmake-target-package depends on +# ) + +# where to find the description of the controller +pluginlib_export_plugin_description_file( + controller_interface + armor_tester_controller.xml +) +# pluginlib_export_plugin_description_file( +# base-class +# description file +# ) + +# test for building +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/decomposition/meta_utils_controller/armor_tester_controller.xml b/decomposition/meta_utils_controller/armor_tester_controller.xml new file mode 100644 index 0000000..443e55c --- /dev/null +++ b/decomposition/meta_utils_controller/armor_tester_controller.xml @@ -0,0 +1,28 @@ + + + + + + An Armor Tester Controller. + + + diff --git a/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp new file mode 100644 index 0000000..a36eee3 --- /dev/null +++ b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp @@ -0,0 +1,87 @@ +#ifndef ARMOR_TESTER_CONTROLLER_HPP_ +#define ARMOR_TESTER_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" + +#include +#include "behavior_interface/msg/armor.hpp" // dji_vel, unitree_vel + +namespace armor_tester_controller // namespace begin here +{ + +class ArmorTesterController : public controller_interface::ChainableControllerInterface +{ + public: + ArmorTesterController() = default; + + // ControllerInterfaceBase and ChainableControllerInterface, a little strange + ~ArmorTesterController() = default; + + // override method from ControllerInterfaceBase (done?) + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + // override method from ControllerInterfaceBase (done?) + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + // override method from ControllerInterfaceBase (done) + controller_interface::CallbackReturn on_init() override; + + // override method from ControllerInterfaceBase (done) + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + // override method from ControllerInterfaceBase (done) + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + // override method from ControllerInterfaceBase (done) + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + // override method from ChainableControllerInterface + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + protected: + // override method from ChainableControllerInterface + #if RCLCPP_VERSION_MAJOR >= 28 // Ros2 Jazzy or latter + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + #else + controller_interface::return_type update_reference_from_subscribers() override; + #endif + + // parameters + armor_tester_controller::Params params_; + std::shared_ptr param_listener_; + + // pid for dji motor + std::shared_ptr dji_pid_; + + // for subscriber + rclcpp::Subscription::SharedPtr vel_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> vel_buffer_; + + // override method from ChainableControllerInterface (done) + std::vector on_export_reference_interfaces() override; + + private: + // callback function for subscriber (done) + void velocity_callback(const std::shared_ptr msg); +}; // class definition ends here + +} // namespace ends here + +#endif // ARMOR_TESTER_CONTROLLER_HPP_ diff --git a/decomposition/meta_utils_controller/package.xml b/decomposition/meta_utils_controller/package.xml new file mode 100644 index 0000000..43b5808 --- /dev/null +++ b/decomposition/meta_utils_controller/package.xml @@ -0,0 +1,34 @@ + + + + meta_utils_controller + 0.0.0 + Package for Meta Utils + hanbinzheng + TODO: License declaration + + ament_cmake + ament_cmake_auto + + generate_parameter_library + + operation_interface + geometry_msgs + behavior_interface + control_msgs + control_toolbox + controller_interface + hardware_interface + realtime_tools + pluginlib + rclcpp + rclcpp_lifecycle + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/decomposition/meta_utils_controller/src/armor_tester_controller.cpp b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp new file mode 100644 index 0000000..564a194 --- /dev/null +++ b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp @@ -0,0 +1,221 @@ +#include "meta_utils_controller/armor_tester_controller.hpp" + +#include +#include +#include +#include +#include +#include "behavior_interface/msg/armor.hpp" // unitree_vel, dji_vel +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_VELOCITY; +constexpr double NaN = std::numeric_limits::quiet_NaN(); + +namespace armor_tester_controller { + controller_interface::CallbackReturn ArmorTesterController::on_init() { + // The first line usually calls the parent on_init() method + // Here is the best place to initialize the variables, reserve memory, + // and declare node parameters used by the controller + + // call father on_init() method + // auto ret = ChainableControllerInterface::on_init(); + // if (ret != controller_interface::CallbackReturn::SUCCESS) { + // return ret; + // } + + // declare and set node parameters + try { + param_listener_ = std::make_shared (get_node()); + } catch (const std::exception & e) { + std::cerr << "Exception thrown during controller's init with message: " + << e.what() << std::endl; + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + // parameters are usually read here + // everything is prepared so that the controller can be started. + + // read parameters + params_ = param_listener_->get_params(); + + // initialize dji pid + dji_pid_ = std::make_shared ( + get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); + if (!dji_pid_->initPid()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for DJI Motor"); + return controller_interface::CallbackReturn::FAILURE; + } + + // topic QoS (Quality of Service) + auto vel_sub_qos = rclcpp::SystemDefaultsQoS(); + vel_sub_qos.keep_last(1); + vel_sub_qos.best_effort(); + // create subscriber + vel_subscriber_ = get_node()->create_subscription( + "~/reference", vel_sub_qos, + std::bind(& ArmorTesterController::velocity_callback, this, std::placeholders::_1)); + // create, initialize the message, and write it the real time buffer + auto vel_msg = std::make_shared(); + vel_msg->dji_vel = NaN; + vel_msg->unitree_vel = NaN; + vel_buffer_.writeFromNonRT(vel_msg); + + RCLCPP_INFO(get_node()->get_logger(), "configure successfully"); + return controller_interface::CallbackReturn::SUCCESS; + } + + + controller_interface::InterfaceConfiguration + ArmorTesterController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration cmd_itf_config; + + cmd_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itf_config.names.reserve(2); + cmd_itf_config.names.push_back( + params_.unitree_joint.name + "/" + HW_IF_VELOCITY); + cmd_itf_config.names.push_back( + params_.dji_joint.name + "/" + HW_IF_EFFORT); + + return cmd_itf_config; + } + + controller_interface::InterfaceConfiguration + ArmorTesterController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration state_itf_config; + + state_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itf_config.names.reserve(1); + + // only the dji velocity is required + state_itf_config.names.push_back(params_.dji_joint.name + "/" + HW_IF_VELOCITY); + + return state_itf_config; + } + + void ArmorTesterController::velocity_callback( + const std::shared_ptr msg) { + // shared_ptr<..>, and still writeFromNonRT(msg)? not writeFromNonRT(*msg)??? + vel_buffer_.writeFromNonRT(msg); + } + + controller_interface::CallbackReturn + ArmorTesterController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // set the default value in command + auto msg_ptr = * (vel_buffer_.readFromRT()); + msg_ptr->unitree_vel = NaN; + msg_ptr->dji_vel = NaN; + + // initialize the reference_interfaces_, where write the control reference (tgt) + // in update, the controller implement the control via reference_interfaces_ + reference_interfaces_.assign(reference_interfaces_.size(), NaN); + + return controller_interface::CallbackReturn::SUCCESS; + } + + #if RCLCPP_VERSION_MAJOR >= 28 // Ros2 Jazzy or later + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + #else + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers() { + #endif + // shared_ptr must be allocated immediately to avoid dangling + auto current_vel_msg = * (vel_buffer_.readFromRT()); + + if (!std::isnan(current_vel_msg->unitree_vel) && + !std::isnan(current_vel_msg->dji_vel)) { + // set the velocity into the reference_interfaces_ + reference_interfaces_[0] = current_vel_msg->unitree_vel; + reference_interfaces_[1] = current_vel_msg->dji_vel; + + current_vel_msg->unitree_vel = NaN; + current_vel_msg->dji_vel = NaN; + } + + return controller_interface::return_type::OK; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + // we use C-c to stop the program, + // and stop the device in the deconstructor of the hardware interface + // on_deactivate() here is useless for us. + + return controller_interface::CallbackReturn::SUCCESS; + } + + std::vector + ArmorTesterController::on_export_reference_interfaces() { + reference_interfaces_.resize(2, NaN); + + std::vector ref_itf; + ref_itf.reserve(reference_interfaces_.size()); + + ref_itf.emplace_back(get_node()->get_name(), + params_.unitree_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[0]); + ref_itf.emplace_back(get_node()->get_name(), + params_.dji_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[1]); + return ref_itf; + } + + controller_interface::return_type + ArmorTesterController::update_and_write_commands(const rclcpp::Time & /*time*/, + const rclcpp::Duration & period) { + if (command_interfaces_.size() != 2 || reference_interfaces_.size() != 2) { + RCLCPP_WARN(get_node()->get_logger(), "ref_itf or cmd_itf has wrong size"); + // return controller_interface::return_type::ERROR; + } + + auto ref_unitree_vel = reference_interfaces_[0]; + auto ref_dji_vel = reference_interfaces_[1]; + // write to the command interface for unitree + if (!std::isnan(ref_unitree_vel)) { + command_interfaces_[0].set_value(ref_unitree_vel); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of unitree velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + // write to the command interface for dji with pid + if (!std::isnan(ref_dji_vel)) { + double current_dji_vel = std::numeric_limits::quiet_NaN(); + + for (auto & state_itf : state_interfaces_) { + // write to the command interface for unitree + if (state_itf.get_name() == params_.dji_joint.name + "/" + HW_IF_VELOCITY) { + current_dji_vel = state_itf.get_value(); + break; + } + } + + if (!std::isnan(current_dji_vel)) { + double dji_vel_err = ref_dji_vel - current_dji_vel; + double dji_vel_eff = dji_pid_->computeCommand(dji_vel_err, period); + command_interfaces_[1].set_value(dji_vel_eff); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; + } + +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(armor_tester_controller::ArmorTesterController, + controller_interface::ChainableControllerInterface) diff --git a/decomposition/meta_utils_controller/src/armor_tester_controller.yaml b/decomposition/meta_utils_controller/src/armor_tester_controller.yaml new file mode 100644 index 0000000..0265dd8 --- /dev/null +++ b/decomposition/meta_utils_controller/src/armor_tester_controller.yaml @@ -0,0 +1,15 @@ +armor_tester_controller: + unitree_joint: + name: + { + type: string, + default_value: "unitree_motor", + description: "specify the name of the unitree motor", + } + dji_joint: + name: + { + type: string, + default_value: "dji_motor", + description: "specify the name of the dji motor", + } diff --git a/decomposition/metav_description/urdf/playground/armor_tester.xacro b/decomposition/metav_description/urdf/playground/armor_tester.xacro new file mode 100644 index 0000000..e83b53e --- /dev/null +++ b/decomposition/metav_description/urdf/playground/armor_tester.xacro @@ -0,0 +1,46 @@ + + + + + + + + + meta_hardware/MetaRobotUnitreeMotorInterface + /dev/tty_RS485_1 + + + + + + + + + + + meta_hardware/MetaRobotDjiMotorNetwork + can_1 + + + + + + + M3508 + 3 + 20.0 + 0.0 + + + + + + + + + + + diff --git a/interfaces/behavior_interface/CMakeLists.txt b/interfaces/behavior_interface/CMakeLists.txt index 8f766ce..5dfa48e 100644 --- a/interfaces/behavior_interface/CMakeLists.txt +++ b/interfaces/behavior_interface/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) # find_package( REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Armor.msg" "msg/Aim.msg" "msg/Chassis.msg" "msg/Move.msg" diff --git a/interfaces/behavior_interface/msg/Armor.msg b/interfaces/behavior_interface/msg/Armor.msg new file mode 100644 index 0000000..e3f5830 --- /dev/null +++ b/interfaces/behavior_interface/msg/Armor.msg @@ -0,0 +1,2 @@ +float64 unitree_vel # in radians, for unitree motor +float64 dji_vel # in radians, for dji motor diff --git a/meta_bringup/config/armor_tester.yaml b/meta_bringup/config/armor_tester.yaml new file mode 100644 index 0000000..71ae295 --- /dev/null +++ b/meta_bringup/config/armor_tester.yaml @@ -0,0 +1,21 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + armor_tester_controller: + type: armor_tester_controller/ArmorTesterController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +armor_tester_controller: + ros__parameters: + unitree_joint: + name: "motor1" + dji_joint: + name: "motor2" + gains: + motor2_vel2eff: + { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + +dbus_control_node: + ros__parameters: + dbus_port: "tty_DBUS" diff --git a/meta_bringup/launch/armor_tester.py b/meta_bringup/launch/armor_tester.py new file mode 100644 index 0000000..de098c1 --- /dev/null +++ b/meta_bringup/launch/armor_tester.py @@ -0,0 +1,119 @@ +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +import os +import sys + +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started', + ) +] + +def generate_launch_description(): + # get uddf via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'armor_tester.xacro']) + ]) + + # node for robot state publisher + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'robot_description': robot_description_content} + ], + output='both', + emulate_tty=True + ) + + # ros2 control related launch, to read the yaml configure file + robot_controller_config = PathJoinSubstitution( + [ + FindPackageShare('meta_bringup'), + 'config', + 'armor_tester.yaml' + ] + ) + + # controller manager node + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + ) + + # node for dbus_controll, not a controller + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + output='both', + emulate_tty=True + ) + + # node for armor_tester, and the executable is armor_tester_node + armor_tester_node = Node( + package='armor_tester', + executable='armor_tester_node', + name='armor_tester', + output='both', + parameters=[robot_controller_config], + emulate_tty=True + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + load_controllers = [ + load_controller('armor_tester_controller') + ] + + return LaunchDescription([ + # lauch arguments + *ARGUMENTS, + # load robot state publisher + dbus_control_node, + armor_tester_node, + node_robot_state_publisher, + # launch controller manager(if not in simulation) + controller_manager, + # load joint state broadcaster + load_joint_state_broadcaster, + # load controller + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + ])