Implement rm_dshot_shooter_controllers and update configurations

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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/118>`_ from ye-luo-xi-tui/master
+  Publish shoot state
+* Publish shoot state.
+* Merge pull request `#109 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/86>`_ from NaHCO3bc/Readme
+  Fix the dependence part bug.
+* Modify the test file folder.
+* Fix the dependence part bug.
+* Merge pull request `#85 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/45>`_ from mlione/master
+  Delete some config files in rm_controllers.
+* Delete some config files in rm_controller.
+* Merge pull request `#46 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/ye-luo-xi-tui/rm_controllers/issues/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 <https://github.com/rm-controls/rm_controllers/issues/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 <https://github.com/rm-controls/rm_controllers/issues/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

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 ()

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<br />
+Affiliation: DynamicX<br />
+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) .

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"))