diff --git a/rm_dshot_shooter_controllers/CHANGELOG.rst b/rm_dshot_shooter_controllers/CHANGELOG.rst new file mode 100644 index 00000000..ec958138 --- /dev/null +++ b/rm_dshot_shooter_controllers/CHANGELOG.rst @@ -0,0 +1,142 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rm_shooter_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.11 (2023-06-20) +------------------- +* Merge pull request `#129 `_ from ye-luo-xi-tui/master + Shooter_controller would not check block when friction wheel don't rotate +* Shooter_controller would not check block when friction wheel don't rotate. +* Merge branch 'master' into dev/balance +* Merge pull request `#120 `_ from ye-luo-xi-tui/master + 0.1.10 +* Merge branch 'rm-controls:master' into gpio_calibration_controller +* Contributors: 1moule, ye-luo-xi-tui, yezi, yuchen + +0.1.10 (2023-03-25) +------------------- +* Merge pull request `#118 `_ from ye-luo-xi-tui/master + Publish shoot state +* Publish shoot state. +* Merge pull request `#109 `_ from ye-luo-xi-tui/master + Modifier default value of forward_push_threshold and exit_push_threshold +* Modifier default value of forward_push_threshold and exit_push_threshold. +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui, yezi + +0.1.9 (2023-02-21) +------------------ +* Merge branch 'master' into balance_standard +* Merge remote-tracking branch 'origin/fix_return' into fix_return +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Merge branch 'rm-controls:master' into fix_return +* Contributors: L-SY, lsy, ye-luo-xi-tui, yezi + +0.1.8 (2022-11-24) +------------------ +* Merge pull request `#93 `_ from NaHCO3bc/master + Fix the bug that the shooter cannot be turned from push to ready +* Modify the name and the description of the params about push forward threshold. +* Optimize the logic of entering the block mode. +* Fix the bug that shooter cannot push or enter block when the position error is too big. +* Modify the params name. +* Modify the params about enter and exit push mode. +* Parametric position difference of trigger. +* Fix the bug that the shooter cannot be turned from push to ready. +* Merge pull request `#86 `_ from NaHCO3bc/Readme + Fix the dependence part bug. +* Modify the test file folder. +* Fix the dependence part bug. +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: NaHCO3bc, ye-luo-xi-tui + +0.1.7 (2022-09-10) +------------------ +* Try two fix double shoot +* Merge remote-tracking branch 'origin/master' +* Fix bug of shooting if statement +* Contributors: qiayuan + +0.1.6 (2022-06-16) +------------------ +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + +0.1.5 (2022-06-10) +------------------ +* Add hz counting in trigger test mode. +* Merge remote-tracking branch 'origin/master' +* Add testing option to shooter for testing the trigger without friction wheel +* 0.1.4 +* Merge branch 'master' into gimbal_track +* Merge pull request `#45 `_ from mlione/master + Delete some config files in rm_controllers. +* Delete some config files in rm_controller. +* Merge pull request `#46 `_ from ye-luo-xi-tui/master + Deprecated imu_filter_controller +* Contributors: BruceLannn, QiayuanLiao, YuuinIH, mlione, qiayuan, yezi + +0.1.3 (2022-03-28) +------------------ +* Merge branch 'master' into forward_feed +* Merge pull request `#40 `_ from ye-luo-xi-tui/maintain + Delete configuration of robot_state_controller in each of controllers' config file +* Merge branch 'master' into maintain + # Conflicts: + # rm_chassis_controllers/config/standard3.yaml +* Merge pull request `#41 `_ from ye-luo-xi-tui/standard3 + Update standard3 config +* Delete configuration of robot_state_controller in each of controllers' config file +* Merge branch 'master' into standard3 +* Update standard3 config +* Merge remote-tracking branch 'origin/master' +* Contributors: QiayuanLiao, qiayuan, ye-luo-xi-tui, yezi + +0.1.2 (2022-01-08) +------------------ +* Merge branch 'master' of https://github.com/YuuinIH/rm_controllers +* Merge branch 'gimbal/opti_or_simplify' into chassis/balance_imu_interface +* Update the config of rm_shooter_controller, load only one controller on launch instead of spawn controllers +* Merge branch 'master' into chassis/fix_filter +* Merge remote-tracking branch 'origin/master' +* Merge branch 'namespace' + # Conflicts: + # rm_chassis_controllers/README.md +* Merge pull request `#15 `_ from ye-luo-xi-tui/namespace + Change name of namespace:from hardware_interface to rm_control +* Change name of namespace:from hardware_interface to rm_control. +* Merge pull request `#8 `_ from Edwinlinks/master + Update README.md in rm_shooter_controller +* Update the format of Bugs & Feature Requests README.md in rm_shooter_controller +* Fix LICENSE url +* Update the format of Bugs & Feature Requests README.md in rm_shooter_controller +* Update the README.md in rm_shooter_controller +* Use “pragma once” in rm_shooter_controllers headers instead of include guards. +* Correct dependencies and interface type. +* Correct some shooter param's description. +* Simplify shooter README.md. +* Update shooter param's description. +* Correct readme format. +* Correct readme format. +* Update controllers README. +* Merge remote-tracking branch 'origin/master' +* Update README.md +* Delet a space bar. + Delet space bar after sudo rosdep install --from-paths src +* Add some param's description +* Update shooter controller's README.md +* Fix wrong naming "include/rm_shooter_controller" +* Run pre-commit +* Format rm_shooter_controllers using clang-format +* Contributors: BruceLannn, QiayuanLiao, YuuinIH, chenzheng, link Edwin, qiayuan, yezi, 沐 + +0.1.1 (2021-08-12) +------------------ +* Set all version to the same +* Add license to rm_chassis_controllers and rm_gimbal_controllers source files +* Merge remote-tracking branch 'alias_memory/metapackage' +* Move all files to rm_shooter_controllers/rm_shooter_controllers, prepare for merge +* Contributors: qiayuan diff --git a/rm_dshot_shooter_controllers/CMakeLists.txt b/rm_dshot_shooter_controllers/CMakeLists.txt new file mode 100644 index 00000000..c9af943a --- /dev/null +++ b/rm_dshot_shooter_controllers/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.10) +project(rm_dshot_shooter_controllers) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror) + +## Find catkin macros and libraries +find_package(catkin REQUIRED + COMPONENTS + roscpp + rm_common + velocity_controllers + controller_interface + effort_controllers + dynamic_reconfigure +) + +generate_dynamic_reconfigure_options( + cfg/Shooter.cfg +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + rm_common + velocity_controllers + effort_controllers + dynamic_reconfigure + LIBRARIES ${PROJECT_NAME} +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/standard.cpp) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES}) + +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +# Mark other files for installation +install( + DIRECTORY test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install( + FILES rm_dshot_shooter_controllers_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/rm_dshot_shooter_controllers/README.md b/rm_dshot_shooter_controllers/README.md new file mode 100644 index 00000000..8cc4e698 --- /dev/null +++ b/rm_dshot_shooter_controllers/README.md @@ -0,0 +1,112 @@ +# rm_shooter_controller + +## Overview + +The rm_shooter_controller has four states: STOP, READY, PUSH, and BLOCK, it controls the left and right friction wheels and the trigger wheel through PID algorithm according to the command. It can set the bullet speed by setting the angular velocity of the friction wheel, and at the same time realizes the block detection. + +**Keywords:** ROS, robomaster, shooter + +#### License + +The source code is released under a [BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). + +**Author: DynamicX
+Affiliation: DynamicX
+Maintainer: DynamicX** + +The rm_shooter_controller package has been tested under [ROS](http://www.ros.org) Melodic and Noetic on respectively 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. + +### Hardware interface type + ++ `JointStateInterface` Used to obtain the speed of friction wheel and trigger wheel and the position of trigger wheel. ++ `EffortJointInterface` Used to send torque commands for friction wheels and trigger wheel. + +## Installation + +### Installation from Packages + +To install all packages from this repository as Debian packages use + +``` +sudo apt-get install ros-noetic-rm-shooter-controllers +``` + +or better use `rosdep`: + +``` +sudo rosdep install --from-paths src +``` + +### Dependencies + +- roscpp +- rm_common +- effort_controllers +- dynamic_reconfigure + +## Cfg + ++ **shooter.cfg:** Add parameters related to friction wheel's angular velocity corresponding to each bullet speed and trigger block detection parameters. + +## ROS API + +#### Subscribed Topics + +* **`command`** (rm_msgs/ShootCmd) + + Commands of controller state, bullet speed, frequency of shooting. + +#### Parameters + +* **`block_effort`, `block_speed`, `block_duration`** (double) + + When the torque of the trigger motor is greater than `block_effort` (in N·m), and the angular velocity of trigger motor is less than `block_speed` (in rad/s), it will be regarded as blocking if it continues for `block_duration` (in s), and the the state of shooter controller will switch to BLOCK. + +* **`block_overtime`** (double) + + If the time to enter BLOCK state exceeds `block_overtime` (in s), the state of shooter controller will switch to PUSH. + +* **`anti_block_angle`** (double) + + If shooter controller enter BLOCK state, the friction wheel will reverse `anti_block_angle` (in rad) to try to get rid of blocking. When the friction wheel get rid of BLOCK state successfully, the state of shooter controller will switch to PUSH. + +* **`anti_block_threshold`** (double) + + If the anti angle of the friction wheel exceeds `anti_block_threshold` (in rad), it means that friction wheel reverse successfully. + +* **`qd_10`, `qd_15`, `qd_18`, `qd_30`** (double) + + These parameters mean the friction wheel's angular velocity, the number of it's name expresses different bullet speeds (in m/s). + +### Controller configuration examples + +#### Complete description + +``` +shooter_controller: + type: rm_shooter_controllers/Controller + publish_rate: 50 + friction_left: + joint: "left_friction_wheel_joint" + pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } + friction_right: + joint: "right_friction_wheel_joint" + pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } + trigger: + joint: "trigger_joint" + pid: { p: 50.0, i: 0.0, d: 1.5, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + push_per_rotation: 8 + push_qd_threshold: 0.90 + block_effort: 0.95 + block_duration: 0.05 + block_overtime: 0.5 + anti_block_angle: 0.2 + anti_block_threshold: 0.1 + qd_15: 460.0 + qd_18: 515.0 + qd_30: 740.0 +``` + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker](https://github.com/gdut-dynamic-x/simple_chassis_controller/issues) . diff --git a/rm_dshot_shooter_controllers/cfg/Shooter.cfg b/rm_dshot_shooter_controllers/cfg/Shooter.cfg new file mode 100755 index 00000000..d6034974 --- /dev/null +++ b/rm_dshot_shooter_controllers/cfg/Shooter.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +PACKAGE = "rm_dshot_shooter_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("block_effort", double_t, 0, "Trigger block effort", 0.95, 0.0, 10) +gen.add("block_duration", double_t, 0, "Trigger block duration", 0.05, 0.0, 2.0) +gen.add("block_speed", double_t, 0, "Trigger block speed", 0.5, 0.0, 5.) +gen.add("block_overtime", double_t, 0, "Trigger block overtime", 0.5, 0.0, 5.) +gen.add("anti_block_angle", double_t, 0, "Trigger anti block angle", 0.35, 0.0, 1.5) +gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold", 0.05, 0.0, 0.2) +gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) +gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) +gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) +gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) +gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) + +exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_dshot_shooter_controllers/include/rm_dshot_shooter_controllers/standard.h b/rm_dshot_shooter_controllers/include/rm_dshot_shooter_controllers/standard.h new file mode 100644 index 00000000..cef682c6 --- /dev/null +++ b/rm_dshot_shooter_controllers/include/rm_dshot_shooter_controllers/standard.h @@ -0,0 +1,136 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021, Qiayuan Liao + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +// +// Created by huakang on 2021/1/18. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rm_dshot_shooter_controllers +{ +struct Config +{ + double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, + forward_push_threshold, exit_push_threshold; + double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; +}; + +class Controller : public controller_interface::MultiInterfaceController +{ +public: + Controller() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void update(const ros::Time& time, const ros::Duration& period) override; + void starting(const ros::Time& /*time*/) override; + +private: + void stop(const ros::Time& time, const ros::Duration& period); + void ready(const ros::Duration& period); + void push(const ros::Time& time, const ros::Duration& period); + void block(const ros::Time& time, const ros::Duration& period); + void setSpeed(const rm_msgs::ShootCmd& cmd, const ros::Duration& period); + void normalize(); + void judgeBulletShoot(const ros::Time& time, const ros::Duration& period); + void commandCB(const rm_msgs::ShootCmdConstPtr& msg) + { + cmd_rt_buffer_.writeFromNonRT(*msg); + } + + void reconfigCB(rm_dshot_shooter_controllers::ShooterConfig& config, uint32_t /*level*/); + + hardware_interface::EffortJointInterface* effort_joint_interface_{}; + hardware_interface::VelocityJointInterface* velocity_joint_interface_{}; + std::vector> ctrls_friction_; + std::vector>> friction_pid_controllers_; + effort_controllers::JointPositionController ctrl_trigger_; + LowPassFilter* lp_filter_; + int push_per_rotation_{}, count_{}; + double push_wheel_speed_threshold_{}; + double freq_threshold_{}; + bool dynamic_reconfig_initialized_ = false; + bool state_changed_ = false; + bool enter_ready_ = false; + bool maybe_block_ = false; + int friction_block_count = 0; + bool last_friction_wheel_block = false, friction_wheel_block = false; + double friction_block_vel_{}; + double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{}; + bool has_shoot_ = false, has_shoot_last_ = false; + bool wheel_speed_drop_ = true; + double last_fricition_speed_{}; + double last_friction_change_speed_{}; + + ros::Time last_shoot_time_, block_time_, last_block_time_; + enum + { + STOP, + READY, + PUSH, + BLOCK + }; + int state_ = STOP; + Config config_{}; + realtime_tools::RealtimeBuffer config_rt_buffer; + realtime_tools::RealtimeBuffer cmd_rt_buffer_; + rm_msgs::ShootCmd cmd_; + std::shared_ptr> local_heat_state_pub_; + std::shared_ptr> shoot_state_pub_; + std::vector>>> + friction_state_publishers_; + int loop_count_ = 0; + ros::Subscriber cmd_subscriber_; + dynamic_reconfigure::Server* d_srv_{}; +}; + +} // namespace rm_dshot_shooter_controllers diff --git a/rm_dshot_shooter_controllers/package.xml b/rm_dshot_shooter_controllers/package.xml new file mode 100644 index 00000000..2d17bf8e --- /dev/null +++ b/rm_dshot_shooter_controllers/package.xml @@ -0,0 +1,26 @@ + + + rm_dshot_shooter_controllers + 0.1.11 + RoboMaster standard robot Shooter controller + Qiayuan Liao + BSD + Qiayuan Liao + + + catkin + + roscpp + + rm_common + + controller_interface + effort_controllers + velocity_controllers + dynamic_reconfigure + + + + + diff --git a/rm_dshot_shooter_controllers/rm_dshot_shooter_controllers_plugins.xml b/rm_dshot_shooter_controllers/rm_dshot_shooter_controllers_plugins.xml new file mode 100644 index 00000000..21a8efa3 --- /dev/null +++ b/rm_dshot_shooter_controllers/rm_dshot_shooter_controllers_plugins.xml @@ -0,0 +1,12 @@ + + + + The Controller is RoboMaster standard robot + Shooter controller. It expects a EffortJointInterface and VelocityJointInterface + type of hardware interface. + + + + diff --git a/rm_dshot_shooter_controllers/src/standard.cpp b/rm_dshot_shooter_controllers/src/standard.cpp new file mode 100644 index 00000000..03799bbb --- /dev/null +++ b/rm_dshot_shooter_controllers/src/standard.cpp @@ -0,0 +1,410 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021, Qiayuan Liao + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +// +// Created by xie on 2025/11/1. +// + +#include +#include +#include +#include +#include "ros/console.h" +#include "rm_dshot_shooter_controllers/standard.h" + +namespace rm_dshot_shooter_controllers +{ +bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) +{ + config_ = { .block_effort = getParam(controller_nh, "block_effort", 0.), + .block_speed = getParam(controller_nh, "block_speed", 0.), + .block_duration = getParam(controller_nh, "block_duration", 0.), + .block_overtime = getParam(controller_nh, "block_overtime", 0.), + .anti_block_angle = getParam(controller_nh, "anti_block_angle", 0.), + .anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.), + .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), + .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), + .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.), + .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.), + .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) }; + config_rt_buffer.initRT(config_); + push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); + push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.); + freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); + anti_friction_block_duty_cycle_ = getParam(controller_nh, "anti_friction_block_duty_cycle", 0.5); + anti_friction_block_vel_ = getParam(controller_nh, "anti_friction_block_vel", 810.0); + friction_block_vel_ = getParam(controller_nh, "friction_block_vel", 1.0); + + cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); + local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( + controller_nh, "/local_heat_state/shooter_state", 10)); + shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); + // Init dynamic reconfigure + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, + auto&& PH2) { + reconfigCB(std::forward(PH1), std::forward(PH2)); + }; + d_srv_->setCallback(cb); + + XmlRpc::XmlRpcValue friction; + effort_joint_interface_ = robot_hw->get(); + velocity_joint_interface_ = robot_hw->get(); + controller_nh.getParam("friction", friction); + for (const auto& its : friction) + { + std::vector wheel_speed_offset_temp; + std::vector ctrl_frictions; + std::vector> pid_controllers_temp; + std::vector>> state_pubs; + for (const auto& it : its.second) + { + ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction/" + its.first + "/" + it.first); + auto* ctrl_friction = new velocity_controllers::JointVelocityController; + if (ctrl_friction->init(velocity_joint_interface_, nh)) + { + ctrl_frictions.push_back(ctrl_friction); + } + else + { + return false; + } + auto pid = std::make_shared(); + ros::NodeHandle pid_nh(nh, "pid"); + if (!pid->init(pid_nh, false)) + { + ROS_ERROR_STREAM("Failed to initialize PID for " << it.first); + return false; + } + pid_controllers_temp.push_back(pid); + std::unique_ptr> state_pub; + state_pub = + std::make_unique>(nh, "state", 1); + state_pubs.push_back(std::move(state_pub)); + } + friction_state_publishers_.push_back(std::move(state_pubs)); + ctrls_friction_.push_back(ctrl_frictions); + friction_pid_controllers_.push_back(pid_controllers_temp); + } + lp_filter_ = new LowPassFilter(controller_nh); + ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger"); + return ctrl_trigger_.init(effort_joint_interface_, nh_trigger); +} + +void Controller::starting(const ros::Time& /*time*/) +{ + state_ = STOP; + state_changed_ = true; +} + +void Controller::update(const ros::Time& time, const ros::Duration& period) +{ + cmd_ = *cmd_rt_buffer_.readFromRT(); + config_ = *config_rt_buffer.readFromRT(); + if (state_ != cmd_.mode) + { + if (state_ != BLOCK) + if ((state_ != PUSH || cmd_.mode != READY) || + (cmd_.mode == READY && + (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < + config_.exit_push_threshold || + cmd_.hz >= freq_threshold_))) + { + if (state_ == STOP && cmd_.mode == READY) + enter_ready_ = true; + state_ = cmd_.mode; + state_changed_ = true; + } + } + + if (state_ != STOP) + setSpeed(cmd_, period); + switch (state_) + { + case READY: + ready(period); + break; + case PUSH: + push(time, period); + break; + case STOP: + stop(time, period); + break; + case BLOCK: + block(time, period); + break; + } + judgeBulletShoot(time, period); + if (shoot_state_pub_->trylock()) + { + shoot_state_pub_->msg_.stamp = time; + shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->unlockAndPublish(); + } + ctrl_trigger_.update(time, period); +} + +void Controller::stop(const ros::Time& time, const ros::Duration& period) +{ + if (state_changed_) + { // on enter + state_changed_ = false; + ROS_INFO("[Shooter] Enter STOP"); + for (auto& ctrl_frictions : ctrls_friction_) + { + for (auto& ctrl_friction : ctrl_frictions) + { + ctrl_friction->joint_.setCommand(0.); + } + } + ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition()); + } +} + +void Controller::ready(const ros::Duration& period) +{ + if (state_changed_) + { // on enter + state_changed_ = false; + ROS_INFO("[Shooter] Enter READY"); + + normalize(); + } +} + +void Controller::push(const ros::Time& time, const ros::Duration& period) +{ + if (state_changed_) + { // on enter + state_changed_ = false; + ROS_INFO("[Shooter] Enter PUSH"); + } + bool wheel_speed_ready = true; + for (size_t i = 0; i < ctrls_friction_.size(); i++) + { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) + { + if (ctrls_friction_[i][j]->joint_.getVelocity() <= M_PI) + wheel_speed_ready = false; + } + } + if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + { // Time to shoot!!! + if (cmd_.hz >= freq_threshold_) + { + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_), + -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; + } + else if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < + config_.forward_push_threshold) + { + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; + } + // Check block + if ((ctrl_trigger_.joint_.getEffort() < -config_.block_effort && + std::abs(ctrl_trigger_.joint_.getVelocity()) < config_.block_speed) || + ((time - last_shoot_time_).toSec() > 1 / cmd_.hz && + std::abs(ctrl_trigger_.joint_.getVelocity()) < config_.block_speed)) + { + if (!maybe_block_) + { + block_time_ = time; + maybe_block_ = true; + } + if ((time - block_time_).toSec() >= config_.block_duration) + { + state_ = BLOCK; + state_changed_ = true; + ROS_INFO("[Shooter] Exit PUSH"); + } + } + else + maybe_block_ = false; + } + else + ROS_DEBUG("[Shooter] Wait for friction wheel"); +} + +void Controller::block(const ros::Time& time, const ros::Duration& period) +{ + if (state_changed_) + { // on enter + state_changed_ = false; + ROS_INFO("[Shooter] Trigger Enter BLOCK"); + last_block_time_ = time; + ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition() + config_.anti_block_angle); + } + if (std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.joint_.getPosition()) < + config_.anti_block_threshold || + (time - last_block_time_).toSec() > config_.block_overtime) + { + normalize(); + state_ = PUSH; + state_changed_ = true; + ROS_INFO("[Shooter] Trigger Exit BLOCK"); + } +} + +void Controller::setSpeed(const rm_msgs::ShootCmd& cmd, const ros::Duration& period) +{ + for (size_t i = 0; i < ctrls_friction_.size(); i++) + { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) + { + double target_speed = 0.0; + // Used to distinguish the front and rear friction wheels. + if (j == 0) + { + target_speed = cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_back; + } + else if (j == 1) + { + target_speed = cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_front; + } + double current_speed = ctrls_friction_[i][j]->joint_.getVelocity(); + double error = target_speed - current_speed; + double command = friction_pid_controllers_[i][j]->computeCommand(error, period); + double final_command = target_speed + command; + ctrls_friction_[i][j]->joint_.setCommand(final_command); + if (loop_count_ % 10 == 0) + { + if (friction_state_publishers_[i][j] && friction_state_publishers_[i][j]->trylock()) + { + friction_state_publishers_[i][j]->msg_.header.stamp = ros::Time::now(); + friction_state_publishers_[i][j]->msg_.set_point = target_speed; + friction_state_publishers_[i][j]->msg_.process_value = current_speed; + friction_state_publishers_[i][j]->msg_.error = error; + friction_state_publishers_[i][j]->msg_.time_step = period.toSec(); + friction_state_publishers_[i][j]->msg_.command = final_command; + + double p, i_gain, d, i_clamp, dummy; + bool antiwindup; + friction_pid_controllers_[i][j]->getGains(p, i_gain, d, i_clamp, dummy, antiwindup); + friction_state_publishers_[i][j]->msg_.p = p; + friction_state_publishers_[i][j]->msg_.i = i_gain; + friction_state_publishers_[i][j]->msg_.d = d; + friction_state_publishers_[i][j]->msg_.i_clamp = i_clamp; + friction_state_publishers_[i][j]->msg_.antiwindup = static_cast(antiwindup); + + friction_state_publishers_[i][j]->unlockAndPublish(); + } + } + } + } + loop_count_++; +} + +void Controller::normalize() +{ + double push_angle = 2. * M_PI / static_cast(push_per_rotation_); + if (cmd_.hz <= freq_threshold_) + { + ctrl_trigger_.setCommand( + push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); + } + else if (enter_ready_) + { + ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); + enter_ready_ = false; + } + else + ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.01) / push_angle)); +} + +void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) +{ + lp_filter_->input(ctrls_friction_[0][0]->joint_.getVelocity()); + double friction_speed = lp_filter_->output(); + double friction_change_speed = abs(friction_speed) - last_fricition_speed_; + double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_; + if (state_ != STOP) + { + if (friction_change_speed_derivative > 0 && has_shoot_) + has_shoot_ = false; + if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ && + friction_change_speed_derivative < 0) + has_shoot_ = true; + } + last_fricition_speed_ = abs(friction_speed); + last_friction_change_speed_ = friction_change_speed; + + if (local_heat_state_pub_->trylock()) + { + local_heat_state_pub_->msg_.stamp = time; + local_heat_state_pub_->msg_.has_shoot = has_shoot_; + local_heat_state_pub_->msg_.friction_speed = friction_speed; + local_heat_state_pub_->msg_.friction_change_speed = friction_change_speed; + local_heat_state_pub_->msg_.friction_change_speed_derivative = friction_change_speed_derivative; + local_heat_state_pub_->unlockAndPublish(); + } +} +void Controller::reconfigCB(rm_dshot_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) +{ + ROS_INFO("[Shooter] Dynamic params change"); + if (!dynamic_reconfig_initialized_) + { + Config init_config = *config_rt_buffer.readFromNonRT(); // config init use yaml + config.block_effort = init_config.block_effort; + config.block_speed = init_config.block_speed; + config.block_duration = init_config.block_duration; + config.block_overtime = init_config.block_overtime; + config.anti_block_angle = init_config.anti_block_angle; + config.anti_block_threshold = init_config.anti_block_threshold; + config.forward_push_threshold = init_config.forward_push_threshold; + config.exit_push_threshold = init_config.exit_push_threshold; + config.extra_wheel_speed = init_config.extra_wheel_speed; + config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; + config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold; + dynamic_reconfig_initialized_ = true; + } + Config config_non_rt{ .block_effort = config.block_effort, + .block_speed = config.block_speed, + .block_duration = config.block_duration, + .block_overtime = config.block_overtime, + .anti_block_angle = config.anti_block_angle, + .anti_block_threshold = config.anti_block_threshold, + .forward_push_threshold = config.forward_push_threshold, + .exit_push_threshold = config.exit_push_threshold, + .extra_wheel_speed = config.extra_wheel_speed, + .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, + .wheel_speed_raise_threshold = config.wheel_speed_raise_threshold }; + config_rt_buffer.writeFromNonRT(config_non_rt); +} + +} // namespace rm_dshot_shooter_controllers + +PLUGINLIB_EXPORT_CLASS(rm_dshot_shooter_controllers::Controller, controller_interface::ControllerBase) diff --git a/rm_dshot_shooter_controllers/test/load_controllers.launch b/rm_dshot_shooter_controllers/test/load_controllers.launch new file mode 100644 index 00000000..887a2b14 --- /dev/null +++ b/rm_dshot_shooter_controllers/test/load_controllers.launch @@ -0,0 +1,5 @@ + + + + diff --git a/rm_dshot_shooter_controllers/test/shooter_config_template.yaml b/rm_dshot_shooter_controllers/test/shooter_config_template.yaml new file mode 100644 index 00000000..2be2adf7 --- /dev/null +++ b/rm_dshot_shooter_controllers/test/shooter_config_template.yaml @@ -0,0 +1,21 @@ +controllers: + shooter_controller: + type: rm_dshot_shooter_controllers/Controller + publish_rate: 50 + friction_left: + joint: "left_friction_wheel_joint" + pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } + friction_right: + joint: "right_friction_wheel_joint" + pid: { p: 0.001, i: 0.01, d: 0.0, i_clamp_max: 0.01, i_clamp_min: -0.01, antiwindup: true, publish_state: true } + trigger: + joint: "trigger_joint" + pid: { p: 50.0, i: 0.0, d: 1.5, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } + push_per_rotation: 8 + push_wheel_speed_threshold: 0.90 + block_effort: 0.95 + block_speed: 0.1 + block_duration: 0.05 + block_overtime: 0.5 + anti_block_angle: 0.2 + anti_block_threshold: 0.1