From 32f16c0737adc82189e8d84848cc3e2b45050eb1 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Thu, 28 Jul 2022 21:39:29 +0800 Subject: [PATCH 001/130] Add README.md about robot_state_controller. --- robot_state_controller/README.md | 123 +++++++++++++++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 robot_state_controller/README.md diff --git a/robot_state_controller/README.md b/robot_state_controller/README.md new file mode 100644 index 00000000..65c6c368 --- /dev/null +++ b/robot_state_controller/README.md @@ -0,0 +1,123 @@ +# robot_state_controller + +## Overview + +The robot_state_controller uses the URDF specified by the parameter robot_description and the joint positions from the `hardware_interface::JointStateInterface` to calculate the forward kinematics of the robot and publish the results via tf, and reads and manages the external incoming tf. + +**keywords:** ROS, urdf, transformation, joint + +#### 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 robot_state_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 get the positions of different joints. ++ `RobotStateInterface` Used to obtain and maintain the transformation relationship of each link of the entire robot. + +## Installation + +### Installation from Packages + +To install all packages from this repository as Debian packages use + +```shell +sudo apt-get install ros-noetic-robot-state-controller +``` + +or better use `rosdep`: + +```shell +sudo rosdep install --from-paths src +``` + +### Building from Source + +#### Dependencies + +* roscpp +* roslint +* rm_common +* hardware_interface +* controller_interface +* urdf +* tf2_ros +* tf2_kdl +* kdl_parser +* realtime_tools +* pluginlib + +#### Building + +To build this package with catkin build. Clone the latest version from this repository into your catkin workspace. + +```bash +catkin_workspace/src +git clone https://github.com/rm-controls/rm_controllers.git +rosdep install --from-paths . --ignore-src +catkin build +``` + +## Usage + +Run the controller with mon launch: + +```shell +mon launch robot_state_controller load_controllers.launch +``` + +## Launch files + +- **load_controller.launch:** Load the parameters in config files and load the robot_state_controller. + +## ROS API + +#### Subscribed Topics + +* **`tf`** (tf2_msgs/TFMessage) + + Obtain and manage maintenance real-time transformation information. + +* **`tf_static`**(tf2_msgs/TFMessage) + + Obtain and manage maintenance static transformation information. + +#### Parameters + +* **`publish_rate`** (double) + + Publish frequency of state publisher, default: 50Hz. + +* **`use_tf_static`** (bool) + + Set whether to use the /tf_static latched static transform broadcaster.Default: true. + +* **`ignore_timestamp`** (bool) + + If true, ignore the publish_frequency and the timestamp of joint_states and publish a tf for each of the received joint_states. Default: false. + +* **`buffer_duration`** (double) + + The time to keep a history of transforms. + +* **`robot_description`** (string) + + The urdf xml robot description. + +#### Complete description + +```yaml +robot_state_controller: + type: robot_state_controller/RobotStateController + publish_rate: 100 +``` + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). From c43414b829db3a5da39e3737c044ec4e89c8d28b Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 30 Jul 2022 17:41:13 +0800 Subject: [PATCH 002/130] Add test file floder to replace the launch and config file floder. --- gpio_controller/CMakeLists.txt | 2 +- gpio_controller/{launch => test}/gpio_controller.launch | 0 gpio_controller/{config => test}/gpio_controller.yaml | 0 mimic_joint_controller/CMakeLists.txt | 4 ---- mimic_joint_controller/config/mimic_config_template.yaml | 6 ------ rm_chassis_controllers/CMakeLists.txt | 2 +- .../{launch => test}/load_controllers.launch | 0 rm_chassis_controllers/{config => test}/localization.yaml | 0 .../{config => test}/template_mecanum.yaml | 0 .../{config => test}/template_omni.yaml | 0 .../{config => test}/template_reaction_wheel.yaml | 0 .../{config => test}/template_swerve.yaml | 0 rm_gimbal_controllers/CMakeLists.txt | 2 +- rm_gimbal_controllers/{config => test}/engineer.yaml | 0 .../{config => test}/gimbal_config_template.yaml | 0 rm_gimbal_controllers/{config => test}/hero.yaml | 0 .../{launch => test}/load_controllers.launch | 0 rm_gimbal_controllers/{config => test}/sentry.yaml | 0 rm_gimbal_controllers/{config => test}/standard3.yaml | 0 rm_gimbal_controllers/{config => test}/standard4.yaml | 0 rm_gimbal_controllers/{config => test}/standard5.yaml | 0 rm_orientation_controller/CMakeLists.txt | 4 ++-- .../{launch => test}/load_controllers.launch | 0 .../{config => test}/orientation_config_template.yaml | 0 rm_shooter_controllers/CMakeLists.txt | 2 +- .../{launch => test}/load_controllers.launch | 0 .../{config => test}/shooter_config_template.yaml | 0 robot_state_controller/CMakeLists.txt | 8 ++++---- robot_state_controller/{config => test}/config.yaml | 0 .../{launch => test}/load_controllers.launch | 0 tof_radar_controller/CMakeLists.txt | 2 +- .../{launch => test}/load_controller.launch | 0 .../{config => test}/tof_config_template.yaml | 0 33 files changed, 11 insertions(+), 21 deletions(-) rename gpio_controller/{launch => test}/gpio_controller.launch (100%) rename gpio_controller/{config => test}/gpio_controller.yaml (100%) delete mode 100644 mimic_joint_controller/config/mimic_config_template.yaml rename rm_chassis_controllers/{launch => test}/load_controllers.launch (100%) rename rm_chassis_controllers/{config => test}/localization.yaml (100%) rename rm_chassis_controllers/{config => test}/template_mecanum.yaml (100%) rename rm_chassis_controllers/{config => test}/template_omni.yaml (100%) rename rm_chassis_controllers/{config => test}/template_reaction_wheel.yaml (100%) rename rm_chassis_controllers/{config => test}/template_swerve.yaml (100%) rename rm_gimbal_controllers/{config => test}/engineer.yaml (100%) rename rm_gimbal_controllers/{config => test}/gimbal_config_template.yaml (100%) rename rm_gimbal_controllers/{config => test}/hero.yaml (100%) rename rm_gimbal_controllers/{launch => test}/load_controllers.launch (100%) rename rm_gimbal_controllers/{config => test}/sentry.yaml (100%) rename rm_gimbal_controllers/{config => test}/standard3.yaml (100%) rename rm_gimbal_controllers/{config => test}/standard4.yaml (100%) rename rm_gimbal_controllers/{config => test}/standard5.yaml (100%) rename rm_orientation_controller/{launch => test}/load_controllers.launch (100%) rename rm_orientation_controller/{config => test}/orientation_config_template.yaml (100%) rename rm_shooter_controllers/{launch => test}/load_controllers.launch (100%) rename rm_shooter_controllers/{config => test}/shooter_config_template.yaml (100%) rename robot_state_controller/{config => test}/config.yaml (100%) rename robot_state_controller/{launch => test}/load_controllers.launch (100%) rename tof_radar_controller/{launch => test}/load_controller.launch (100%) rename tof_radar_controller/{config => test}/tof_config_template.yaml (100%) diff --git a/gpio_controller/CMakeLists.txt b/gpio_controller/CMakeLists.txt index 306b37ff..66a76e2a 100644 --- a/gpio_controller/CMakeLists.txt +++ b/gpio_controller/CMakeLists.txt @@ -61,7 +61,7 @@ install( # Mark other files for installation install( - DIRECTORY config launch + DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( diff --git a/gpio_controller/launch/gpio_controller.launch b/gpio_controller/test/gpio_controller.launch similarity index 100% rename from gpio_controller/launch/gpio_controller.launch rename to gpio_controller/test/gpio_controller.launch diff --git a/gpio_controller/config/gpio_controller.yaml b/gpio_controller/test/gpio_controller.yaml similarity index 100% rename from gpio_controller/config/gpio_controller.yaml rename to gpio_controller/test/gpio_controller.yaml diff --git a/mimic_joint_controller/CMakeLists.txt b/mimic_joint_controller/CMakeLists.txt index f18986a3..b0001b27 100644 --- a/mimic_joint_controller/CMakeLists.txt +++ b/mimic_joint_controller/CMakeLists.txt @@ -51,10 +51,6 @@ install( ) # Mark other files for installation -install( - DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) install( FILES mimic_joint_controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/mimic_joint_controller/config/mimic_config_template.yaml b/mimic_joint_controller/config/mimic_config_template.yaml deleted file mode 100644 index 103a38fb..00000000 --- a/mimic_joint_controller/config/mimic_config_template.yaml +++ /dev/null @@ -1,6 +0,0 @@ -controllers: - mimic_joint1_controller: - type: mimic_joint_controller/MimicJointController - target_joint: "joint1" - joint: "mimic_joint1" - pid: { p: 8, i: 0, d: 0.4, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index f1b6d9cf..bf114853 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -115,7 +115,7 @@ install( # Mark other files for installation install( - DIRECTORY config launch + DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( diff --git a/rm_chassis_controllers/launch/load_controllers.launch b/rm_chassis_controllers/test/load_controllers.launch similarity index 100% rename from rm_chassis_controllers/launch/load_controllers.launch rename to rm_chassis_controllers/test/load_controllers.launch diff --git a/rm_chassis_controllers/config/localization.yaml b/rm_chassis_controllers/test/localization.yaml similarity index 100% rename from rm_chassis_controllers/config/localization.yaml rename to rm_chassis_controllers/test/localization.yaml diff --git a/rm_chassis_controllers/config/template_mecanum.yaml b/rm_chassis_controllers/test/template_mecanum.yaml similarity index 100% rename from rm_chassis_controllers/config/template_mecanum.yaml rename to rm_chassis_controllers/test/template_mecanum.yaml diff --git a/rm_chassis_controllers/config/template_omni.yaml b/rm_chassis_controllers/test/template_omni.yaml similarity index 100% rename from rm_chassis_controllers/config/template_omni.yaml rename to rm_chassis_controllers/test/template_omni.yaml diff --git a/rm_chassis_controllers/config/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml similarity index 100% rename from rm_chassis_controllers/config/template_reaction_wheel.yaml rename to rm_chassis_controllers/test/template_reaction_wheel.yaml diff --git a/rm_chassis_controllers/config/template_swerve.yaml b/rm_chassis_controllers/test/template_swerve.yaml similarity index 100% rename from rm_chassis_controllers/config/template_swerve.yaml rename to rm_chassis_controllers/test/template_swerve.yaml diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 8f6d3db3..ab90ee1e 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -113,7 +113,7 @@ install( # Mark other files for installation install( - DIRECTORY config launch + DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( diff --git a/rm_gimbal_controllers/config/engineer.yaml b/rm_gimbal_controllers/test/engineer.yaml similarity index 100% rename from rm_gimbal_controllers/config/engineer.yaml rename to rm_gimbal_controllers/test/engineer.yaml diff --git a/rm_gimbal_controllers/config/gimbal_config_template.yaml b/rm_gimbal_controllers/test/gimbal_config_template.yaml similarity index 100% rename from rm_gimbal_controllers/config/gimbal_config_template.yaml rename to rm_gimbal_controllers/test/gimbal_config_template.yaml diff --git a/rm_gimbal_controllers/config/hero.yaml b/rm_gimbal_controllers/test/hero.yaml similarity index 100% rename from rm_gimbal_controllers/config/hero.yaml rename to rm_gimbal_controllers/test/hero.yaml diff --git a/rm_gimbal_controllers/launch/load_controllers.launch b/rm_gimbal_controllers/test/load_controllers.launch similarity index 100% rename from rm_gimbal_controllers/launch/load_controllers.launch rename to rm_gimbal_controllers/test/load_controllers.launch diff --git a/rm_gimbal_controllers/config/sentry.yaml b/rm_gimbal_controllers/test/sentry.yaml similarity index 100% rename from rm_gimbal_controllers/config/sentry.yaml rename to rm_gimbal_controllers/test/sentry.yaml diff --git a/rm_gimbal_controllers/config/standard3.yaml b/rm_gimbal_controllers/test/standard3.yaml similarity index 100% rename from rm_gimbal_controllers/config/standard3.yaml rename to rm_gimbal_controllers/test/standard3.yaml diff --git a/rm_gimbal_controllers/config/standard4.yaml b/rm_gimbal_controllers/test/standard4.yaml similarity index 100% rename from rm_gimbal_controllers/config/standard4.yaml rename to rm_gimbal_controllers/test/standard4.yaml diff --git a/rm_gimbal_controllers/config/standard5.yaml b/rm_gimbal_controllers/test/standard5.yaml similarity index 100% rename from rm_gimbal_controllers/config/standard5.yaml rename to rm_gimbal_controllers/test/standard5.yaml diff --git a/rm_orientation_controller/CMakeLists.txt b/rm_orientation_controller/CMakeLists.txt index 56ab9c68..27cdca14 100644 --- a/rm_orientation_controller/CMakeLists.txt +++ b/rm_orientation_controller/CMakeLists.txt @@ -94,8 +94,8 @@ install( # Mark other files for installation install( - DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DIRECTORY test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( FILES rm_orientation_controller_plugin.xml diff --git a/rm_orientation_controller/launch/load_controllers.launch b/rm_orientation_controller/test/load_controllers.launch similarity index 100% rename from rm_orientation_controller/launch/load_controllers.launch rename to rm_orientation_controller/test/load_controllers.launch diff --git a/rm_orientation_controller/config/orientation_config_template.yaml b/rm_orientation_controller/test/orientation_config_template.yaml similarity index 100% rename from rm_orientation_controller/config/orientation_config_template.yaml rename to rm_orientation_controller/test/orientation_config_template.yaml diff --git a/rm_shooter_controllers/CMakeLists.txt b/rm_shooter_controllers/CMakeLists.txt index 3d1d09a4..aaeca6cc 100644 --- a/rm_shooter_controllers/CMakeLists.txt +++ b/rm_shooter_controllers/CMakeLists.txt @@ -105,7 +105,7 @@ install( # Mark other files for installation install( - DIRECTORY config launch + DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( diff --git a/rm_shooter_controllers/launch/load_controllers.launch b/rm_shooter_controllers/test/load_controllers.launch similarity index 100% rename from rm_shooter_controllers/launch/load_controllers.launch rename to rm_shooter_controllers/test/load_controllers.launch diff --git a/rm_shooter_controllers/config/shooter_config_template.yaml b/rm_shooter_controllers/test/shooter_config_template.yaml similarity index 100% rename from rm_shooter_controllers/config/shooter_config_template.yaml rename to rm_shooter_controllers/test/shooter_config_template.yaml diff --git a/robot_state_controller/CMakeLists.txt b/robot_state_controller/CMakeLists.txt index 296bf1c5..f5172ffd 100644 --- a/robot_state_controller/CMakeLists.txt +++ b/robot_state_controller/CMakeLists.txt @@ -99,10 +99,10 @@ install( ) ## Mark other files for installation -#install( -# DIRECTORY doc -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) +install( + DIRECTORY test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) ############# ## Testing ## diff --git a/robot_state_controller/config/config.yaml b/robot_state_controller/test/config.yaml similarity index 100% rename from robot_state_controller/config/config.yaml rename to robot_state_controller/test/config.yaml diff --git a/robot_state_controller/launch/load_controllers.launch b/robot_state_controller/test/load_controllers.launch similarity index 100% rename from robot_state_controller/launch/load_controllers.launch rename to robot_state_controller/test/load_controllers.launch diff --git a/tof_radar_controller/CMakeLists.txt b/tof_radar_controller/CMakeLists.txt index 6232eed1..ffac18d3 100644 --- a/tof_radar_controller/CMakeLists.txt +++ b/tof_radar_controller/CMakeLists.txt @@ -61,7 +61,7 @@ install( # Mark other files for installation install( - DIRECTORY config launch + DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( diff --git a/tof_radar_controller/launch/load_controller.launch b/tof_radar_controller/test/load_controller.launch similarity index 100% rename from tof_radar_controller/launch/load_controller.launch rename to tof_radar_controller/test/load_controller.launch diff --git a/tof_radar_controller/config/tof_config_template.yaml b/tof_radar_controller/test/tof_config_template.yaml similarity index 100% rename from tof_radar_controller/config/tof_config_template.yaml rename to tof_radar_controller/test/tof_config_template.yaml From 4607c7162f253422f5c9a15e91037c0f830efda0 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 30 Jul 2022 17:41:20 +0800 Subject: [PATCH 003/130] Delete the Buliding,Usage,Launch files part in all README.md and add Complete description part in some README.md. --- rm_calibration_controllers/README.md | 23 ++++++------- rm_chassis_controllers/README.md | 27 +--------------- rm_gimbal_controllers/README.md | 24 +------------- rm_orientation_controller/README.md | 48 +++++++--------------------- rm_shooter_controllers/README.md | 27 +--------------- robot_state_controller/README.md | 27 +--------------- 6 files changed, 26 insertions(+), 150 deletions(-) diff --git a/rm_calibration_controllers/README.md b/rm_calibration_controllers/README.md index a252ca2d..b26b3f4b 100644 --- a/rm_calibration_controllers/README.md +++ b/rm_calibration_controllers/README.md @@ -34,9 +34,8 @@ Or better, use `rosdep`: sudo rosdep install --from-paths src -### Building from Source +### Dependencies -#### Dependencies * roscpp * roslint * rm_msgs @@ -49,17 +48,6 @@ Or better, use `rosdep`: * control_msgs -#### Building - -To build from source, clone the latest version from this repository into your catkin workspace and compile the package using - - cd catkin_workspace/src - git clone https://github.com/rm-controls/rm_controllers.git - cd ../ - rosdep install --from-paths . --ignore-src - catkin_make - - ## ROS API #### Service @@ -80,6 +68,15 @@ To build from source, clone the latest version from this repository into your ca This is velocity `threshold`. When the real time velocity of target joint lower than threshold, and last for a while, it can switch CALIBRATED from MOVING. +#### Complete description + +```yaml +cover_controller: + type: effort_controllers/JointPositionController + joint: "cover_joint" + pid: { p: 15.0, i: 0.0, d: 1.2, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +``` + ## Bugs & Feature Requests diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index 73f9d74b..c4accf37 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -31,9 +31,7 @@ Or better, use `rosdep`: sudo rosdep install --from-paths src ``` -### Building from Source - -#### Dependencies +### Dependencies * [Robot Operating System (ROS)](http://wiki.ros.org/) (middleware for robotics), * roscpp @@ -53,29 +51,6 @@ sudo rosdep install --from-paths src * imu_sensor_controller * robot_localization -#### Building - -* Build this package with catkin build. Clone the latest version from this repository into your catkin workspace. - -``` -catkin_workspace/src -git clone https://github.com/rm-controls/rm_controllers.git -rosdep install --from-paths . --ignore-src -catkin build -``` - -## Usage - -Run the controller with mon launch: - -``` -mon launch rm_chassis_controllers load_controllers.launch -``` - -## Launch files - -* **load_controllers.launch:** It loads tf, robot_localization and some controllers, robot_state_controller, joint_state_controller and chassis controller are included. - ## ROS API #### Subscribed Topics diff --git a/rm_gimbal_controllers/README.md b/rm_gimbal_controllers/README.md index dfa34a68..080e390e 100644 --- a/rm_gimbal_controllers/README.md +++ b/rm_gimbal_controllers/README.md @@ -33,8 +33,7 @@ Or better, use `rosdep`: sudo rosdep install --from-paths src -### Building from Source -#### Dependencies +### Dependencies * roscpp * roslint * rm_msgs @@ -52,27 +51,6 @@ Or better, use `rosdep`: * visualization_msgs * dynamic_reconfigure -#### Building - -To build from source, clone the latest version from this repository into your catkin workspace and compile the package using - - cd catkin_workspace/src - git clone https://github.com/rm-controls/rm_controllers.git - cd ../ - rosdep install --from-paths . --ignore-src - catkin build - - -## Usage - -Run the controller with mon launch: - - mon launch rm_gimbal_controller load_controllers.launch - -## Launch files - -* **load_controllers.launch**: Load the parameters in config files and load tf and robot_state_controller, joint_state_controller, gimbal_controller. - ## ROS API #### Subscribed Topics diff --git a/rm_orientation_controller/README.md b/rm_orientation_controller/README.md index f3daa954..191f9a71 100644 --- a/rm_orientation_controller/README.md +++ b/rm_orientation_controller/README.md @@ -58,26 +58,13 @@ Or better, use `rosdep`: sudo rosdep install --from-paths src -### Building from Source - -#### Dependencies +### Dependencies - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), - [Eigen] (linear algebra library) sudo rosdep install --from-paths src -#### Building - -To build from source, clone the latest version from this repository into your catkin workspace and compile the package -using - - cd catkin_workspace/src - git clone https://github.com/ethz-asl/ros_best_practices.git - cd ../ - rosdep install --from-paths . --ignore-src - catkin_make - ### Running in Docker Docker is a great way to run an application with all dependencies and libraries bundles together. Make sure @@ -113,14 +100,6 @@ Run the static code analysis with catkin_make roslint_ros_package_template -## Usage - -Describe the quickest way to run this software, for example: - -Run the main node with - - roslaunch ros_package_template ros_package_template.launch - ## Config files Config file folder/set 1 @@ -131,20 +110,6 @@ Config file folder/set 2 * **...** -## Launch files - -* **launch_file_1.launch:** shortly explain what is launched (e.g standard simulation, simulation with gdb,...) - - Argument set 1 - - - **`argument_1`** Short description (e.g. as commented in launch file). Default: `default_value`. - - Argument set 2 - - - **`...`** - -* **...** - ## Nodes ### ros_package_template @@ -183,6 +148,17 @@ Reads temperature measurements and computed the average. ... +#### Complete description + +```yaml +orientation_controller: + type: rm_orientation_controller/Controller + publish_rate: 100 + name: "gimbal_imu" + frame_source: "odom" + frame_target: "base_link" +``` + ## Bugs & Feature Requests Please report bugs and request features using the [Issue Tracker](https://github.com/gdut-dynamic-x/rm_template/issues) diff --git a/rm_shooter_controllers/README.md b/rm_shooter_controllers/README.md index cde2a3e5..03d4dd49 100644 --- a/rm_shooter_controllers/README.md +++ b/rm_shooter_controllers/README.md @@ -37,9 +37,7 @@ or better use `rosdep`: sudo rosdep install --from-paths src ``` -### Building from Source - -#### Dependencies +### Dependencies - rm_description - roscpp @@ -55,33 +53,10 @@ sudo rosdep install --from-paths src - effort_controllers - dynamic_reconfigure -#### Building - -To build this package with catkin build. Clone the latest version from this repository into your catkin workspace. - -``` -catkin_workspace/src -git clone https://github.com/rm-controls/rm_controllers.git -rosdep install --from-paths . --ignore-src -catkin build -``` - -## Usage - -Run the controller with mon launch: - -``` -mon launch rm_shooter_controllers load_controllers.launch -``` - ## Cfg + **shooter.cfg:** Add parameters related to friction wheel's angular velocity corresponding to each bullet speed and trigger block detection parameters. -## Launch files - -- **load_controller.launch:** Load the parameters in config files and load the shooter controller. - ## ROS API #### Subscribed Topics diff --git a/robot_state_controller/README.md b/robot_state_controller/README.md index 65c6c368..adb843cd 100644 --- a/robot_state_controller/README.md +++ b/robot_state_controller/README.md @@ -37,9 +37,7 @@ or better use `rosdep`: sudo rosdep install --from-paths src ``` -### Building from Source - -#### Dependencies +### Dependencies * roscpp * roslint @@ -53,29 +51,6 @@ sudo rosdep install --from-paths src * realtime_tools * pluginlib -#### Building - -To build this package with catkin build. Clone the latest version from this repository into your catkin workspace. - -```bash -catkin_workspace/src -git clone https://github.com/rm-controls/rm_controllers.git -rosdep install --from-paths . --ignore-src -catkin build -``` - -## Usage - -Run the controller with mon launch: - -```shell -mon launch robot_state_controller load_controllers.launch -``` - -## Launch files - -- **load_controller.launch:** Load the parameters in config files and load the robot_state_controller. - ## ROS API #### Subscribed Topics From 1353f358d9331aa775607658ead9b4d27402f1ad Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 3 Sep 2022 20:59:04 +0800 Subject: [PATCH 004/130] Simplify dependencies to only one level of dependencies. --- gpio_controller/CMakeLists.txt | 10 -------- gpio_controller/package.xml | 3 --- mimic_joint_controller/CMakeLists.txt | 7 ------ mimic_joint_controller/package.xml | 2 -- rm_calibration_controllers/CMakeLists.txt | 20 ---------------- rm_calibration_controllers/README.md | 8 ------- rm_calibration_controllers/package.xml | 7 ------ rm_chassis_controllers/CMakeLists.txt | 28 ----------------------- rm_chassis_controllers/README.md | 11 --------- rm_chassis_controllers/package.xml | 12 ---------- rm_gimbal_controllers/CMakeLists.txt | 25 -------------------- rm_gimbal_controllers/README.md | 11 +-------- rm_gimbal_controllers/package.xml | 9 -------- rm_orientation_controller/CMakeLists.txt | 17 -------------- rm_orientation_controller/package.xml | 6 ----- rm_shooter_controllers/CMakeLists.txt | 22 ------------------ rm_shooter_controllers/README.md | 9 -------- rm_shooter_controllers/package.xml | 8 ------- robot_state_controller/CMakeLists.txt | 16 ------------- robot_state_controller/README.md | 5 ---- robot_state_controller/package.xml | 5 ---- tof_radar_controller/CMakeLists.txt | 9 -------- tof_radar_controller/package.xml | 3 --- 23 files changed, 1 insertion(+), 252 deletions(-) diff --git a/gpio_controller/CMakeLists.txt b/gpio_controller/CMakeLists.txt index 66a76e2a..0ff11e49 100644 --- a/gpio_controller/CMakeLists.txt +++ b/gpio_controller/CMakeLists.txt @@ -3,14 +3,10 @@ project(gpio_controller) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface controller_interface realtime_tools ) @@ -20,13 +16,10 @@ catkin_package( include CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common pluginlib - hardware_interface controller_interface realtime_tools LIBRARIES ${PROJECT_NAME} @@ -68,6 +61,3 @@ install( FILES gpio_controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - - -roslint_cpp() diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index d7344e0f..ca0b58b8 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -9,13 +9,10 @@ catkin - rm_msgs rm_common pluginlib roscpp - roslint controller_interface - hardware_interface realtime_tools diff --git a/mimic_joint_controller/CMakeLists.txt b/mimic_joint_controller/CMakeLists.txt index b0001b27..8ad3faf0 100644 --- a/mimic_joint_controller/CMakeLists.txt +++ b/mimic_joint_controller/CMakeLists.txt @@ -3,10 +3,8 @@ project(mimic_joint_controller) find_package(catkin REQUIRED COMPONENTS roscpp - roslint pluginlib - hardware_interface controller_interface ) @@ -15,10 +13,8 @@ catkin_package( include CATKIN_DEPENDS roscpp - roslint pluginlib - hardware_interface controller_interface LIBRARIES ${PROJECT_NAME} ) @@ -55,6 +51,3 @@ install( FILES mimic_joint_controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - - -roslint_cpp() diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 7edfd002..3d82dfa3 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -11,9 +11,7 @@ pluginlib roscpp - roslint controller_interface - hardware_interface diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 3aa5fb07..9321cf69 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -13,16 +13,9 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - realtime_tools effort_controllers - control_msgs ) ################################### @@ -40,16 +33,9 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - realtime_tools effort_controllers - control_msgs LIBRARIES ${PROJECT_NAME} ) @@ -105,9 +91,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/rm_calibration_controllers/README.md b/rm_calibration_controllers/README.md index b26b3f4b..12b8a16c 100644 --- a/rm_calibration_controllers/README.md +++ b/rm_calibration_controllers/README.md @@ -37,16 +37,8 @@ Or better, use `rosdep`: ### Dependencies * roscpp -* roslint -* rm_msgs * rm_common -* pluginlib -* hardware_interface -* controller_interface -* realtime_tools * effort_controllers -* control_msgs - ## ROS API diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 673daba8..94d82ad2 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -11,17 +11,10 @@ catkin roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - realtime_tools effort_controllers - control_msgs diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index bf114853..6688d173 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -13,22 +13,11 @@ add_definitions(-Wall -Werror -Wno-enum-compare) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common - nav_msgs - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs - angles ) find_package(Eigen3 REQUIRED) @@ -49,22 +38,11 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common - nav_msgs - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs - angles LIBRARIES ${PROJECT_NAME} ) @@ -135,9 +113,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index c4accf37..dd9b910d 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -35,20 +35,9 @@ sudo rosdep install --from-paths src * [Robot Operating System (ROS)](http://wiki.ros.org/) (middleware for robotics), * roscpp -* roslint -* rm_msgs * rm_common -* pluginlib -* hardware_interface -* controller_interface -* forward_command_controller -* realtime_tools -* control_toolbox * effort_controllers -* tf2 * tf2_geometry_msgs -* angles -* imu_sensor_controller * robot_localization ## ROS API diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 90726c87..b01721d9 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -13,24 +13,12 @@ catkin roscpp - roslint - rm_msgs rm_common - nav_msgs - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs - angles - imu_sensor_controller robot_localization diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index ab90ee1e..dd4dca24 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,19 +14,10 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_eigen tf2_geometry_msgs visualization_msgs @@ -53,24 +44,14 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_eigen tf2_geometry_msgs visualization_msgs dynamic_reconfigure - angles LIBRARIES ${PROJECT_NAME} ) @@ -133,9 +114,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/rm_gimbal_controllers/README.md b/rm_gimbal_controllers/README.md index 080e390e..71378730 100644 --- a/rm_gimbal_controllers/README.md +++ b/rm_gimbal_controllers/README.md @@ -35,19 +35,10 @@ Or better, use `rosdep`: ### Dependencies * roscpp -* roslint -* rm_msgs * rm_common -* pluginlib -* hardware_interface -* controller_interface -* forward_command_controller -* realtime_tools -* control_toolbox * effort_controllers -* tf2 +* tf2_eigen * tf2_geometry_msgs -* angles * visualization_msgs * dynamic_reconfigure diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index d1d96335..7565b649 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -11,19 +11,10 @@ catkin roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers - tf2 tf2_eigen tf2_geometry_msgs visualization_msgs diff --git a/rm_orientation_controller/CMakeLists.txt b/rm_orientation_controller/CMakeLists.txt index 27cdca14..ad72be07 100644 --- a/rm_orientation_controller/CMakeLists.txt +++ b/rm_orientation_controller/CMakeLists.txt @@ -13,18 +13,12 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - roslint sensor_msgs - rm_msgs rm_common pluginlib - hardware_interface - controller_interface forward_command_controller - realtime_tools - tf2 tf2_geometry_msgs angles ) @@ -39,17 +33,12 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - roslint sensor_msgs - rm_msgs rm_common pluginlib - hardware_interface - controller_interface forward_command_controller - tf2 tf2_geometry_msgs angles LIBRARIES ${PROJECT_NAME} @@ -114,9 +103,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index e265d06c..d4902dc1 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -11,18 +11,12 @@ catkin roscpp - roslint sensor_msgs - rm_msgs rm_common pluginlib - hardware_interface - controller_interface forward_command_controller - realtime_tools - tf2 tf2_geometry_msgs angles diff --git a/rm_shooter_controllers/CMakeLists.txt b/rm_shooter_controllers/CMakeLists.txt index aaeca6cc..726f9d27 100644 --- a/rm_shooter_controllers/CMakeLists.txt +++ b/rm_shooter_controllers/CMakeLists.txt @@ -13,17 +13,9 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers dynamic_reconfigure ) @@ -47,17 +39,9 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers dynamic_reconfigure LIBRARIES ${PROJECT_NAME} @@ -125,9 +109,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/rm_shooter_controllers/README.md b/rm_shooter_controllers/README.md index 03d4dd49..8cc4e698 100644 --- a/rm_shooter_controllers/README.md +++ b/rm_shooter_controllers/README.md @@ -39,17 +39,8 @@ sudo rosdep install --from-paths src ### Dependencies -- rm_description - roscpp -- roslint -- rm_msgs - rm_common -- pluginlib -- controller_interface -- hardware_interface -- forward_command_controller -- realtime_tools -- control_toolbox - effort_controllers - dynamic_reconfigure diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index 3a323c0b..c1c06993 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -11,17 +11,9 @@ catkin roscpp - roslint - rm_msgs rm_common - pluginlib - hardware_interface - controller_interface - forward_command_controller - realtime_tools - control_toolbox effort_controllers dynamic_reconfigure diff --git a/robot_state_controller/CMakeLists.txt b/robot_state_controller/CMakeLists.txt index f5172ffd..e9f3a99a 100644 --- a/robot_state_controller/CMakeLists.txt +++ b/robot_state_controller/CMakeLists.txt @@ -13,17 +13,12 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - roslint rm_common - hardware_interface controller_interface - urdf - tf2_ros tf2_kdl kdl_parser realtime_tools - pluginlib ) ################################### @@ -42,16 +37,11 @@ catkin_package( ${PROJECT_NAME} CATKIN_DEPENDS roscpp - roslint rm_common - hardware_interface controller_interface - urdf - tf2_ros kdl_parser realtime_tools - pluginlib LIBRARIES ${PROJECT_NAME} ) @@ -116,9 +106,3 @@ install( # test/AlgorithmTest.cpp) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) #endif () - -########################## -## Static code analysis ## -########################## - -roslint_cpp() diff --git a/robot_state_controller/README.md b/robot_state_controller/README.md index adb843cd..34a4c25c 100644 --- a/robot_state_controller/README.md +++ b/robot_state_controller/README.md @@ -40,16 +40,11 @@ sudo rosdep install --from-paths src ### Dependencies * roscpp -* roslint * rm_common -* hardware_interface * controller_interface -* urdf -* tf2_ros * tf2_kdl * kdl_parser * realtime_tools -* pluginlib ## ROS API diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index 9ffaf888..fb29bb6a 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -11,17 +11,12 @@ catkin roscpp - roslint rm_common controller_interface - hardware_interface - urdf kdl_parser - tf2_ros tf2_kdl realtime_tools - pluginlib diff --git a/tof_radar_controller/CMakeLists.txt b/tof_radar_controller/CMakeLists.txt index ffac18d3..78f1f87d 100644 --- a/tof_radar_controller/CMakeLists.txt +++ b/tof_radar_controller/CMakeLists.txt @@ -3,14 +3,11 @@ project(tof_radar_controller) find_package(catkin REQUIRED COMPONENTS roscpp - roslint - rm_msgs rm_common pluginlib - hardware_interface controller_interface realtime_tools ) @@ -20,13 +17,10 @@ catkin_package( include CATKIN_DEPENDS roscpp - roslint - rm_msgs rm_common pluginlib - hardware_interface controller_interface realtime_tools LIBRARIES ${PROJECT_NAME} @@ -68,6 +62,3 @@ install( FILES tof_radar_controller_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - - -roslint_cpp() diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index 015cb435..3c55533e 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -9,13 +9,10 @@ catkin - rm_msgs rm_common pluginlib roscpp - roslint controller_interface - hardware_interface realtime_tools From b52457d5efcbee4b084e75cdc726a15080e8e289 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Fri, 9 Sep 2022 22:53:47 +0800 Subject: [PATCH 005/130] Change frame "map" to "odom". --- .../rm_gimbal_controllers/bullet_solver.h | 4 +--- .../rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 20 +++++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 4 files changed, 12 insertions(+), 16 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 38e5171d..c3a6dcc1 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -72,19 +72,17 @@ class BulletSolver { return -output_pitch_; } - void bulletModelPub(const geometry_msgs::TransformStamped& map2pitch, const ros::Time& time); + void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; private: - ros::Time last_publish_time_; std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; bool dynamic_reconfig_initialized_{}; - double publish_rate_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 9357a312..70491f6a 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -82,7 +82,7 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; // ROS Interface ros::Time last_publish_time_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index ef4a36b8..1f898017 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -44,8 +44,6 @@ namespace rm_gimbal_controllers { BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) { - publish_rate_ = getParam(controller_nh, "publish_rate", 50); - config_ = { .resistance_coff_qd_10 = getParam(controller_nh, "resistance_coff_qd_10", 0.), .resistance_coff_qd_15 = getParam(controller_nh, "resistance_coff_qd_15", 0.), .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), @@ -57,7 +55,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .timeout = getParam(controller_nh, "timeout", 0.) }; config_rt_buffer_.initRT(config_); - marker_desire_.header.frame_id = "map"; + marker_desire_.header.frame_id = "odom"; marker_desire_.ns = "model"; marker_desire_.action = visualization_msgs::Marker::ADD; marker_desire_.type = visualization_msgs::Marker::POINTS; @@ -139,12 +137,12 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d return true; } -void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& map2pitch, const ros::Time& time) +void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); marker_real_.points.clear(); double roll{}, pitch{}, yaw{}; - quatToRPY(map2pitch.transform.rotation, roll, pitch, yaw); + quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); int point_num = int(target_rho * 20); @@ -156,9 +154,9 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& map2pit double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + map2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + map2pitch.transform.translation.y; - point_desire.z = rt_bullet_z + map2pitch.transform.translation.z; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } for (int i = 0; i <= point_num; i++) @@ -169,9 +167,9 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& map2pit double rt_bullet_z = (bullet_speed_ * std::sin(-pitch) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_real.x = rt_bullet_rho * std::cos(yaw) + map2pitch.transform.translation.x; - point_real.y = rt_bullet_rho * std::sin(yaw) + map2pitch.transform.translation.y; - point_real.z = rt_bullet_z + map2pitch.transform.translation.z; + point_real.x = rt_bullet_rho * std::cos(yaw) + odom2pitch.transform.translation.x; + point_real.y = rt_bullet_rho * std::sin(yaw) + odom2pitch.transform.translation.y; + point_real.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_real_.points.push_back(point_real); } marker_desire_.header.stamp = time; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 631f97a8..2aaca3e0 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -63,7 +63,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = new BulletSolver(nh_bullet_solver); + bullet_solver_ = std::make_shared(nh_bullet_solver); ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); From 2cfa5246fa485a8a169be299bcbf70456966861e Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Fri, 9 Sep 2022 23:46:45 +0800 Subject: [PATCH 006/130] Optimize rm_orientation_controller. --- rm_orientation_controller/src/orientation_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_orientation_controller/src/orientation_controller.cpp b/rm_orientation_controller/src/orientation_controller.cpp index f91bca04..0f85bff8 100644 --- a/rm_orientation_controller/src/orientation_controller.cpp +++ b/rm_orientation_controller/src/orientation_controller.cpp @@ -81,9 +81,9 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) receive_imu_msg_ = true; geometry_msgs::TransformStamped source2target; source2target.header.stamp = msg->header.stamp; - if (getTransform(ros::Time(0), source2target, msg->orientation.x, msg->orientation.y, msg->orientation.z, - msg->orientation.w)) - tf_broadcaster_.sendTransform(source2target); + getTransform(ros::Time(0), source2target, msg->orientation.x, msg->orientation.y, msg->orientation.z, + msg->orientation.w); + tf_broadcaster_.sendTransform(source2target); } } // namespace rm_orientation_controller From 8521892b8f328d89641d058f8135074de0973125 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 10 Sep 2022 00:34:03 +0800 Subject: [PATCH 007/130] Optimize TRACK mode of rm_gimbal_controller. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2aaca3e0..1241a616 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -425,6 +425,8 @@ void Controller::commandCB(const rm_msgs::GimbalCmdConstPtr& msg) void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) { + if (msg->id == 0) + return; track_rt_buffer_.writeFromNonRT(*msg); } From d4162ef5681ef5c700310919d258df99ebcd8002 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 10 Sep 2022 17:26:45 +0800 Subject: [PATCH 008/130] 0.1.7 --- gpio_controller/CHANGELOG.rst | 5 +++++ mimic_joint_controller/CHANGELOG.rst | 5 +++++ rm_calibration_controllers/CHANGELOG.rst | 5 +++++ rm_chassis_controllers/CHANGELOG.rst | 15 +++++++++++++++ rm_controllers/CHANGELOG.rst | 5 +++++ rm_gimbal_controllers/CHANGELOG.rst | 7 +++++++ rm_orientation_controller/CHANGELOG.rst | 6 ++++++ rm_shooter_controllers/CHANGELOG.rst | 7 +++++++ robot_state_controller/CHANGELOG.rst | 5 +++++ tof_radar_controller/CHANGELOG.rst | 5 +++++ 10 files changed, 65 insertions(+) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index aa5f5262..cea7bbd7 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 38727fc1..0317c72f 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 5097cd04..741fc2b6 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 695c5336..6447e90b 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#83 `_ from rm-controls/dev + Merge the new OmniController to master +* Merge pull request `#82 `_ from NaHCO3bc/dev + Fix some bugs in the new OmniController. +* Compute the params and fix some bugs. +* Rename the function forwardKinematics to odometry. +* Merge pull request `#80 `_ from qiayuanliao/master + New and elegant OmniController +* Fix bug in the new OmniController +* Add a new and elegant OmniController +* Merge remote-tracking branch 'origin/master' +* Contributors: NaHCO3bc, QiayuanLiao, qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index c908911d..bb861cd4 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge pull request `#76 `_ from Edwinlinks/tof-radar-controller diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 628c8584..9797f32c 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Optimize TRACK mode of rm_gimbal_controller. +* Change frame "map" to "odom". +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan, yezi + 0.1.6 (2022-06-16) ------------------ * Merge branch 'gimbal/chassis_vel' diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index f7f0efdd..d291ae18 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Optimize rm_orientation_controller. +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan, yezi + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index 4f427222..d2e61760 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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' diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index 6273603d..79add69d 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 850ed80e..9607bf9a 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge remote-tracking branch 'origin/master' +* Contributors: qiayuan + 0.1.6 (2022-06-16) ------------------ * Merge pull request `#76 `_ from Edwinlinks/tof-radar-controller From 545e24ceb13d17416f7c06d5b90b28c1d0186d6d Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 10 Sep 2022 17:27:26 +0800 Subject: [PATCH 009/130] 0.1.7 --- gpio_controller/CHANGELOG.rst | 4 ++-- gpio_controller/package.xml | 2 +- mimic_joint_controller/CHANGELOG.rst | 4 ++-- mimic_joint_controller/package.xml | 2 +- rm_calibration_controllers/CHANGELOG.rst | 4 ++-- rm_calibration_controllers/package.xml | 2 +- rm_chassis_controllers/CHANGELOG.rst | 4 ++-- rm_chassis_controllers/package.xml | 2 +- rm_controllers/CHANGELOG.rst | 4 ++-- rm_controllers/package.xml | 2 +- rm_gimbal_controllers/CHANGELOG.rst | 4 ++-- rm_gimbal_controllers/package.xml | 2 +- rm_orientation_controller/CHANGELOG.rst | 4 ++-- rm_orientation_controller/package.xml | 2 +- rm_shooter_controllers/CHANGELOG.rst | 4 ++-- rm_shooter_controllers/package.xml | 2 +- robot_state_controller/CHANGELOG.rst | 4 ++-- robot_state_controller/package.xml | 2 +- tof_radar_controller/CHANGELOG.rst | 4 ++-- tof_radar_controller/package.xml | 2 +- 20 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index cea7bbd7..4ead8d5d 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index ca0b58b8..0fe29d01 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -1,7 +1,7 @@ gpio_controller - 0.1.6 + 0.1.7 The gpio_controller package muyuexin diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 0317c72f..bd316593 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 3d82dfa3..20993de8 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -1,7 +1,7 @@ mimic_joint_controller - 0.1.6 + 0.1.7 The mimic_joint_controller package ljq diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 741fc2b6..274d62bb 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 94d82ad2..0c64d0b4 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -1,7 +1,7 @@ rm_calibration_controllers - 0.1.6 + 0.1.7 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 6447e90b..1e4fe588 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge pull request `#83 `_ from rm-controls/dev Merge the new OmniController to master * Merge pull request `#82 `_ from NaHCO3bc/dev diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index b01721d9..9ec00f0a 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -1,7 +1,7 @@ rm_chassis_controllers - 0.1.6 + 0.1.7 RoboMaster standard robot Chassis controller Qiayuan Liao BSD diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index bb861cd4..739eb26f 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/rm_controllers/package.xml b/rm_controllers/package.xml index 70240901..afe591b3 100644 --- a/rm_controllers/package.xml +++ b/rm_controllers/package.xml @@ -1,7 +1,7 @@ rm_controllers - 0.1.6 + 0.1.7 Meta package that contains package for RoboMaster. Qiayuan Liao diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 9797f32c..764f99dc 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Optimize TRACK mode of rm_gimbal_controller. * Change frame "map" to "odom". * Merge remote-tracking branch 'origin/master' diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index 7565b649..49ebfaae 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -1,7 +1,7 @@ rm_gimbal_controllers - 0.1.6 + 0.1.7 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index d291ae18..1c5c8b1b 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Optimize rm_orientation_controller. * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan, yezi diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index d4902dc1..b1f0bad8 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -1,7 +1,7 @@ rm_orientation_controller - 0.1.6 + 0.1.7 RoboMaster standard robot orientation controller Qiayuan Liao BSD diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index d2e61760..d6c36a0a 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Try two fix double shoot * Merge remote-tracking branch 'origin/master' * Fix bug of shooting if statement diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index c1c06993..47c32fc7 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -1,7 +1,7 @@ rm_shooter_controllers - 0.1.6 + 0.1.7 RoboMaster standard robot Shooter controller Qiayuan Liao BSD diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index 79add69d..f79e2c65 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index fb29bb6a..0d4fcd8f 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -1,7 +1,7 @@ robot_state_controller - 0.1.6 + 0.1.7 A template for ROS packages. Qiayuan Liao BSD diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 9607bf9a..8b0bebf6 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.7 (2022-09-10) +------------------ * Merge remote-tracking branch 'origin/master' * Contributors: qiayuan diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index 3c55533e..8e366e81 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -1,7 +1,7 @@ tof_radar_controller - 0.1.6 + 0.1.7 The tof radar controller package luotinkai From 8a51b60a2ddbe2ac19806bb5072e7043c80d986c Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 10 Sep 2022 18:09:36 +0800 Subject: [PATCH 010/130] Fix the dependence part bug. --- rm_chassis_controllers/CMakeLists.txt | 1 + rm_chassis_controllers/package.xml | 1 + rm_chassis_controllers/test/load_controllers.launch | 4 ++-- rm_gimbal_controllers/CMakeLists.txt | 1 + rm_gimbal_controllers/package.xml | 1 + rm_gimbal_controllers/test/load_controllers.launch | 2 +- rm_orientation_controller/CMakeLists.txt | 1 + rm_orientation_controller/package.xml | 1 + rm_orientation_controller/test/load_controllers.launch | 2 +- rm_shooter_controllers/CMakeLists.txt | 1 + rm_shooter_controllers/package.xml | 1 + rm_shooter_controllers/test/load_controllers.launch | 2 +- robot_state_controller/test/load_controllers.launch | 2 +- tof_radar_controller/test/load_controller.launch | 2 +- 14 files changed, 15 insertions(+), 7 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index baa73a25..35af37eb 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED rm_common + controller_interface effort_controllers tf2_geometry_msgs ) diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index b01721d9..a14168ce 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -16,6 +16,7 @@ rm_common + controller_interface effort_controllers tf2_geometry_msgs diff --git a/rm_chassis_controllers/test/load_controllers.launch b/rm_chassis_controllers/test/load_controllers.launch index ee517b26..03fa717e 100644 --- a/rm_chassis_controllers/test/load_controllers.launch +++ b/rm_chassis_controllers/test/load_controllers.launch @@ -1,9 +1,9 @@ - + - + false rm_common + controller_interface effort_controllers tf2_eigen tf2_geometry_msgs diff --git a/rm_gimbal_controllers/test/load_controllers.launch b/rm_gimbal_controllers/test/load_controllers.launch index 90cbb77d..77e15dde 100644 --- a/rm_gimbal_controllers/test/load_controllers.launch +++ b/rm_gimbal_controllers/test/load_controllers.launch @@ -3,7 +3,7 @@ - + diff --git a/rm_orientation_controller/CMakeLists.txt b/rm_orientation_controller/CMakeLists.txt index ad72be07..d3afddee 100644 --- a/rm_orientation_controller/CMakeLists.txt +++ b/rm_orientation_controller/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED rm_common pluginlib + controller_interface forward_command_controller tf2_geometry_msgs angles diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index d4902dc1..f14022e0 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -16,6 +16,7 @@ rm_common pluginlib + controller_interface forward_command_controller tf2_geometry_msgs angles diff --git a/rm_orientation_controller/test/load_controllers.launch b/rm_orientation_controller/test/load_controllers.launch index f33f3639..b2c1e564 100644 --- a/rm_orientation_controller/test/load_controllers.launch +++ b/rm_orientation_controller/test/load_controllers.launch @@ -1,6 +1,6 @@ - + diff --git a/rm_shooter_controllers/CMakeLists.txt b/rm_shooter_controllers/CMakeLists.txt index 726f9d27..75d5a1e7 100644 --- a/rm_shooter_controllers/CMakeLists.txt +++ b/rm_shooter_controllers/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED rm_common + controller_interface effort_controllers dynamic_reconfigure ) diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index c1c06993..a7c0fca3 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -14,6 +14,7 @@ rm_common + controller_interface effort_controllers dynamic_reconfigure diff --git a/rm_shooter_controllers/test/load_controllers.launch b/rm_shooter_controllers/test/load_controllers.launch index 4df660da..311de5b0 100644 --- a/rm_shooter_controllers/test/load_controllers.launch +++ b/rm_shooter_controllers/test/load_controllers.launch @@ -1,6 +1,6 @@ - + diff --git a/robot_state_controller/test/load_controllers.launch b/robot_state_controller/test/load_controllers.launch index 35ee088f..8e6433d8 100644 --- a/robot_state_controller/test/load_controllers.launch +++ b/robot_state_controller/test/load_controllers.launch @@ -1,5 +1,5 @@ - + diff --git a/tof_radar_controller/test/load_controller.launch b/tof_radar_controller/test/load_controller.launch index 097335ec..c330bc28 100644 --- a/tof_radar_controller/test/load_controller.launch +++ b/tof_radar_controller/test/load_controller.launch @@ -1,6 +1,6 @@ - + From d4a40873de271b5741afc5e2eb0bb3260c56ab40 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 10 Sep 2022 21:54:37 +0800 Subject: [PATCH 011/130] Modify the test file folder. --- rm_gimbal_controllers/test/engineer.yaml | 40 ----------- .../test/gimbal_config_template.yaml | 24 +++---- rm_gimbal_controllers/test/hero.yaml | 35 ---------- .../test/load_controllers.launch | 3 +- rm_gimbal_controllers/test/sentry.yaml | 69 ------------------- rm_gimbal_controllers/test/standard3.yaml | 24 ------- rm_gimbal_controllers/test/standard4.yaml | 24 ------- rm_gimbal_controllers/test/standard5.yaml | 25 ------- .../test/load_controllers.launch | 3 +- .../test/orientation_config_template.yaml | 5 +- .../test/load_controllers.launch | 3 +- .../test/shooter_config_template.yaml | 30 ++------ .../test/load_controller.launch | 3 +- 13 files changed, 20 insertions(+), 268 deletions(-) delete mode 100644 rm_gimbal_controllers/test/engineer.yaml delete mode 100644 rm_gimbal_controllers/test/hero.yaml delete mode 100644 rm_gimbal_controllers/test/sentry.yaml delete mode 100644 rm_gimbal_controllers/test/standard3.yaml delete mode 100644 rm_gimbal_controllers/test/standard4.yaml delete mode 100644 rm_gimbal_controllers/test/standard5.yaml diff --git a/rm_gimbal_controllers/test/engineer.yaml b/rm_gimbal_controllers/test/engineer.yaml deleted file mode 100644 index 00709d10..00000000 --- a/rm_gimbal_controllers/test/engineer.yaml +++ /dev/null @@ -1,40 +0,0 @@ -controllers: - robot_state_controller: - type: robot_state_controller/RobotStateController - publish_rate: 50 - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - gimbal_controller: - type: rm_gimbal_controllers/Controller - time_compensation: 0.03 - publish_rate: 100 - chassis_angular_data_num: 20 - yaw: - joint: "yaw_joint" - pid: { p: 0.4, i: 0.0, d: 0.04, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 0.8, i: 0.0, d: 0.03, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.2 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 20 - vel_data_num: 30 - center_data_num: 100 - gyro_data_num: 100 diff --git a/rm_gimbal_controllers/test/gimbal_config_template.yaml b/rm_gimbal_controllers/test/gimbal_config_template.yaml index 71c767fe..37e9b3ab 100644 --- a/rm_gimbal_controllers/test/gimbal_config_template.yaml +++ b/rm_gimbal_controllers/test/gimbal_config_template.yaml @@ -7,30 +7,26 @@ controllers: publish_rate: 50 gimbal_controller: type: rm_gimbal_controllers/Controller - time_compensation: 0.03 - publish_rate: 100 - chassis_angular_data_num: 20 yaw: joint: "yaw_joint" - pid: { p: 0.4, i: 0.0, d: 0.04, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } + pid: { p: 7.5, i: 0, d: 0.3, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } + k_chassis_vel: 0.01 pitch: joint: "pitch_joint" - pid: { p: 0.8, i: 0.0, d: 0.03, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } + pid: { p: 22.0, i: 0, d: 0.75, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + imu_name: "gimbal_imu" + feedforward: + gravity: 0.0 + enable_gravity_compensation: false + mass_origin: [ 0.0,0.0,0.0 ] bullet_solver: resistance_coff_qd_10: 0.45 resistance_coff_qd_15: 1.0 resistance_coff_qd_16: 0.7 resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 + resistance_coff_qd_30: 5.0 g: 9.81 - delay: 0.2 + delay: 0.15 dt: 0.001 timeout: 0.001 publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 20 - vel_data_num: 30 - center_data_num: 100 - gyro_data_num: 100 diff --git a/rm_gimbal_controllers/test/hero.yaml b/rm_gimbal_controllers/test/hero.yaml deleted file mode 100644 index 2ca4ff7b..00000000 --- a/rm_gimbal_controllers/test/hero.yaml +++ /dev/null @@ -1,35 +0,0 @@ -controllers: - gimbal_controller: - type: rm_gimbal_controllers/Controller - time_compensation: 0.008 - publish_rate: 100 - chassis_angular_data_num: 20 - camera_topic: "/mv_camera/camera_info" - yaw: - joint: "yaw_joint" - pid: { p: 8, i: 100, d: 0.6, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 20, i: 200, d: 0.4, i_clamp_max: 0.8, i_clamp_min: -0.8, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.1 - resistance_coff_qd_15: 0.4 - resistance_coff_qd_16: 0.8 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.18 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 10 - vel_data_num: 30 - center_data_num: 100 - gyro_data_num: 100 diff --git a/rm_gimbal_controllers/test/load_controllers.launch b/rm_gimbal_controllers/test/load_controllers.launch index 77e15dde..e2eb54f5 100644 --- a/rm_gimbal_controllers/test/load_controllers.launch +++ b/rm_gimbal_controllers/test/load_controllers.launch @@ -1,9 +1,8 @@ - - + diff --git a/rm_gimbal_controllers/test/sentry.yaml b/rm_gimbal_controllers/test/sentry.yaml deleted file mode 100644 index bd004a5d..00000000 --- a/rm_gimbal_controllers/test/sentry.yaml +++ /dev/null @@ -1,69 +0,0 @@ -controllers: - upper_gimbal_controller: - type: rm_gimbal_controllers/Controller - time_compensation: 0.004 - publish_rate: 100 - chassis_angular_data_num: 20 - detection_topic: "/upper_detection" - camera_topic: "/upper_camera/camera_info" - detection_frame: "upper_detection" - yaw: - joint: "upper_yaw_joint" - pid: { p: 5.0, i: 0, d: 0.3, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "upper_pitch_joint" - pid: { p: 8.0, i: 50, d: 0.3, i_clamp_max: 0.1, i_clamp_min: -0.1, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 0.4 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.2 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 20 - vel_data_num: 30 - center_data_num: 100 - gyro_data_num: 100 - lower_gimbal_controller: - type: rm_gimbal_controllers/Controller - time_compensation: 0.004 - publish_rate: 100 - chassis_angular_data_num: 20 - detection_topic: "/lower_detection" - camera_topic: "/lower_camera/camera_info" - detection_frame: "lower_detection" - yaw: - joint: "lower_yaw_joint" - pid: { p: 5.0, i: 0, d: 0.3, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "lower_pitch_joint" - pid: { p: 5.0, i: 100, d: 0.2, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 0.4 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.2 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 20 - vel_data_num: 30 - center_data_num: 100 - gyro_data_num: 100 diff --git a/rm_gimbal_controllers/test/standard3.yaml b/rm_gimbal_controllers/test/standard3.yaml deleted file mode 100644 index 447e1de0..00000000 --- a/rm_gimbal_controllers/test/standard3.yaml +++ /dev/null @@ -1,24 +0,0 @@ -controllers: - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 10, i: 100, d: 0.4, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 15, i: 50, d: 0.3, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 diff --git a/rm_gimbal_controllers/test/standard4.yaml b/rm_gimbal_controllers/test/standard4.yaml deleted file mode 100644 index 767656c6..00000000 --- a/rm_gimbal_controllers/test/standard4.yaml +++ /dev/null @@ -1,24 +0,0 @@ -controllers: - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 8, i: 0, d: 0.4, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 20.0, i: 1, d: 0.45, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 0.1 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 diff --git a/rm_gimbal_controllers/test/standard5.yaml b/rm_gimbal_controllers/test/standard5.yaml deleted file mode 100644 index 21ed7d71..00000000 --- a/rm_gimbal_controllers/test/standard5.yaml +++ /dev/null @@ -1,25 +0,0 @@ -controllers: - gimbal_controller: - type: rm_gimbal_controllers/Controller - publish_rate: 100 - yaw: - joint: "yaw_joint" - pid: { p: 30.0, i: 0., d: 1.2, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 20.0, i: 1, d: 0.45, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 diff --git a/rm_orientation_controller/test/load_controllers.launch b/rm_orientation_controller/test/load_controllers.launch index b2c1e564..3fa433ac 100644 --- a/rm_orientation_controller/test/load_controllers.launch +++ b/rm_orientation_controller/test/load_controllers.launch @@ -1,6 +1,5 @@ - - + diff --git a/rm_orientation_controller/test/orientation_config_template.yaml b/rm_orientation_controller/test/orientation_config_template.yaml index aca21478..c042d3f1 100644 --- a/rm_orientation_controller/test/orientation_config_template.yaml +++ b/rm_orientation_controller/test/orientation_config_template.yaml @@ -1,8 +1,7 @@ controllers: orientation_controller: - imu_name: gimbal_imu type: rm_orientation_controller/Controller - frame_fixed: "gimbal_imu" + publish_rate: 100 + name: "gimbal_imu" frame_source: "odom" frame_target: "base_link" - publish_rate: 100 diff --git a/rm_shooter_controllers/test/load_controllers.launch b/rm_shooter_controllers/test/load_controllers.launch index 311de5b0..49cc1fd6 100644 --- a/rm_shooter_controllers/test/load_controllers.launch +++ b/rm_shooter_controllers/test/load_controllers.launch @@ -1,6 +1,5 @@ - - + diff --git a/rm_shooter_controllers/test/shooter_config_template.yaml b/rm_shooter_controllers/test/shooter_config_template.yaml index c8e554a1..b51d78db 100644 --- a/rm_shooter_controllers/test/shooter_config_template.yaml +++ b/rm_shooter_controllers/test/shooter_config_template.yaml @@ -1,37 +1,15 @@ controllers: - upper_shooter_controller: + shooter_controller: type: rm_shooter_controllers/Controller publish_rate: 50 friction_left: - joint: "upper_left_friction_wheel_joint" + 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: "upper_right_friction_wheel_joint" + 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: "upper_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_speed: 0.1 - block_duration: 0.05 - block_overtime: 0.5 - anti_block_angle: 0.2 - anti_block_threshold: 0.1 - qd_15: 458.0 - qd_30: 714.0 - lower_shooter_controller: - type: rm_shooter_controllers/Controller - publish_rate: 50 - friction_left: - joint: "lower_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: "lower_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: "lower_trigger_joint" + 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 diff --git a/tof_radar_controller/test/load_controller.launch b/tof_radar_controller/test/load_controller.launch index c330bc28..f67839db 100644 --- a/tof_radar_controller/test/load_controller.launch +++ b/tof_radar_controller/test/load_controller.launch @@ -1,6 +1,5 @@ - - + From f95b79322e7968c3bcc99d926c772ee9e60d0998 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Mon, 12 Sep 2022 19:55:49 +0800 Subject: [PATCH 012/130] Fix the dependence bug in rm_calibration_controllers. --- rm_calibration_controllers/CMakeLists.txt | 1 + rm_calibration_controllers/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 9321cf69..4a680f4b 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED roscpp rm_common + controller_interface effort_controllers ) diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 94d82ad2..e5a3cade 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -14,6 +14,7 @@ rm_common + controller_interface effort_controllers From db14edab2ecc6c1b0cef4c031cf9cd392dbd00b0 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 18 Sep 2022 01:41:04 +0800 Subject: [PATCH 013/130] Estimate chassis vel with moving average. --- .../rm_gimbal_controllers/gimbal_base.h | 89 ++++++++++++++++++- rm_gimbal_controllers/src/gimbal_base.cpp | 25 ++++-- 2 files changed, 105 insertions(+), 9 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 70491f6a..b41ced6a 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -54,6 +54,93 @@ namespace rm_gimbal_controllers { +class Vector3WithFilter +{ +public: + Vector3WithFilter(int num_data) + { + for (int i = 0; i < 3; i++) + filter_vector_.push_back(std::make_shared>(num_data)); + } + void input(double vector[3], double period) + { + for (int i = 0; i < 3; i++) + { + if (period < 0) + return; + if (period > 0.1) + filter_vector_[i]->clear(); + filter_vector_[i]->input(vector[i]); + } + } + double x() + { + return filter_vector_[0]->output(); + } + double y() + { + return filter_vector_[1]->output(); + } + double z() + { + return filter_vector_[2]->output(); + } + +private: + std::vector>> filter_vector_; +}; +class ChassisVel +{ +public: + ChassisVel(const ros::NodeHandle& nh) + { + double num_data; + nh.param("num_data", num_data, 20.0); + nh.param("debug", is_debug_, true); + linear_ = std::make_shared(num_data); + angular_ = std::make_shared(num_data); + if (is_debug_) + { + real_pub_.reset(new realtime_tools::RealtimePublisher(nh, "real", 1)); + filtered_pub_.reset(new realtime_tools::RealtimePublisher(nh, "filtered", 1)); + } + } + std::shared_ptr linear_; + std::shared_ptr angular_; + void update(double linear_vel[3], double angular_vel[3], double period) + { + linear_->input(linear_vel, period); + angular_->input(angular_vel, period); + if (is_debug_ && loop_count_ % 10 == 0) + { + if (real_pub_->trylock()) + { + real_pub_->msg_.linear.x = linear_vel[0]; + filtered_pub_->msg_.linear.x = linear_->x(); + real_pub_->msg_.linear.y = linear_vel[1]; + filtered_pub_->msg_.linear.y = linear_->y(); + real_pub_->msg_.linear.z = linear_vel[2]; + filtered_pub_->msg_.linear.z = linear_->z(); + real_pub_->msg_.angular.x = angular_vel[0]; + filtered_pub_->msg_.angular.x = angular_->x(); + real_pub_->msg_.angular.y = angular_vel[1]; + filtered_pub_->msg_.angular.y = angular_->y(); + real_pub_->msg_.angular.z = angular_vel[2]; + filtered_pub_->msg_.angular.z = angular_->z(); + + real_pub_->unlockAndPublish(); + filtered_pub_->unlockAndPublish(); + } + } + loop_count_++; + } + +private: + bool is_debug_; + int loop_count_; + std::shared_ptr> real_pub_{}, filtered_pub_{}; +}; + class Controller : public controller_interface::MultiInterfaceController @@ -108,7 +195,7 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; enum { diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 1241a616..cc73c2e7 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -62,6 +62,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); @@ -383,7 +385,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_.angular.z); + ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z()); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } @@ -408,13 +410,20 @@ double Controller::feedForward(const ros::Time& time) void Controller::updateChassisVel() { double tf_period = odom2base_.header.stamp.toSec() - last_odom2base_.header.stamp.toSec(); - if (tf_period > 0.0 && tf_period < 0.1) - { - chassis_vel_.linear.x = (odom2base_.transform.translation.x - last_odom2base_.transform.translation.x) / tf_period; - chassis_vel_.linear.y = (odom2base_.transform.translation.y - last_odom2base_.transform.translation.y) / tf_period; - chassis_vel_.angular.z = - (yawFromQuat(odom2base_.transform.rotation) - yawFromQuat(last_odom2base_.transform.rotation)) / tf_period; - } + double linear_x = (odom2base_.transform.translation.x - last_odom2base_.transform.translation.x) / tf_period; + double linear_y = (odom2base_.transform.translation.y - last_odom2base_.transform.translation.y) / tf_period; + double linear_z = (odom2base_.transform.translation.z - last_odom2base_.transform.translation.z) / tf_period; + double last_angular_position_x, last_angular_position_y, last_angular_position_z, angular_position_x, + angular_position_y, angular_position_z; + quatToRPY(odom2base_.transform.rotation, angular_position_x, angular_position_y, angular_position_z); + quatToRPY(last_odom2base_.transform.rotation, last_angular_position_x, last_angular_position_y, + last_angular_position_z); + double angular_x = angles::shortest_angular_distance(last_angular_position_x, angular_position_x) / tf_period; + double angular_y = angles::shortest_angular_distance(last_angular_position_y, angular_position_y) / tf_period; + double angular_z = angles::shortest_angular_distance(last_angular_position_z, angular_position_z) / tf_period; + double linear_vel[3]{ linear_x, linear_y, linear_z }; + double angular_vel[3]{ angular_x, angular_y, angular_z }; + chassis_vel_->update(linear_vel, angular_vel, tf_period); last_odom2base_ = odom2base_; } From 668c88461f752797f78614d6605bf9d671882e3b Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 19 Sep 2022 21:03:07 +0800 Subject: [PATCH 014/130] Subtract chassis_vel from target_vel. --- rm_gimbal_controllers/src/gimbal_base.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index cc73c2e7..62ca8077 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -240,9 +240,12 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - target_pos.x = target_pos.x - odom2pitch_.transform.translation.x; - target_pos.y = target_pos.y - odom2pitch_.transform.translation.y; - target_pos.z = target_pos.z - odom2pitch_.transform.translation.z; + target_pos.x -= odom2pitch_.transform.translation.x; + target_pos.y -= odom2pitch_.transform.translation.y; + target_pos.z -= odom2pitch_.transform.translation.z; + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed); From cfdfb49bcd36b5e39159aa014c26503326ed18de Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Wed, 5 Oct 2022 23:01:27 +0800 Subject: [PATCH 015/130] Dealing with the situation of imu offline. --- .../orientation_controller.h | 8 +++-- .../src/orientation_controller.cpp | 32 +++++++++++-------- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h index e9e79888..b03ac226 100644 --- a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h +++ b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h @@ -6,14 +6,14 @@ #include #include -#include +#include #include #include #include namespace rm_orientation_controller { -class Controller : public controller_interface::MultiInterfaceController { public: @@ -26,9 +26,11 @@ class Controller : public controller_interface::MultiInterfaceControllerget()->getHandle(name); + imu_sensor_ = robot_hw->get()->getHandle(name); robot_state_ = robot_hw->get()->getHandle("robot_state"); tf_broadcaster_.init(root_nh); @@ -31,19 +31,23 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro void Controller::update(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::TransformStamped source2target; - source2target.header.stamp = time; - source2target.header.stamp.nsec += 1; // Avoid redundant timestamp - source2target_msg_.header.stamp = time; - source2target_msg_.header.stamp.nsec += 1; - source2target_msg_ = - getTransform(ros::Time(0), source2target, imu_sensor_.getOrientation()[0], imu_sensor_.getOrientation()[1], - imu_sensor_.getOrientation()[2], imu_sensor_.getOrientation()[3]) ? - source2target : - source2target_msg_; - robot_state_.setTransform(source2target_msg_, "rm_orientation_controller"); - if (!receive_imu_msg_) - tf_broadcaster_.sendTransform(source2target_msg_); + if (imu_sensor_.getTimeStamp() > last_imu_update_time_) + { + last_imu_update_time_ = imu_sensor_.getTimeStamp(); + geometry_msgs::TransformStamped source2target; + source2target.header.stamp = time; + source2target.header.stamp.nsec += 1; // Avoid redundant timestamp + source2target_msg_.header.stamp = time; + source2target_msg_.header.stamp.nsec += 1; + source2target_msg_ = + getTransform(ros::Time(0), source2target, imu_sensor_.getOrientation()[0], imu_sensor_.getOrientation()[1], + imu_sensor_.getOrientation()[2], imu_sensor_.getOrientation()[3]) ? + source2target : + source2target_msg_; + robot_state_.setTransform(source2target_msg_, "rm_orientation_controller"); + if (!receive_imu_msg_) + tf_broadcaster_.sendTransform(source2target_msg_); + } } bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, From f0ca37ad4f4ad0aee93daaf3720c8f0748c09d30 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 13 Oct 2022 18:11:49 +0800 Subject: [PATCH 016/130] Fix trylock bug. --- .../include/rm_gimbal_controllers/gimbal_base.h | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index b41ced6a..003d9dca 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -116,19 +116,23 @@ class ChassisVel if (real_pub_->trylock()) { real_pub_->msg_.linear.x = linear_vel[0]; - filtered_pub_->msg_.linear.x = linear_->x(); real_pub_->msg_.linear.y = linear_vel[1]; - filtered_pub_->msg_.linear.y = linear_->y(); real_pub_->msg_.linear.z = linear_vel[2]; - filtered_pub_->msg_.linear.z = linear_->z(); real_pub_->msg_.angular.x = angular_vel[0]; - filtered_pub_->msg_.angular.x = angular_->x(); real_pub_->msg_.angular.y = angular_vel[1]; - filtered_pub_->msg_.angular.y = angular_->y(); real_pub_->msg_.angular.z = angular_vel[2]; - filtered_pub_->msg_.angular.z = angular_->z(); real_pub_->unlockAndPublish(); + } + if (filtered_pub_->trylock()) + { + filtered_pub_->msg_.linear.x = linear_->x(); + filtered_pub_->msg_.linear.y = linear_->y(); + filtered_pub_->msg_.linear.z = linear_->z(); + filtered_pub_->msg_.angular.x = angular_->x(); + filtered_pub_->msg_.angular.y = angular_->y(); + filtered_pub_->msg_.angular.z = angular_->z(); + filtered_pub_->unlockAndPublish(); } } From 28321696d0353f9a1ae035a763a2237dee57732c Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Thu, 20 Oct 2022 14:46:46 +0800 Subject: [PATCH 017/130] Fix the bug that the shooter cannot be turned from push to ready. --- rm_shooter_controllers/src/standard.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 29055c35..5d2d5790 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -72,9 +72,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_friction_r = ros::NodeHandle(controller_nh, "friction_right"); ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger"); effort_joint_interface_ = robot_hw->get(); - return !(!ctrl_friction_l_.init(effort_joint_interface_, nh_friction_l) || - !ctrl_friction_r_.init(effort_joint_interface_, nh_friction_r) || - !ctrl_trigger_.init(effort_joint_interface_, nh_trigger)); + return ctrl_friction_l_.init(effort_joint_interface_, nh_friction_l) && + ctrl_friction_r_.init(effort_joint_interface_, nh_friction_r) && + ctrl_trigger_.init(effort_joint_interface_, nh_trigger); } void Controller::starting(const ros::Time& /*time*/) @@ -91,7 +91,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { if (state_ != BLOCK) if ((state_ != PUSH || cmd_.mode != READY) || - (state_ == PUSH && cmd_.mode == READY && + (cmd_.mode == READY && std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < 0.01)) { state_ = cmd_.mode; @@ -159,9 +159,12 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! - ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - - 2. * M_PI / static_cast(push_per_rotation_)); - last_shoot_time_ = time; + if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < 0.01) + { + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; + } } else ROS_DEBUG("[Shooter] Wait for friction wheel"); From e0ca26baab8984fde32c5e56c0306bdbd7d32db9 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Mon, 24 Oct 2022 21:04:02 +0800 Subject: [PATCH 018/130] Parametric position difference of trigger. --- rm_shooter_controllers/cfg/Shooter.cfg | 1 + .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 6 +++++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 248b43e0..b9086984 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -11,6 +11,7 @@ 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("pos_diff",double_t,0,"Trigger position difference to enter push mode",0.001,0.0,1) gen.add("qd_10", double_t, 0, "Friction wheel rotation speed when bullet speed is 10m/s", 300, 0.0, 9999) gen.add("qd_15", double_t, 0, "Friction wheel rotation speed when bullet speed is 15m/s", 300, 0.0, 9999) gen.add("qd_16", double_t, 0, "Friction wheel rotation speed when bullet speed is 16m/s", 300, 0.0, 9999) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 8ae3332c..69312832 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -55,7 +55,7 @@ namespace rm_shooter_controllers struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold; - double qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; + double pos_diff, qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; }; class Controller : public controller_interface::MultiInterfaceController= 1. / cmd_.hz) { // Time to shoot!!! - if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < 0.01) + if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < + config_.pos_diff) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); @@ -247,6 +249,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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.pos_diff = init_config.pos_diff; config.qd_10 = init_config.qd_10; config.qd_15 = init_config.qd_15; config.qd_16 = init_config.qd_16; @@ -261,6 +264,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .block_overtime = config.block_overtime, .anti_block_angle = config.anti_block_angle, .anti_block_threshold = config.anti_block_threshold, + .pos_diff = config.pos_diff, .qd_10 = config.qd_10, .qd_15 = config.qd_15, .qd_16 = config.qd_16, From 952a0e49ffd6937f0a2cc714a33714419286b4e4 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Fri, 4 Nov 2022 16:05:37 +0800 Subject: [PATCH 019/130] Modify the params about enter and exit push mode. --- rm_shooter_controllers/cfg/Shooter.cfg | 3 ++- .../include/rm_shooter_controllers/standard.h | 5 +++-- rm_shooter_controllers/src/standard.cpp | 14 +++++++++----- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index b9086984..0251043f 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -11,7 +11,8 @@ 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("pos_diff",double_t,0,"Trigger position difference to enter push mode",0.001,0.0,1) +gen.add("enter_pos_threshold",double_t,0,"The trigger position threshold to enter push mode",0.001,0.0,1) +gen.add("exit_pos_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("qd_10", double_t, 0, "Friction wheel rotation speed when bullet speed is 10m/s", 300, 0.0, 9999) gen.add("qd_15", double_t, 0, "Friction wheel rotation speed when bullet speed is 15m/s", 300, 0.0, 9999) gen.add("qd_16", double_t, 0, "Friction wheel rotation speed when bullet speed is 16m/s", 300, 0.0, 9999) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 69312832..80c4ab8b 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -54,8 +54,9 @@ namespace rm_shooter_controllers { struct Config { - double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold; - double pos_diff, qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; + double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, + enter_pos_threshold, exit_pos_threshold; + double qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; }; class Controller : public controller_interface::MultiInterfaceController= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.pos_diff) + config_.enter_pos_threshold) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); @@ -249,7 +251,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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.pos_diff = init_config.pos_diff; + config.enter_pos_threshold = init_config.enter_pos_threshold; + config.exit_pos_threshold = init_config.exit_pos_threshold; config.qd_10 = init_config.qd_10; config.qd_15 = init_config.qd_15; config.qd_16 = init_config.qd_16; @@ -264,7 +267,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .block_overtime = config.block_overtime, .anti_block_angle = config.anti_block_angle, .anti_block_threshold = config.anti_block_threshold, - .pos_diff = config.pos_diff, + .enter_pos_threshold = config.enter_pos_threshold, + .exit_pos_threshold = config.exit_pos_threshold, .qd_10 = config.qd_10, .qd_15 = config.qd_15, .qd_16 = config.qd_16, From 99e808e3546df6c60987140ec907f3c51363ed1e Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Fri, 4 Nov 2022 19:14:33 +0800 Subject: [PATCH 020/130] Modify the params name. --- rm_shooter_controllers/cfg/Shooter.cfg | 8 ++++---- .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 16 ++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 0251043f..80828b63 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -7,12 +7,12 @@ 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("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("enter_pos_threshold",double_t,0,"The trigger position threshold to enter push mode",0.001,0.0,1) -gen.add("exit_pos_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) +gen.add("enter_push_threshold",double_t,0,"The trigger position threshold to enter 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("qd_10", double_t, 0, "Friction wheel rotation speed when bullet speed is 10m/s", 300, 0.0, 9999) gen.add("qd_15", double_t, 0, "Friction wheel rotation speed when bullet speed is 15m/s", 300, 0.0, 9999) gen.add("qd_16", double_t, 0, "Friction wheel rotation speed when bullet speed is 16m/s", 300, 0.0, 9999) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 80c4ab8b..89bfa4b6 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -55,7 +55,7 @@ namespace rm_shooter_controllers struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, - enter_pos_threshold, exit_pos_threshold; + enter_push_threshold, exit_push_threshold; double qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 84c238ca..456d60b3 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -50,8 +50,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .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.), - .enter_pos_threshold = getParam(controller_nh, "enter_pos_threshold", 0.), - .exit_pos_threshold = getParam(controller_nh, "exit_pos_threshold", 0.), + .enter_push_threshold = getParam(controller_nh, "enter_push_threshold", 0.), + .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.), .qd_10 = getParam(controller_nh, "qd_10", 0.), .qd_15 = getParam(controller_nh, "qd_15", 0.), .qd_16 = getParam(controller_nh, "qd_16", 0.), @@ -95,7 +95,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) 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_pos_threshold)) + config_.exit_push_threshold)) { state_ = cmd_.mode; state_changed_ = true; @@ -163,7 +163,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.enter_pos_threshold) + config_.enter_push_threshold) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); @@ -251,8 +251,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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.enter_pos_threshold = init_config.enter_pos_threshold; - config.exit_pos_threshold = init_config.exit_pos_threshold; + config.enter_push_threshold = init_config.enter_push_threshold; + config.exit_push_threshold = init_config.exit_push_threshold; config.qd_10 = init_config.qd_10; config.qd_15 = init_config.qd_15; config.qd_16 = init_config.qd_16; @@ -267,8 +267,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .block_overtime = config.block_overtime, .anti_block_angle = config.anti_block_angle, .anti_block_threshold = config.anti_block_threshold, - .enter_pos_threshold = config.enter_pos_threshold, - .exit_pos_threshold = config.exit_pos_threshold, + .enter_push_threshold = config.enter_push_threshold, + .exit_push_threshold = config.exit_push_threshold, .qd_10 = config.qd_10, .qd_15 = config.qd_15, .qd_16 = config.qd_16, From 74c54dd4333b62870c5b0fb41be6930403763ca2 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Thu, 10 Nov 2022 17:38:37 +0800 Subject: [PATCH 021/130] Fix the bug that shooter cannot push or enter block when the position error is too big. --- rm_shooter_controllers/src/standard.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 456d60b3..5ec7031f 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -174,8 +174,9 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ROS_DEBUG("[Shooter] Wait for friction wheel"); // Check block - if (ctrl_trigger_.joint_.getEffort() < -config_.block_effort && - std::abs(ctrl_trigger_.joint_.getVelocity()) < config_.block_speed) + 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) { if (!maybe_block_) { From c4788a95acfe8bf34577e04a01ad72c46749145b Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sun, 13 Nov 2022 22:28:52 +0800 Subject: [PATCH 022/130] Optimize the logic of entering the block mode. --- rm_shooter_controllers/src/standard.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 5ec7031f..2a7729fc 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -176,7 +176,8 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) // 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) + ((time - last_shoot_time_).toSec() > 1 / cmd_.hz && + std::abs(ctrl_trigger_.joint_.getVelocity()) < config_.block_speed)) { if (!maybe_block_) { From 44fa75591b784014956e0d67d184a0ca560bdf08 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Wed, 16 Nov 2022 12:34:08 +0800 Subject: [PATCH 023/130] Modify the name and the description of the params about push forward threshold. --- rm_shooter_controllers/cfg/Shooter.cfg | 2 +- .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 80828b63..787e6de3 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -11,7 +11,7 @@ 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("enter_push_threshold",double_t,0,"The trigger position threshold to enter push mode",0.01,0.0,1) +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("qd_10", double_t, 0, "Friction wheel rotation speed when bullet speed is 10m/s", 300, 0.0, 9999) gen.add("qd_15", double_t, 0, "Friction wheel rotation speed when bullet speed is 15m/s", 300, 0.0, 9999) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 89bfa4b6..f451f743 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -55,7 +55,7 @@ namespace rm_shooter_controllers struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, - enter_push_threshold, exit_push_threshold; + forward_push_threshold, exit_push_threshold; double qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 2a7729fc..365961e0 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -50,7 +50,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .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.), - .enter_push_threshold = getParam(controller_nh, "enter_push_threshold", 0.), + .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.), .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.), .qd_10 = getParam(controller_nh, "qd_10", 0.), .qd_15 = getParam(controller_nh, "qd_15", 0.), @@ -163,7 +163,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.enter_push_threshold) + config_.forward_push_threshold) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); @@ -253,7 +253,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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.enter_push_threshold = init_config.enter_push_threshold; + config.forward_push_threshold = init_config.forward_push_threshold; config.exit_push_threshold = init_config.exit_push_threshold; config.qd_10 = init_config.qd_10; config.qd_15 = init_config.qd_15; @@ -269,7 +269,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .block_overtime = config.block_overtime, .anti_block_angle = config.anti_block_angle, .anti_block_threshold = config.anti_block_threshold, - .enter_push_threshold = config.enter_push_threshold, + .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, .qd_10 = config.qd_10, .qd_15 = config.qd_15, From 174d96e2f20250cc8d6c61576ac611d665fdb3fc Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Sun, 20 Nov 2022 23:36:52 +0800 Subject: [PATCH 024/130] Update fix can not return . --- .../joint_calibration_controller.h | 2 +- .../src/joint_calibration_controller.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 5cf4cd4a..5e47f0ef 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -107,7 +107,7 @@ class JointCalibrationController CALIBRATED }; int state_{}, countdown_{}; - double velocity_search_{}, target_position_{}, velocity_threshold_{}, position_threshold_{}; + double velocity_search_{}, target_position_{}, target_velocity_{}, velocity_threshold_{}, position_threshold_{}; double positive_position_{}, negative_position_{}; bool is_return_{}, is_center_{}, returned_{}; rm_control::ActuatorExtraHandle actuator_; diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index d0a98364..0380a5a4 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -79,6 +79,11 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); return false; } + if (!nh_return.getParam("target_position", target_velocity_)) + { + ROS_ERROR("Velocity value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); + return false; + } if (!controller_nh.getParam("threshold", position_threshold_)) { ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); @@ -162,7 +167,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); state_ = CALIBRATED; if (is_return_) - position_ctrl_.joint_.setCommand(target_position_); + position_ctrl_.setCommand(target_position_); else { velocity_ctrl_.joint_.setCommand(0.); @@ -178,6 +183,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati { if ((std::abs(position_ctrl_.joint_.getPosition()) - target_position_) < position_threshold_) returned_ = true; + position_ctrl_.setCommand(target_position_,target_velocity_); position_ctrl_.update(time, period); } else From 4a74c866ea907329681efc99619cd23263924b8e Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Mon, 21 Nov 2022 12:47:02 +0800 Subject: [PATCH 025/130] eyes is lost. --- .../src/joint_calibration_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index 0380a5a4..ec36a52f 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -79,10 +79,10 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); return false; } - if (!nh_return.getParam("target_position", target_velocity_)) + if (!nh_return.getParam("target_velocity", target_velocity_)) { - ROS_ERROR("Velocity value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); - return false; + ROS_ERROR("Velocity value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); + return false; } if (!controller_nh.getParam("threshold", position_threshold_)) { @@ -183,7 +183,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati { if ((std::abs(position_ctrl_.joint_.getPosition()) - target_position_) < position_threshold_) returned_ = true; - position_ctrl_.setCommand(target_position_,target_velocity_); + position_ctrl_.setCommand(target_position_, target_velocity_); position_ctrl_.update(time, period); } else From 4f4335b98c472f3d44c648cd3e888b172ea05ab4 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Wed, 23 Nov 2022 14:34:23 +0800 Subject: [PATCH 026/130] Add some test codes for balance chassis. --- rm_chassis_controllers/CMakeLists.txt | 4 +- .../include/rm_chassis_controllers/balance.h | 43 +++++ .../rm_chassis_controllers/reaction_wheel.h | 42 ----- rm_chassis_controllers/package.xml | 1 + .../rm_chassis_controllers_plugins.xml | 4 +- rm_chassis_controllers/src/balance.cpp | 158 ++++++++++++++++ rm_chassis_controllers/src/reaction_wheel.cpp | 173 ------------------ .../test/template_reaction_wheel.yaml | 27 ++- 8 files changed, 225 insertions(+), 227 deletions(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/balance.h delete mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/reaction_wheel.h create mode 100644 rm_chassis_controllers/src/balance.cpp delete mode 100644 rm_chassis_controllers/src/reaction_wheel.cpp diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 35af37eb..f9823864 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED controller_interface effort_controllers tf2_geometry_msgs + gazebo_msgs ) find_package(Eigen3 REQUIRED) @@ -44,6 +45,7 @@ catkin_package( effort_controllers tf2_geometry_msgs + gazebo_msgs LIBRARIES ${PROJECT_NAME} ) @@ -64,7 +66,7 @@ add_library(${PROJECT_NAME} src/omni.cpp src/sentry.cpp src/swerve.cpp - src/reaction_wheel.cpp + src/balance.cpp ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h new file mode 100644 index 00000000..e215aece --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -0,0 +1,43 @@ +// +// Created by yezi on 2022/11/15. +// + +#pragma once + +#include +#include +#include +#include +#include + +#include "rm_chassis_controllers/chassis_base.h" + +namespace rm_chassis_controllers +{ +using Eigen::Matrix; +class BalanceController : public ChassisBase +{ +public: + BalanceController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +private: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; + geometry_msgs::Twist odometry() override; + static const int STATE_DIM = 5; + static const int CONTROL_DIM = 2; + Eigen::Matrix k_{}; + Eigen::Matrix a_{}, q_{}; + Eigen::Matrix b_{}; + Eigen::Matrix r_{}; + Eigen::Matrix x_; + double wheel_radius_; + + hardware_interface::ImuSensorHandle imu_handle_; + hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_, momentum_block_joint_handle_; + ros::Subscriber model_states_sub_; + void modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg); +}; + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/reaction_wheel.h b/rm_chassis_controllers/include/rm_chassis_controllers/reaction_wheel.h deleted file mode 100644 index 10ef569a..00000000 --- a/rm_chassis_controllers/include/rm_chassis_controllers/reaction_wheel.h +++ /dev/null @@ -1,42 +0,0 @@ -// -// Created by qiayuan on 2022/4/17. -// -// Reference: The Cubli: A Cube that can Jump Up and Balance - -#pragma once - -#include -#include -#include -#include - -namespace rm_chassis_controllers -{ -using Eigen::Matrix; -class ReactionWheelController - : public controller_interface::MultiInterfaceController -{ -public: - ReactionWheelController() = default; - - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - void starting(const ros::Time& time) override; - void update(const ros::Time& time, const ros::Duration& period) override; - -private: - static const int STATE_DIM = 3; - static const int CONTROL_DIM = 1; - Eigen::Matrix k_{}; - Eigen::Matrix a_{}, q_{}; - Eigen::Matrix b_{}; - Eigen::Matrix r_{}; - - hardware_interface::ImuSensorHandle imu_handle_; - hardware_interface::JointHandle joint_handle_; - hardware_interface::JointStateHandle left_wheel_handle_, right_wheel_handle_; - double alpha_, torque_g_, pitch_offset_; -}; - -} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 1a659f13..371e64d7 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -19,6 +19,7 @@ controller_interface effort_controllers tf2_geometry_msgs + gazebo_msgs robot_localization diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 1244eff8..07abcd48 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -24,8 +24,8 @@ type of hardware interface. - The ReactionWheelController is RoboMaster balance standard robot Balance controller. It expects a diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp new file mode 100644 index 00000000..f11a8471 --- /dev/null +++ b/rm_chassis_controllers/src/balance.cpp @@ -0,0 +1,158 @@ +// +// Created by qiayuan on 2022/11/15. +// +#include "rm_chassis_controllers/balance.h" + +#include +#include +#include +#include +#include + +namespace rm_chassis_controllers +{ +bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + ChassisBase::init(robot_hw, root_nh, controller_nh); + + imu_handle_ = robot_hw->get()->getHandle( + getParam(controller_nh, "imu_name", std::string("base_imu"))); + left_wheel_joint_handle_ = robot_hw->get()->getHandle("left_wheel_joint"); + right_wheel_joint_handle_ = robot_hw->get()->getHandle("right_wheel_joint"); + momentum_block_joint_handle_ = + robot_hw->get()->getHandle("momentum_block_joint"); + + // i_b is moment of inertia of the pendulum body around the pivot point, + // i_w is the moment of inertia of the wheel around the rotational axis of the motor + // l is the distance between the motor axis and the pivot point + // l_b is the distance between the center of mass of the pendulum body and the pivot point + double m_b, m_p, m_c, l_p, l_b, g; + + if (!controller_nh.getParam("m_b", m_b)) + { + ROS_ERROR("Params m_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("m_p", m_p)) + { + ROS_ERROR("Params m_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("m_c", m_c)) + { + ROS_ERROR("Params i_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("l_p", l_p)) + { + ROS_ERROR("Params i_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("l_b", l_b)) + { + ROS_ERROR("Params l_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("g", g)) + { + ROS_ERROR("Params g doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("wheel_radius", wheel_radius_)) + { + ROS_ERROR("Params wheel_radius doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + } + + q_.setZero(); + r_.setZero(); + XmlRpc::XmlRpcValue q, r; + controller_nh.getParam("q", q); + controller_nh.getParam("r", r); + // Check and get Q + ROS_ASSERT(q.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(q.size() == STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) + { + ROS_ASSERT(q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || q[i].getType() == XmlRpc::XmlRpcValue::TypeInt); + if (q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) + q_(i, i) = static_cast(q[i]); + else if (q[i].getType() == XmlRpc::XmlRpcValue::TypeInt) + q_(i, i) = static_cast(q[i]); + } + // Check and get R + ROS_ASSERT(r.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(r.size() == CONTROL_DIM); + for (int i = 0; i < CONTROL_DIM; ++i) + { + ROS_ASSERT(r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || r[i].getType() == XmlRpc::XmlRpcValue::TypeInt); + if (r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) + r_(i, i) = static_cast(r[i]); + else if (r[i].getType() == XmlRpc::XmlRpcValue::TypeInt) + r_(i, i) = static_cast(r[i]); + } + + // Continuous model \dot{x} = A x + B u + // double m = m_b + m_c + m_p; + // double a_2_0 = -g * (m_b + m_p) / m_c; + // double a_2_1 = -g * m_b / (l_p * m_c); + // double a_3_0 = m * g / (l_p * m_c); + // double a_3_1 = m_b * g * (m_c + m_p) / (l_p * l_p * m_c * m_p); + // double a_4_0 = m * g * (l_p - l_b) / (m_c * l_p); + // double a_4_1 = g * m_b * (-l_b * m_c - l_b * m_p + l_p * m_p) / (l_p * l_p * m_c * m_p); + // + // double b_2_0 = 1 / m_c; + // double b_2_1 = (l_b - l_p) / (l_p * m_c); + // double b_3_0 = -1 / (l_p * m_c); + // double b_3_1 = (-l_b * m_c - l_b * m_p + l_p * m_p) / (l_p * l_p * m_c * m_p); + // double b_4_0 = (l_b - l_p) / (l_p * m_c); + // double b_4_1 = + // l_b * l_b / (l_p * l_p * m_p) + l_b * l_b / (l_p * l_p * m_c) - 2.0 * l_b / (l_p * m_c) + 1 / m_c + 1 / m_b; + a_ << 0., 0., 0., 1., 0., 0., 0., 0., 0., 1., -1.6066, -2.4065, 0., 0., 0., 25.0596, 106.6442, 0., 0., 0., 7.9258, + -12.4064, 0., 0., 0.; + b_ << 0., 0., 0., 0., 0.6148, -0.0419, -6.9291, -0.6828, 0.3477, 0.6761; + + Lqr lqr(a_, b_, q_, r_); + if (!lqr.computeK()) + { + ROS_ERROR("Failed to compute K of LQR."); + return false; + } + + k_ = lqr.getK(); + ROS_INFO_STREAM("K of LQR:" << k_); + + model_states_sub_ = root_nh.subscribe("/gazebo/model_states", 1, + &BalanceController::modelStatesCallBack, this); + return true; +} + +void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period) +{ + x_[1] = momentum_block_joint_handle_.getPosition(); + x_[2] -= vel_cmd_.x; + x_[4] = momentum_block_joint_handle_.getVelocity(); + Eigen::Matrix u; + u = k_ * (-x_); + + right_wheel_joint_handle_.setCommand(u(0) / 2); + left_wheel_joint_handle_.setCommand(u(0) / 2); + momentum_block_joint_handle_.setCommand(u(1)); +} + +void BalanceController::modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg) +{ + double roll, pitch, yaw; + quatToRPY(msg->pose[1].orientation, roll, pitch, yaw); + x_[0] = pitch; + x_[2] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 > 0 ? 1 : -1) * + sqrt(pow(msg->twist[1].linear.x, 2) + pow(msg->twist[1].linear.y, 2)); + x_[3] = msg->twist[1].angular.y; +} +geometry_msgs::Twist BalanceController::odometry() +{ + geometry_msgs::Twist twist; + return twist; +} +} // namespace rm_chassis_controllers +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BalanceController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/reaction_wheel.cpp b/rm_chassis_controllers/src/reaction_wheel.cpp deleted file mode 100644 index 873d4f77..00000000 --- a/rm_chassis_controllers/src/reaction_wheel.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// -// Created by qiayuan on 2022/4/17. -// -#include "rm_chassis_controllers/reaction_wheel.h" - -#include -#include -#include -#include -#include - -namespace rm_chassis_controllers -{ -bool ReactionWheelController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) -{ - if (!MultiInterfaceController::init(robot_hw, root_nh, controller_nh)) - return false; - - imu_handle_ = robot_hw->get()->getHandle( - getParam(controller_nh, "imu_name", std::string("base_imu"))); - joint_handle_ = robot_hw->get()->getHandle( - getParam(controller_nh, "joint", std::string("reaction_wheel_joint"))); - - left_wheel_handle_ = robot_hw->get()->getHandle("left_wheel_joint"); - right_wheel_handle_ = robot_hw->get()->getHandle("right_wheel_joint"); - - // i_b is moment of inertia of the pendulum body around the pivot point, - // i_w is the moment of inertia of the wheel around the rotational axis of the motor - // l is the distance between the motor axis and the pivot point - // l_b is the distance between the center of mass of the pendulum body and the pivot point - double m_b, m_w, i_b, i_w, l, l_b, g; - - if (!controller_nh.getParam("m_b", m_b)) - { - ROS_ERROR("Params m_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("m_w", m_w)) - { - ROS_ERROR("Params m_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("i_b", i_b)) - { - ROS_ERROR("Params i_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("i_w", i_w)) - { - ROS_ERROR("Params i_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("l", l)) - { - ROS_ERROR("Params l doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("l_b", l_b)) - { - ROS_ERROR("Params l_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("g", g)) - { - ROS_ERROR("Params g doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - - if (!controller_nh.getParam("alpha", alpha_)) - { - ROS_ERROR("Params alpha doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - q_.setZero(); - r_.setZero(); - XmlRpc::XmlRpcValue q, r; - controller_nh.getParam("q", q); - controller_nh.getParam("r", r); - // Check and get Q - ROS_ASSERT(q.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(q.size() == STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) - { - ROS_ASSERT(q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || q[i].getType() == XmlRpc::XmlRpcValue::TypeInt); - if (q[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) - q_(i, i) = static_cast(q[i]); - else if (q[i].getType() == XmlRpc::XmlRpcValue::TypeInt) - q_(i, i) = static_cast(q[i]); - } - // Check and get R - ROS_ASSERT(r.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(r.size() == CONTROL_DIM); - for (int i = 0; i < CONTROL_DIM; ++i) - { - ROS_ASSERT(r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || r[i].getType() == XmlRpc::XmlRpcValue::TypeInt); - if (r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) - r_(i, i) = static_cast(r[i]); - else if (r[i].getType() == XmlRpc::XmlRpcValue::TypeInt) - r_(i, i) = static_cast(r[i]); - } - - // Continuous model \dot{x} = A x + B u - torque_g_ = (m_b * l_b + m_w * l) * g; - double temp = i_b + m_w * l * l; - double a_1_0 = torque_g_ / temp; - double a_1_1 = 0.; // TODO: dynamic friction coefficient - double a_1_2 = 0.; // TODO: dynamic friction coefficient - double a_2_0 = -a_1_0; - double a_2_1 = 0.; - double a_2_2 = 0.; - double b_1 = -1. / temp; - double b_2 = (temp + i_w) / (i_w * temp); - a_ << 0., 1., 0., a_1_0, a_1_1, a_1_2, a_2_0, a_2_1, a_2_2; - b_ << 0., b_1, b_2; - - Lqr lqr(a_, b_, q_, r_); - if (!lqr.computeK()) - { - ROS_ERROR("Failed to compute K of LQR."); - return false; - } - - k_ = lqr.getK(); - ROS_INFO_STREAM("K of LQR:" << k_); - return true; -} - -void ReactionWheelController::starting(const ros::Time& time) -{ - pitch_offset_ = 0.; -} - -void ReactionWheelController::update(const ros::Time& time, const ros::Duration& period) -{ - // TODO: simplify, add new quatToRPY - geometry_msgs::Quaternion quat; - quat.x = imu_handle_.getOrientation()[0]; - quat.y = imu_handle_.getOrientation()[1]; - quat.z = imu_handle_.getOrientation()[2]; - quat.w = imu_handle_.getOrientation()[3]; - double roll{}, pitch{}, yaw{}; - quatToRPY(quat, roll, pitch, yaw); - Eigen::Matrix x; - x(0) = pitch; - x(1) = imu_handle_.getAngularVelocity()[1]; - x(2) = joint_handle_.getVelocity(); - - pitch_offset_ = (1. - alpha_) * pitch_offset_ + alpha_ * pitch; - - if (std::abs(pitch_offset_) > 0.2) // TODO pitch_max rosparam - { - if (std::abs(x(2)) < 1e-2) // Damped - pitch_offset_ = 0.; - else // Damping - { - joint_handle_.setCommand(-0.4 * x(2)); // TODO damping_factor rosparam - return; - } - } - - // double pitch_des = std::asin((left_wheel_handle_.getEffort() + right_wheel_handle_.getEffort()) / torque_g_); - double pitch_des = pitch_offset_; - // double pitch_des = 0.; - - x(0) = x(0) - pitch_des; - Eigen::Matrix u; - u = k_ * (-x); // regulate to zero: K*(0 - x) - joint_handle_.setCommand(u(0)); -} - -} // namespace rm_chassis_controllers -PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ReactionWheelController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml index 7f912edd..170d9ecd 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/template_reaction_wheel.yaml @@ -1,18 +1,27 @@ controllers: chassis_controller: - type: rm_chassis_controllers/ReactionWheelController + type: rm_chassis_controllers/BalanceController publish_rate: 100 enable_odom_tf: true + power: + effort_coeff: 12.0 + vel_coeff: 0.0048 + power_offset: 0 + twist_angular: 0.5233 + timeout: 0.1 + pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] imu_name: "base_imu" joint: "reaction_wheel_joint" - m_total: 13.489909 - l: 0.18 - g: -9.81 + m_b: 1.854 + m_c: 5.2 + m_p: 10.098 + l_p: 0.039 + l_b: 0.1389 + g: 9.81 + wheel_radius: 0.125 - inertia_total: 1.955277 - inertia_wheel: 1.795e-3 - - q: [ 1.5 , 10 , 0.01 ] - r: [ 50 ] + q: [ 1000, 1, 2000, 1000, 1 ] + r: [ 1,10 ] From 40fa13d6dc590c983fbf60c065daed085ceccc4e Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 24 Nov 2022 21:03:32 +0800 Subject: [PATCH 027/130] Fix mistake of orientation data. --- .../include/rm_chassis_controllers/balance.h | 4 +- rm_chassis_controllers/src/balance.cpp | 45 ++++++++++++++----- .../test/template_reaction_wheel.yaml | 2 +- 3 files changed, 37 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index e215aece..4b477014 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -25,7 +25,7 @@ class BalanceController : public ChassisBase k_{}; Eigen::Matrix a_{}, q_{}; @@ -33,10 +33,12 @@ class BalanceController : public ChassisBase r_{}; Eigen::Matrix x_; double wheel_radius_; + double position_des_ = 0; hardware_interface::ImuSensorHandle imu_handle_; hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_, momentum_block_joint_handle_; ros::Subscriber model_states_sub_; + ros::Publisher state_pub_; void modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg); }; diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index f11a8471..5a6c3e5f 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace rm_chassis_controllers { @@ -108,9 +109,9 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan // double b_4_0 = (l_b - l_p) / (l_p * m_c); // double b_4_1 = // l_b * l_b / (l_p * l_p * m_p) + l_b * l_b / (l_p * l_p * m_c) - 2.0 * l_b / (l_p * m_c) + 1 / m_c + 1 / m_b; - a_ << 0., 0., 0., 1., 0., 0., 0., 0., 0., 1., -1.6066, -2.4065, 0., 0., 0., 25.0596, 106.6442, 0., 0., 0., 7.9258, - -12.4064, 0., 0., 0.; - b_ << 0., 0., 0., 0., 0.6148, -0.0419, -6.9291, -0.6828, 0.3477, 0.6761; + a_ << 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., -1.6066, -2.4065, 0., 0., 0., 0., + 25.0596, 106.6442, 0., 0., 0., 0., 7.9258, -12.4064, 0., 0., 0.; + b_ << 0., 0., 0., 0., 0., 0., 0.6148, -0.0419, -6.9291, -0.6828, 0.3477, 0.6761; Lqr lqr(a_, b_, q_, r_); if (!lqr.computeK()) @@ -124,16 +125,40 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan model_states_sub_ = root_nh.subscribe("/gazebo/model_states", 1, &BalanceController::modelStatesCallBack, this); + state_pub_ = root_nh.advertise("/state", 10); return true; } void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period) { - x_[1] = momentum_block_joint_handle_.getPosition(); - x_[2] -= vel_cmd_.x; - x_[4] = momentum_block_joint_handle_.getVelocity(); + position_des_ += vel_cmd_.x * period.toSec(); + geometry_msgs::Quaternion quaternion; + quaternion.x = imu_handle_.getOrientation()[0]; + quaternion.y = imu_handle_.getOrientation()[1]; + quaternion.z = imu_handle_.getOrientation()[2]; + quaternion.w = imu_handle_.getOrientation()[3]; + double roll, pitch, yaw; + quatToRPY(quaternion, roll, pitch, yaw); + x_[0] += x_[3] * period.toSec(); + x_[1] = pitch; + x_[2] = momentum_block_joint_handle_.getPosition(); + x_[4] = imu_handle_.getAngularVelocity()[1]; + x_[5] = momentum_block_joint_handle_.getVelocity(); Eigen::Matrix u; - u = k_ * (-x_); + auto x = x_; + x(0) -= position_des_; + x(3) -= vel_cmd_.x; + if (x(0) > 1) + x(0) = 1; + u = k_ * (-x); + std_msgs::Float64MultiArray state; + for (int i = 0; i < 6; i++) + { + state.data.push_back(x(i)); + } + state.data.push_back(u(0)); + state.data.push_back(u(1)); + state_pub_.publish(state); right_wheel_joint_handle_.setCommand(u(0) / 2); left_wheel_joint_handle_.setCommand(u(0) / 2); @@ -142,12 +167,8 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe void BalanceController::modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg) { - double roll, pitch, yaw; - quatToRPY(msg->pose[1].orientation, roll, pitch, yaw); - x_[0] = pitch; - x_[2] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 > 0 ? 1 : -1) * + x_[3] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 > 0 ? 1 : -1) * sqrt(pow(msg->twist[1].linear.x, 2) + pow(msg->twist[1].linear.y, 2)); - x_[3] = msg->twist[1].angular.y; } geometry_msgs::Twist BalanceController::odometry() { diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml index 170d9ecd..ddf62e07 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/template_reaction_wheel.yaml @@ -23,5 +23,5 @@ controllers: g: 9.81 wheel_radius: 0.125 - q: [ 1000, 1, 2000, 1000, 1 ] + q: [ 800, 1000, 1, 2000, 1000, 1 ] r: [ 1,10 ] From a9a925065029015492f3a4adc8a975443f6393ae Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 24 Nov 2022 23:17:43 +0800 Subject: [PATCH 028/130] 0.1.8 --- gpio_controller/CHANGELOG.rst | 6 ++++++ mimic_joint_controller/CHANGELOG.rst | 6 ++++++ rm_calibration_controllers/CHANGELOG.rst | 10 ++++++++++ rm_chassis_controllers/CHANGELOG.rst | 15 +++++++++++++++ rm_controllers/CHANGELOG.rst | 6 ++++++ rm_gimbal_controllers/CHANGELOG.rst | 15 +++++++++++++++ rm_orientation_controller/CHANGELOG.rst | 13 +++++++++++++ rm_shooter_controllers/CHANGELOG.rst | 19 +++++++++++++++++++ robot_state_controller/CHANGELOG.rst | 9 +++++++++ tof_radar_controller/CHANGELOG.rst | 10 ++++++++++ 10 files changed, 109 insertions(+) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 4ead8d5d..6d445a1b 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: ye-luo-xi-tui + 0.1.7 (2022-09-10) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index bd316593..bf5e89d4 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: ye-luo-xi-tui + 0.1.7 (2022-09-10) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 274d62bb..5bf1cf2d 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into target_velocity_correction +* Merge pull request `#87 `_ from NaHCO3bc/Readme + Fix the dependence bug in rm_calibration_controllers. +* Fix the dependence bug in rm_calibration_controllers. +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: NaHCO3bc, ye-luo-xi-tui, yezi + 0.1.7 (2022-09-10) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 1e4fe588..03316907 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into target_velocity_correction +* Merge pull request `#89 `_ from rm-controls/dev + Merge branch 'dev' into master +* Merge pull request `#88 `_ from NaHCO3bc/dev + Add a push_back of joint_handles in the new OmniController. +* Add a push_back of joint_handles in the new OmniController. +* Merge pull request `#86 `_ from NaHCO3bc/Readme + Fix the dependence part bug. +* Fix the dependence part bug. +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: NaHCO3bc, ye-luo-xi-tui, yezi + 0.1.7 (2022-09-10) ------------------ * Merge pull request `#83 `_ from rm-controls/dev diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index 739eb26f..b08beea0 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#85 `_ from ye-luo-xi-tui/master + 0.1.7 +* Contributors: ye-luo-xi-tui + 0.1.7 (2022-09-10) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 764f99dc..81d89574 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#92 `_ from ye-luo-xi-tui/target_velocity_correction + Estimate chassis vel with moving average and subtract chassis_vel from target_vel +* Fix trylock bug. +* Subtract chassis_vel from target_vel. +* Estimate chassis vel with moving average. +* 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, yezi + 0.1.7 (2022-09-10) ------------------ * Optimize TRACK mode of rm_gimbal_controller. diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 1c5c8b1b..32d7fc94 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#91 `_ from ye-luo-xi-tui/imu_offline + Dealing with the situation of imu offline. +* Dealing with the situation of imu offline. +* 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, yezi + 0.1.7 (2022-09-10) ------------------ * Optimize rm_orientation_controller. diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index d6c36a0a..af9cc042 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index f79e2c65..fd810f15 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#86 `_ from NaHCO3bc/Readme + Fix the dependence part bug. +* 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) ------------------ * Merge remote-tracking branch 'origin/master' diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 8b0bebf6..046490a6 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------ * Merge remote-tracking branch 'origin/master' From c7b41784a185ae53c761de68cf18c42d92d03a05 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 24 Nov 2022 23:18:13 +0800 Subject: [PATCH 029/130] 0.1.8 --- gpio_controller/CHANGELOG.rst | 4 ++-- gpio_controller/package.xml | 2 +- mimic_joint_controller/CHANGELOG.rst | 4 ++-- mimic_joint_controller/package.xml | 2 +- rm_calibration_controllers/CHANGELOG.rst | 4 ++-- rm_calibration_controllers/package.xml | 2 +- rm_chassis_controllers/CHANGELOG.rst | 4 ++-- rm_chassis_controllers/package.xml | 2 +- rm_controllers/CHANGELOG.rst | 4 ++-- rm_controllers/package.xml | 2 +- rm_gimbal_controllers/CHANGELOG.rst | 4 ++-- rm_gimbal_controllers/package.xml | 2 +- rm_orientation_controller/CHANGELOG.rst | 4 ++-- rm_orientation_controller/package.xml | 2 +- rm_shooter_controllers/CHANGELOG.rst | 4 ++-- rm_shooter_controllers/package.xml | 2 +- robot_state_controller/CHANGELOG.rst | 4 ++-- robot_state_controller/package.xml | 2 +- tof_radar_controller/CHANGELOG.rst | 4 ++-- tof_radar_controller/package.xml | 2 +- 20 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 6d445a1b..62f41add 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master 0.1.7 * Contributors: ye-luo-xi-tui diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index 0fe29d01..acb17bed 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -1,7 +1,7 @@ gpio_controller - 0.1.7 + 0.1.8 The gpio_controller package muyuexin diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index bf5e89d4..b95df227 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master 0.1.7 * Contributors: ye-luo-xi-tui diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 20993de8..141dd696 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -1,7 +1,7 @@ mimic_joint_controller - 0.1.7 + 0.1.8 The mimic_joint_controller package ljq diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 5bf1cf2d..62ce9044 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge branch 'master' into target_velocity_correction * Merge pull request `#87 `_ from NaHCO3bc/Readme Fix the dependence bug in rm_calibration_controllers. diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 262c3c7f..78125c9b 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -1,7 +1,7 @@ rm_calibration_controllers - 0.1.7 + 0.1.8 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 03316907..219133af 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge branch 'master' into target_velocity_correction * Merge pull request `#89 `_ from rm-controls/dev Merge branch 'dev' into master diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 1a659f13..d2bff7ab 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -1,7 +1,7 @@ rm_chassis_controllers - 0.1.7 + 0.1.8 RoboMaster standard robot Chassis controller Qiayuan Liao BSD diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index b08beea0..4f86c071 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master 0.1.7 * Contributors: ye-luo-xi-tui diff --git a/rm_controllers/package.xml b/rm_controllers/package.xml index afe591b3..dbe5f065 100644 --- a/rm_controllers/package.xml +++ b/rm_controllers/package.xml @@ -1,7 +1,7 @@ rm_controllers - 0.1.7 + 0.1.8 Meta package that contains package for RoboMaster. Qiayuan Liao diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 81d89574..f9a9c58b 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#92 `_ from ye-luo-xi-tui/target_velocity_correction Estimate chassis vel with moving average and subtract chassis_vel from target_vel * Fix trylock bug. diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index 0fc11d24..ff1b5155 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -1,7 +1,7 @@ rm_gimbal_controllers - 0.1.7 + 0.1.8 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 32d7fc94..49e7bc60 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#91 `_ from ye-luo-xi-tui/imu_offline Dealing with the situation of imu offline. * Dealing with the situation of imu offline. diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index 36eb3944..4ac9774e 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -1,7 +1,7 @@ rm_orientation_controller - 0.1.7 + 0.1.8 RoboMaster standard robot orientation controller Qiayuan Liao BSD diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index af9cc042..1084b2b5 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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. diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index 688fc7ea..49ef1b4b 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -1,7 +1,7 @@ rm_shooter_controllers - 0.1.7 + 0.1.8 RoboMaster standard robot Shooter controller Qiayuan Liao BSD diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index fd810f15..1e65c265 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#86 `_ from NaHCO3bc/Readme Fix the dependence part bug. * Fix the dependence part bug. diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index 0d4fcd8f..7c577b09 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -1,7 +1,7 @@ robot_state_controller - 0.1.7 + 0.1.8 A template for ROS packages. Qiayuan Liao BSD diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 046490a6..8176479a 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.8 (2022-11-24) +------------------ * Merge pull request `#86 `_ from NaHCO3bc/Readme Fix the dependence part bug. * Modify the test file folder. diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index 8e366e81..fdecfad4 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -1,7 +1,7 @@ tof_radar_controller - 0.1.7 + 0.1.8 The tof radar controller package luotinkai From 76c40c55bc44514b8962e4ab6ed1ed934eb3ba99 Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Sun, 27 Nov 2022 15:20:10 +0800 Subject: [PATCH 030/130] delete useless target_velocity_. --- .../joint_calibration_controller.h | 2 +- .../src/joint_calibration_controller.cpp | 7 +------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 5e47f0ef..5cf4cd4a 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -107,7 +107,7 @@ class JointCalibrationController CALIBRATED }; int state_{}, countdown_{}; - double velocity_search_{}, target_position_{}, target_velocity_{}, velocity_threshold_{}, position_threshold_{}; + double velocity_search_{}, target_position_{}, velocity_threshold_{}, position_threshold_{}; double positive_position_{}, negative_position_{}; bool is_return_{}, is_center_{}, returned_{}; rm_control::ActuatorExtraHandle actuator_; diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index ec36a52f..4f9e7656 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -79,11 +79,6 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); return false; } - if (!nh_return.getParam("target_velocity", target_velocity_)) - { - ROS_ERROR("Velocity value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); - return false; - } if (!controller_nh.getParam("threshold", position_threshold_)) { ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); @@ -183,7 +178,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati { if ((std::abs(position_ctrl_.joint_.getPosition()) - target_position_) < position_threshold_) returned_ = true; - position_ctrl_.setCommand(target_position_, target_velocity_); + position_ctrl_.setCommand(target_position_); position_ctrl_.update(time, period); } else From fc6e4dbc2abefc90b01b545d87903e071df82df5 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 28 Nov 2022 21:44:55 +0800 Subject: [PATCH 031/130] Revised dynamics model and remove gazebo dependency. --- rm_chassis_controllers/CMakeLists.txt | 1 + .../include/rm_chassis_controllers/balance.h | 8 +-- rm_chassis_controllers/package.xml | 1 + rm_chassis_controllers/src/balance.cpp | 58 +++++++++++-------- .../test/template_reaction_wheel.yaml | 4 +- 5 files changed, 40 insertions(+), 32 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index f9823864..4f4489e0 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED effort_controllers tf2_geometry_msgs gazebo_msgs + angles ) find_package(Eigen3 REQUIRED) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 4b477014..b89b30df 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -8,7 +8,6 @@ #include #include #include -#include #include "rm_chassis_controllers/chassis_base.h" @@ -25,8 +24,8 @@ class BalanceController : public ChassisBase k_{}; Eigen::Matrix a_{}, q_{}; Eigen::Matrix b_{}; @@ -34,12 +33,11 @@ class BalanceController : public ChassisBase x_; double wheel_radius_; double position_des_ = 0; + double yaw_des_ = 0; hardware_interface::ImuSensorHandle imu_handle_; hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_, momentum_block_joint_handle_; - ros::Subscriber model_states_sub_; ros::Publisher state_pub_; - void modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg); }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 371e64d7..3038bc6d 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -20,6 +20,7 @@ effort_controllers tf2_geometry_msgs gazebo_msgs + angles robot_localization diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 5a6c3e5f..2e043af4 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace rm_chassis_controllers { @@ -109,9 +110,13 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan // double b_4_0 = (l_b - l_p) / (l_p * m_c); // double b_4_1 = // l_b * l_b / (l_p * l_p * m_p) + l_b * l_b / (l_p * l_p * m_c) - 2.0 * l_b / (l_p * m_c) + 1 / m_c + 1 / m_b; - a_ << 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., -1.6066, -2.4065, 0., 0., 0., 0., - 25.0596, 106.6442, 0., 0., 0., 0., 7.9258, -12.4064, 0., 0., 0.; - b_ << 0., 0., 0., 0., 0., 0., 0.6148, -0.0419, -6.9291, -0.6828, 0.3477, 0.6761; + a_ << 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., + 0., 0., 0., 0., 1.0, 0., 0., -1.60656831231196, -2.40650017923402, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0., 25.0595846927374, 106.644165861824, 0., 0., 0., 0., 0., 0., 7.92579199849074, -12.4063744589733, 0., 0., + 0., 0.; + b_ << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.614753821325744, 0.614753821325744, 0.0132316980332364, + -1.65961661528494, 1.65961661528494, 0., -6.929097995272, -6.929097995272, -0.586363305461782, 0.347697890217537, + 0.347697890217537, 0.607588490877498; Lqr lqr(a_, b_, q_, r_); if (!lqr.computeK()) @@ -123,15 +128,12 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan k_ = lqr.getK(); ROS_INFO_STREAM("K of LQR:" << k_); - model_states_sub_ = root_nh.subscribe("/gazebo/model_states", 1, - &BalanceController::modelStatesCallBack, this); state_pub_ = root_nh.advertise("/state", 10); return true; } void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period) { - position_des_ += vel_cmd_.x * period.toSec(); geometry_msgs::Quaternion quaternion; quaternion.x = imu_handle_.getOrientation()[0]; quaternion.y = imu_handle_.getOrientation()[1]; @@ -139,37 +141,43 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe quaternion.w = imu_handle_.getOrientation()[3]; double roll, pitch, yaw; quatToRPY(quaternion, roll, pitch, yaw); - x_[0] += x_[3] * period.toSec(); - x_[1] = pitch; - x_[2] = momentum_block_joint_handle_.getPosition(); - x_[4] = imu_handle_.getAngularVelocity()[1]; - x_[5] = momentum_block_joint_handle_.getVelocity(); + x_[4] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - + imu_handle_.getAngularVelocity()[1]) * + 0.125; + x_[0] += x_[4] * period.toSec(); + x_[1] = yaw; + x_[2] = pitch; + x_[3] = momentum_block_joint_handle_.getPosition(); + x_[5] = imu_handle_.getAngularVelocity()[2]; + x_[6] = imu_handle_.getAngularVelocity()[1]; + x_[7] = momentum_block_joint_handle_.getVelocity(); + if (vel_cmd_.z != 0.) + yaw_des_ = x_[1] + vel_cmd_.z * period.toSec(); + if (vel_cmd_.x != 0.) + position_des_ = x_[0] + vel_cmd_.x * period.toSec(); Eigen::Matrix u; auto x = x_; x(0) -= position_des_; - x(3) -= vel_cmd_.x; - if (x(0) > 1) - x(0) = 1; + x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); + x(4) -= vel_cmd_.x; + x(5) -= vel_cmd_.z; u = k_ * (-x); std_msgs::Float64MultiArray state; - for (int i = 0; i < 6; i++) + for (int i = 0; i < 8; i++) { state.data.push_back(x(i)); } - state.data.push_back(u(0)); - state.data.push_back(u(1)); + for (int i = 0; i < 3; i++) + { + state.data.push_back(u(i)); + } state_pub_.publish(state); - right_wheel_joint_handle_.setCommand(u(0) / 2); - left_wheel_joint_handle_.setCommand(u(0) / 2); - momentum_block_joint_handle_.setCommand(u(1)); + left_wheel_joint_handle_.setCommand(u(0)); + right_wheel_joint_handle_.setCommand(u(1)); + momentum_block_joint_handle_.setCommand(u(2)); } -void BalanceController::modelStatesCallBack(const gazebo_msgs::ModelStates::ConstPtr& msg) -{ - x_[3] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 > 0 ? 1 : -1) * - sqrt(pow(msg->twist[1].linear.x, 2) + pow(msg->twist[1].linear.y, 2)); -} geometry_msgs::Twist BalanceController::odometry() { geometry_msgs::Twist twist; diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml index ddf62e07..fc62c612 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/template_reaction_wheel.yaml @@ -23,5 +23,5 @@ controllers: g: 9.81 wheel_radius: 0.125 - q: [ 800, 1000, 1, 2000, 1000, 1 ] - r: [ 1,10 ] + q: [ 0.5, 0.5, 40, 51, 4, 0.2, 0.1, 1 ] + r: [ 0.01, 0.01, 0.0001 ] From 54c8cd60a71940ad991e777ab203535bb7aa0679 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Wed, 30 Nov 2022 00:11:17 +0800 Subject: [PATCH 032/130] Try to use PID controller to fix the problem that one block's position is differ from another. --- .../include/rm_chassis_controllers/balance.h | 5 ++++- rm_chassis_controllers/src/balance.cpp | 17 ++++++++++++----- .../test/template_reaction_wheel.yaml | 1 + 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index b89b30df..20eeb1d2 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -36,7 +37,9 @@ class BalanceController : public ChassisBaseget()->getHandle("left_wheel_joint"); right_wheel_joint_handle_ = robot_hw->get()->getHandle("right_wheel_joint"); - momentum_block_joint_handle_ = - robot_hw->get()->getHandle("momentum_block_joint"); + left_momentum_block_joint_handle_ = + robot_hw->get()->getHandle("left_momentum_block_joint"); + right_momentum_block_joint_handle_ = + robot_hw->get()->getHandle("right_momentum_block_joint"); + if (!pid_controller_.init(ros::NodeHandle(controller_nh, "pid_block_error"))) + return false; // i_b is moment of inertia of the pendulum body around the pivot point, // i_w is the moment of inertia of the wheel around the rotational axis of the motor // l is the distance between the motor axis and the pivot point @@ -147,10 +151,10 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe x_[0] += x_[4] * period.toSec(); x_[1] = yaw; x_[2] = pitch; - x_[3] = momentum_block_joint_handle_.getPosition(); + x_[3] = left_momentum_block_joint_handle_.getPosition(); x_[5] = imu_handle_.getAngularVelocity()[2]; x_[6] = imu_handle_.getAngularVelocity()[1]; - x_[7] = momentum_block_joint_handle_.getVelocity(); + x_[7] = left_momentum_block_joint_handle_.getVelocity(); if (vel_cmd_.z != 0.) yaw_des_ = x_[1] + vel_cmd_.z * period.toSec(); if (vel_cmd_.x != 0.) @@ -175,7 +179,10 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe left_wheel_joint_handle_.setCommand(u(0)); right_wheel_joint_handle_.setCommand(u(1)); - momentum_block_joint_handle_.setCommand(u(2)); + left_momentum_block_joint_handle_.setCommand(u(2) / 2); + double error = left_momentum_block_joint_handle_.getPosition() - right_momentum_block_joint_handle_.getPosition(); + double commanded_effort = pid_controller_.computeCommand(error, period); + right_momentum_block_joint_handle_.setCommand(u(2) / 2 + commanded_effort); } geometry_msgs::Twist BalanceController::odometry() diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml index fc62c612..74554f85 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/template_reaction_wheel.yaml @@ -10,6 +10,7 @@ controllers: twist_angular: 0.5233 timeout: 0.1 pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid_block_error: { p: 0, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] imu_name: "base_imu" From ca6d165d0cc78627816a6d6402f3baa70ed0f283 Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Wed, 30 Nov 2022 21:06:46 +0800 Subject: [PATCH 033/130] Delete repetitive set. --- rm_calibration_controllers/src/joint_calibration_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index 4f9e7656..68a2dc02 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -178,7 +178,6 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati { if ((std::abs(position_ctrl_.joint_.getPosition()) - target_position_) < position_threshold_) returned_ = true; - position_ctrl_.setCommand(target_position_); position_ctrl_.update(time, period); } else From 1a7d1d161f397726eab0f34c87def8859b8b7b53 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 1 Dec 2022 21:40:40 +0800 Subject: [PATCH 034/130] Transform IMU's data to base_link. --- rm_chassis_controllers/src/balance.cpp | 47 +++++++++++++++++++++----- 1 file changed, 39 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 08897b45..c47e04ee 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -138,13 +139,43 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Quaternion quaternion; - quaternion.x = imu_handle_.getOrientation()[0]; - quaternion.y = imu_handle_.getOrientation()[1]; - quaternion.z = imu_handle_.getOrientation()[2]; - quaternion.w = imu_handle_.getOrientation()[3]; + geometry_msgs::Vector3 gyro, angular_vel_base; + gyro.x = imu_handle_.getAngularVelocity()[0]; + gyro.y = imu_handle_.getAngularVelocity()[1]; + gyro.z = imu_handle_.getAngularVelocity()[2]; + try + { + tf2::doTransform(gyro, angular_vel_base, + robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), time)); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + tf2::Transform odom2imu, imu2base, odom2base; + try + { + geometry_msgs::TransformStamped tf_msg; + tf_msg = robot_state_handle_.lookupTransform(imu_handle_.getFrameId(), "base_link", time); + tf2::fromMsg(tf_msg.transform, imu2base); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + left_wheel_joint_handle_.setCommand(0.); + right_wheel_joint_handle_.setCommand(0.); + left_momentum_block_joint_handle_.setCommand(0.); + right_momentum_block_joint_handle_.setCommand(0.); + return; + } + tf2::Quaternion odom2imu_quaternion; + odom2imu_quaternion.setValue(imu_handle_.getOrientation()[0], imu_handle_.getOrientation()[1], + imu_handle_.getOrientation()[2], imu_handle_.getOrientation()[3]); + odom2imu.setRotation(odom2imu_quaternion); + odom2base = odom2imu * imu2base; double roll, pitch, yaw; - quatToRPY(quaternion, roll, pitch, yaw); + quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); x_[4] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - imu_handle_.getAngularVelocity()[1]) * 0.125; @@ -152,8 +183,8 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe x_[1] = yaw; x_[2] = pitch; x_[3] = left_momentum_block_joint_handle_.getPosition(); - x_[5] = imu_handle_.getAngularVelocity()[2]; - x_[6] = imu_handle_.getAngularVelocity()[1]; + x_[5] = angular_vel_base.z; + x_[6] = angular_vel_base.y; x_[7] = left_momentum_block_joint_handle_.getVelocity(); if (vel_cmd_.z != 0.) yaw_des_ = x_[1] + vel_cmd_.z * period.toSec(); From cafef8d7de68433a587f4cc32a525fe781fdedd1 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 1 Dec 2022 23:19:04 +0800 Subject: [PATCH 035/130] Add balance_chassis's odometry. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 10 ++++++++++ .../test/template_reaction_wheel.yaml | 1 + 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 20eeb1d2..ff2ab1c9 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -32,7 +32,7 @@ class BalanceController : public ChassisBase b_{}; Eigen::Matrix r_{}; Eigen::Matrix x_; - double wheel_radius_; + double wheel_radius_, wheel_base_; double position_des_ = 0; double yaw_des_ = 0; diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index c47e04ee..533ddbd2 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -69,6 +69,12 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan if (!controller_nh.getParam("wheel_radius", wheel_radius_)) { ROS_ERROR("Params wheel_radius doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("wheel_base", wheel_base_)) + { + ROS_ERROR("Params wheel_base doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; } q_.setZero(); @@ -219,6 +225,10 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe geometry_msgs::Twist BalanceController::odometry() { geometry_msgs::Twist twist; + twist.linear.x = + (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) * wheel_radius_ / 2; + twist.angular.z = + (right_wheel_joint_handle_.getVelocity() - left_wheel_joint_handle_.getVelocity()) * wheel_radius_ / wheel_base_; return twist; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/template_reaction_wheel.yaml index 74554f85..9dc3ef51 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/template_reaction_wheel.yaml @@ -23,6 +23,7 @@ controllers: l_b: 0.1389 g: 9.81 wheel_radius: 0.125 + wheel_base: 0.42 q: [ 0.5, 0.5, 40, 51, 4, 0.2, 0.1, 1 ] r: [ 0.01, 0.01, 0.0001 ] From 1bfcaeb62c9296a09a228321dd5709c20f53dc25 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 6 Dec 2022 03:30:34 +0800 Subject: [PATCH 036/130] Compute state matrix and control matrix with given params. --- rm_chassis_controllers/CMakeLists.txt | 2 - .../include/rm_chassis_controllers/balance.h | 6 +- rm_chassis_controllers/package.xml | 1 - rm_chassis_controllers/src/balance.cpp | 246 +++++++++++++----- 4 files changed, 187 insertions(+), 68 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 4f4489e0..08976ce7 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(catkin REQUIRED controller_interface effort_controllers tf2_geometry_msgs - gazebo_msgs angles ) @@ -46,7 +45,6 @@ catkin_package( effort_controllers tf2_geometry_msgs - gazebo_msgs LIBRARIES ${PROJECT_NAME} ) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index ff2ab1c9..51a0f85c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -8,7 +8,6 @@ #include #include #include -#include #include "rm_chassis_controllers/chassis_base.h" @@ -25,8 +24,8 @@ class BalanceController : public ChassisBase k_{}; Eigen::Matrix a_{}, q_{}; Eigen::Matrix b_{}; @@ -39,7 +38,6 @@ class BalanceController : public ChassisBasecontroller_interface effort_controllers tf2_geometry_msgs - gazebo_msgs angles robot_localization diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 533ddbd2..9f6b2131 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include namespace rm_chassis_controllers @@ -28,37 +28,45 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan right_momentum_block_joint_handle_ = robot_hw->get()->getHandle("right_momentum_block_joint"); - if (!pid_controller_.init(ros::NodeHandle(controller_nh, "pid_block_error"))) - return false; // i_b is moment of inertia of the pendulum body around the pivot point, // i_w is the moment of inertia of the wheel around the rotational axis of the motor // l is the distance between the motor axis and the pivot point // l_b is the distance between the center of mass of the pendulum body and the pivot point - double m_b, m_p, m_c, l_p, l_b, g; + double m_w, m, m_b, i_w, l, y_b, z_b, g, i_m; + if (!controller_nh.getParam("m_w", m_w)) + { + ROS_ERROR("Params m_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("m", m)) + { + ROS_ERROR("Params m doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } if (!controller_nh.getParam("m_b", m_b)) { ROS_ERROR("Params m_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("m_p", m_p)) + if (!controller_nh.getParam("i_w", i_w)) { - ROS_ERROR("Params m_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Params i_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("m_c", m_c)) + if (!controller_nh.getParam("l", l)) { - ROS_ERROR("Params i_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Params l doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("l_p", l_p)) + if (!controller_nh.getParam("y_b", y_b)) { - ROS_ERROR("Params i_w doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Params y_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("l_b", l_b)) + if (!controller_nh.getParam("z_b", z_b)) { - ROS_ERROR("Params l_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Params z_b doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } if (!controller_nh.getParam("g", g)) @@ -66,6 +74,11 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ROS_ERROR("Params g doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } + if (!controller_nh.getParam("i_m", i_m)) + { + ROS_ERROR("Params i_m doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } if (!controller_nh.getParam("wheel_radius", wheel_radius_)) { ROS_ERROR("Params wheel_radius doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); @@ -73,7 +86,7 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } if (!controller_nh.getParam("wheel_base", wheel_base_)) { - ROS_ERROR("Params wheel_base doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Params wheel_base_ doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } @@ -106,29 +119,135 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } // Continuous model \dot{x} = A x + B u - // double m = m_b + m_c + m_p; - // double a_2_0 = -g * (m_b + m_p) / m_c; - // double a_2_1 = -g * m_b / (l_p * m_c); - // double a_3_0 = m * g / (l_p * m_c); - // double a_3_1 = m_b * g * (m_c + m_p) / (l_p * l_p * m_c * m_p); - // double a_4_0 = m * g * (l_p - l_b) / (m_c * l_p); - // double a_4_1 = g * m_b * (-l_b * m_c - l_b * m_p + l_p * m_p) / (l_p * l_p * m_c * m_p); - // - // double b_2_0 = 1 / m_c; - // double b_2_1 = (l_b - l_p) / (l_p * m_c); - // double b_3_0 = -1 / (l_p * m_c); - // double b_3_1 = (-l_b * m_c - l_b * m_p + l_p * m_p) / (l_p * l_p * m_c * m_p); - // double b_4_0 = (l_b - l_p) / (l_p * m_c); - // double b_4_1 = - // l_b * l_b / (l_p * l_p * m_p) + l_b * l_b / (l_p * l_p * m_c) - 2.0 * l_b / (l_p * m_c) + 1 / m_c + 1 / m_b; - a_ << 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., - 0., 0., 0., 0., 1.0, 0., 0., -1.60656831231196, -2.40650017923402, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., - 0., 0., 25.0595846927374, 106.644165861824, 0., 0., 0., 0., 0., 0., 7.92579199849074, -12.4063744589733, 0., 0., - 0., 0.; - b_ << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.614753821325744, 0.614753821325744, 0.0132316980332364, - -1.65961661528494, 1.65961661528494, 0., -6.929097995272, -6.929097995272, -0.586363305461782, 0.347697890217537, - 0.347697890217537, 0.607588490877498; + double a_5_2 = -(pow(wheel_radius_, 2) * g * (pow(l, 2) * pow(m, 2) + 2 * m_b * pow(l, 2) * m + 2 * i_m * m_b)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_5_3 = -(pow(wheel_radius_, 2) * g * l * m * m_b) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_5_4 = a_5_3; + double a_7_2 = + (g * l * m * + (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_b + 2 * pow(wheel_radius_, 2) * m_w)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_7_3 = (g * m_b * (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_7_4 = a_7_3; + double a_8_2 = + (g * (i_m - l * m * z_b) * + (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_b + 2 * pow(wheel_radius_, 2) * m_w)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_8_3 = -(g * m_b * + (2 * i_w * l + 2 * i_w * z_b + 2 * pow(wheel_radius_, 2) * l * m_w + + pow(wheel_radius_, 2) * m * z_b + 2 * pow(wheel_radius_, 2) * m_w * z_b)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double a_8_4 = a_8_3; + double a_9_2 = a_8_2; + double a_9_3 = a_8_3; + double a_9_4 = a_8_4; + + double b_5_0 = (wheel_radius_ * (m * pow(l, 2) + wheel_radius_ * m * l + i_m)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double b_5_1 = b_5_0; + double b_5_2 = (pow(wheel_radius_, 2) * l * m * z_b) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double b_5_3 = b_5_2; + double b_6_0 = -wheel_radius_ / (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w)); + double b_6_1 = -b_6_0; + double b_6_2 = (2 * pow(wheel_radius_, 2) * y_b) / (pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w)); + double b_6_3 = -b_6_2; + double b_7_0 = -(2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w + wheel_radius_ * l * m) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double b_7_1 = b_7_0; + double b_7_2 = -(z_b * (2 * i_w + pow(wheel_radius_, 2) * m + 2 * pow(wheel_radius_, 2) * m_w)) / + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w); + double b_7_3 = b_7_2; + double b_9_0 = + (2 * pow(i_w, 2) * l * wheel_base_ + 2 * pow(i_w, 2) * wheel_base_ * z_b - + 4 * pow(wheel_radius_, 3) * i_m * m_w * wheel_base_ + pow(wheel_radius_, 3) * i_m * m * y_b + + 2 * pow(wheel_radius_, 3) * i_m * m_w * y_b + 8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * wheel_base_ + + 8 * pow(wheel_radius_, 4) * pow(m_w, 2) * wheel_base_ * z_b - wheel_radius_ * i_m * i_w * wheel_base_ + + 2 * wheel_radius_ * i_m * i_w * y_b + 2 * pow(wheel_radius_, 3) * pow(l, 2) * m * m_w * y_b + + 10 * pow(wheel_radius_, 2) * i_w * l * m_w * wheel_base_ + 2 * wheel_radius_ * i_w * pow(l, 2) * m * y_b + + pow(wheel_radius_, 2) * i_w * m * wheel_base_ * z_b + 10 * pow(wheel_radius_, 2) * i_w * m_w * wheel_base_ * z_b + + 4 * pow(wheel_radius_, 4) * m * m_w * wheel_base_ * z_b + + 4 * pow(wheel_radius_, 3) * l * m * m_w * wheel_base_ * z_b + wheel_radius_ * i_w * l * m * wheel_base_ * z_b) / + (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w) * + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w)); + double b_9_1 = + (2 * pow(i_w, 2) * l * wheel_base_ + 2 * pow(i_w, 2) * wheel_base_ * z_b - + 4 * pow(wheel_radius_, 3) * i_m * m_w * wheel_base_ - pow(wheel_radius_, 3) * i_m * m * y_b - + 2 * pow(wheel_radius_, 3) * i_m * m_w * y_b + 8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * wheel_base_ + + 8 * pow(wheel_radius_, 4) * pow(m_w, 2) * wheel_base_ * z_b - wheel_radius_ * i_m * i_w * wheel_base_ - + 2 * wheel_radius_ * i_m * i_w * y_b - 2 * pow(wheel_radius_, 3) * pow(l, 2) * m * m_w * y_b + + 10 * pow(wheel_radius_, 2) * i_w * l * m_w * wheel_base_ - 2 * wheel_radius_ * i_w * pow(l, 2) * m * y_b + + pow(wheel_radius_, 2) * i_w * m * wheel_base_ * z_b + 10 * pow(wheel_radius_, 2) * i_w * m_w * wheel_base_ * z_b + + 4 * pow(wheel_radius_, 4) * m * m_w * wheel_base_ * z_b + + 4 * pow(wheel_radius_, 3) * l * m * m_w * wheel_base_ * z_b + wheel_radius_ * i_w * l * m * wheel_base_ * z_b) / + (wheel_base_ * (4 * m_w * pow(wheel_radius_, 2) + i_w) * + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w)); + double b_9_2 = + (-4 * m * pow(wheel_radius_, 4) * pow(l, 2) * m_w * pow(y_b, 2) + + 8 * pow(wheel_radius_, 4) * l * pow(m_w, 2) * pow(wheel_base_, 2) * z_b + + 8 * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) + + 4 * m * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) * pow(z_b, 2) - + 4 * i_m * pow(wheel_radius_, 4) * m_w * pow(y_b, 2) - 2 * i_m * m * pow(wheel_radius_, 4) * pow(y_b, 2) - + 4 * m * pow(wheel_radius_, 2) * i_w * pow(l, 2) * pow(y_b, 2) + + 10 * pow(wheel_radius_, 2) * i_w * l * m_w * pow(wheel_base_, 2) * z_b + + 10 * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) * pow(z_b, 2) + + m * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) * pow(z_b, 2) - + 4 * i_m * pow(wheel_radius_, 2) * i_w * pow(y_b, 2) + 2 * pow(i_w, 2) * l * pow(wheel_base_, 2) * z_b + + 2 * pow(i_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2)) / + (pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w) * + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w)); + double b_9_3 = + (8 * m * pow(wheel_radius_, 4) * pow(l, 2) * pow(m_w, 2) * pow(wheel_base_, 2) + + 4 * m * m_b * pow(wheel_radius_, 4) * pow(l, 2) * m_w * pow(y_b, 2) + + 8 * m_b * pow(wheel_radius_, 4) * l * pow(m_w, 2) * pow(wheel_base_, 2) * z_b + + 8 * m_b * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) + + 8 * i_m * pow(wheel_radius_, 4) * pow(m_w, 2) * pow(wheel_base_, 2) + + 4 * m * m_b * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) * pow(z_b, 2) + + 4 * i_m * m * pow(wheel_radius_, 4) * m_w * pow(wheel_base_, 2) + + 4 * i_m * m_b * pow(wheel_radius_, 4) * m_w * pow(y_b, 2) + + 2 * i_m * m * m_b * pow(wheel_radius_, 4) * pow(y_b, 2) + + 10 * m * pow(wheel_radius_, 2) * i_w * pow(l, 2) * m_w * pow(wheel_base_, 2) + + 4 * m * m_b * pow(wheel_radius_, 2) * i_w * pow(l, 2) * pow(y_b, 2) + + 10 * m_b * pow(wheel_radius_, 2) * i_w * l * m_w * pow(wheel_base_, 2) * z_b + + 10 * m_b * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) * pow(z_b, 2) + + 10 * i_m * pow(wheel_radius_, 2) * i_w * m_w * pow(wheel_base_, 2) + + m * m_b * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) * pow(z_b, 2) + + i_m * m * pow(wheel_radius_, 2) * i_w * pow(wheel_base_, 2) + + 4 * i_m * m_b * pow(wheel_radius_, 2) * i_w * pow(y_b, 2) + + 2 * m * pow(i_w, 2) * pow(l, 2) * pow(wheel_base_, 2) + 2 * m_b * pow(i_w, 2) * l * pow(wheel_base_, 2) * z_b + + 2 * m_b * pow(i_w, 2) * pow(wheel_base_, 2) * pow(z_b, 2) + 2 * i_m * pow(i_w, 2) * pow(wheel_base_, 2)) / + (m_b * pow(wheel_base_, 2) * (4 * m_w * pow(wheel_radius_, 2) + i_w) * + (2 * i_m * i_w + 2 * i_w * pow(l, 2) * m + pow(wheel_radius_, 2) * i_m * m + + 2 * pow(wheel_radius_, 2) * i_m * m_w + 2 * pow(wheel_radius_, 2) * pow(l, 2) * m * m_w)); + double b_8_0 = b_9_1; + double b_8_1 = b_9_0; + double b_8_2 = b_9_3; + double b_8_3 = b_9_2; + a_ << 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1.0, 0., 0., a_5_2, + a_5_3, a_5_4, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., a_7_2, a_7_3, a_7_4, 0., 0., 0., + 0., 0., 0., 0., a_8_2, a_8_3, a_8_4, 0., 0., 0., 0., 0., 0., 0., a_9_2, a_9_3, a_9_4, 0., 0., 0., 0., 0.; + b_ << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., b_5_0, b_5_1, b_5_2, b_5_3, + b_6_0, b_6_1, b_6_2, b_6_3, b_7_0, b_7_1, b_7_2, b_7_3, b_8_0, b_8_1, b_8_2, b_8_3, b_9_0, b_9_1, b_9_2, b_9_3; + + ROS_INFO_STREAM("A:" << a_); + ROS_INFO_STREAM("B:" << b_); Lqr lqr(a_, b_, q_, r_); if (!lqr.computeK()) { @@ -139,7 +258,7 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan k_ = lqr.getK(); ROS_INFO_STREAM("K of LQR:" << k_); - state_pub_ = root_nh.advertise("/state", 10); + state_pub_ = root_nh.advertise("/state", 10); return true; } @@ -182,16 +301,18 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe odom2base = odom2imu * imu2base; double roll, pitch, yaw; quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); - x_[4] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - + x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - imu_handle_.getAngularVelocity()[1]) * - 0.125; - x_[0] += x_[4] * period.toSec(); + wheel_radius_; + x_[0] += x_[5] * period.toSec(); x_[1] = yaw; x_[2] = pitch; x_[3] = left_momentum_block_joint_handle_.getPosition(); - x_[5] = angular_vel_base.z; - x_[6] = angular_vel_base.y; - x_[7] = left_momentum_block_joint_handle_.getVelocity(); + x_[4] = right_momentum_block_joint_handle_.getPosition(); + x_[6] = angular_vel_base.z; + x_[7] = angular_vel_base.y; + x_[8] = left_momentum_block_joint_handle_.getVelocity(); + x_[9] = right_momentum_block_joint_handle_.getVelocity(); if (vel_cmd_.z != 0.) yaw_des_ = x_[1] + vel_cmd_.z * period.toSec(); if (vel_cmd_.x != 0.) @@ -200,35 +321,38 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe auto x = x_; x(0) -= position_des_; x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); - x(4) -= vel_cmd_.x; - x(5) -= vel_cmd_.z; + x(5) -= vel_cmd_.x; + x(6) -= vel_cmd_.z; u = k_ * (-x); - std_msgs::Float64MultiArray state; - for (int i = 0; i < 8; i++) - { - state.data.push_back(x(i)); - } - for (int i = 0; i < 3; i++) - { - state.data.push_back(u(i)); - } + rm_msgs::BalanceState state; + state.header.stamp = time; + state.x = x(0); + state.phi = x(1); + state.theta = x(2); + state.x_b_l = x(3); + state.x_b_r = x(4); + state.x_dot = x(5); + state.phi_dot = x(6); + state.theta_dot = x(7); + state.x_b_l_dot = x(8); + state.x_b_r_dot = x(9); + state.T_l = u(0); + state.T_r = u(1); + state.f_b_l = u(2); + state.f_b_r = u(3); state_pub_.publish(state); left_wheel_joint_handle_.setCommand(u(0)); right_wheel_joint_handle_.setCommand(u(1)); - left_momentum_block_joint_handle_.setCommand(u(2) / 2); - double error = left_momentum_block_joint_handle_.getPosition() - right_momentum_block_joint_handle_.getPosition(); - double commanded_effort = pid_controller_.computeCommand(error, period); - right_momentum_block_joint_handle_.setCommand(u(2) / 2 + commanded_effort); + left_momentum_block_joint_handle_.setCommand(u(2)); + right_momentum_block_joint_handle_.setCommand(u(3)); } geometry_msgs::Twist BalanceController::odometry() { geometry_msgs::Twist twist; - twist.linear.x = - (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) * wheel_radius_ / 2; - twist.angular.z = - (right_wheel_joint_handle_.getVelocity() - left_wheel_joint_handle_.getVelocity()) * wheel_radius_ / wheel_base_; + twist.linear.x = x_[5]; + twist.angular.z = x_[6]; return twist; } } // namespace rm_chassis_controllers From 3f7988713b07f726de01619cac650d370c23b099 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 1 Jan 2023 11:19:48 +0800 Subject: [PATCH 037/130] Add power limit. --- rm_chassis_controllers/src/balance.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 9f6b2131..bc988767 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -23,6 +23,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan getParam(controller_nh, "imu_name", std::string("base_imu"))); left_wheel_joint_handle_ = robot_hw->get()->getHandle("left_wheel_joint"); right_wheel_joint_handle_ = robot_hw->get()->getHandle("right_wheel_joint"); + joint_handles_.push_back(left_wheel_joint_handle_); + joint_handles_.push_back(right_wheel_joint_handle_); left_momentum_block_joint_handle_ = robot_hw->get()->getHandle("left_momentum_block_joint"); right_momentum_block_joint_handle_ = From d9014d869f99c09dd84b01da94e17d42a03cfdea Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 1 Jan 2023 11:24:29 +0800 Subject: [PATCH 038/130] ci:fix clang setup. --- .github/workflows/format.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index b3770ce9..0e1c4fe8 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -11,12 +11,14 @@ on: jobs: pre-commit: name: pre-commit - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 - uses: actions/setup-python@v2 - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 + run: | + wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - + sudo apt-get install clang-format-10 - name: Install catkin-lint run: | lsb_release -sc From 2eb05798f3b3e26e2eb9baf33028b619c30a62ac Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 2 Jan 2023 12:43:40 +0800 Subject: [PATCH 039/130] Fix warning. --- rm_chassis_controllers/src/balance.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index bc988767..3e525c99 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -297,8 +297,11 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe return; } tf2::Quaternion odom2imu_quaternion; + tf2::Vector3 odom2imu_origin; odom2imu_quaternion.setValue(imu_handle_.getOrientation()[0], imu_handle_.getOrientation()[1], imu_handle_.getOrientation()[2], imu_handle_.getOrientation()[3]); + odom2imu_origin.setValue(0, 0, 0); + odom2imu.setOrigin(odom2imu_origin); odom2imu.setRotation(odom2imu_quaternion); odom2base = odom2imu * imu2base; double roll, pitch, yaw; From f0f1380881a81c8f4cd031c37a8c91c62b315168 Mon Sep 17 00:00:00 2001 From: StarHeart Date: Mon, 2 Jan 2023 19:21:47 +0800 Subject: [PATCH 040/130] ci: lock ubuntu version to 20.04 --- .github/workflows/deb_package.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/deb_package.yml b/.github/workflows/deb_package.yml index 9455d5cc..fe6e48b8 100644 --- a/.github/workflows/deb_package.yml +++ b/.github/workflows/deb_package.yml @@ -9,7 +9,7 @@ on: workflow_dispatch: jobs: build: - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 - uses: actions/checkout@v2 From fe79d89a0caf80b0d594822154acd689a6a105d0 Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Fri, 6 Jan 2023 01:38:42 +0800 Subject: [PATCH 041/130] Add engineer trigger ui . --- .../src/joint_calibration_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index 68a2dc02..76a54a2c 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -129,7 +129,10 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); state_ = CALIBRATED; if (is_return_) - position_ctrl_.joint_.setCommand(target_position_); + { + position_ctrl_.setCommand(target_position_); + returned_ = true; + } else { velocity_ctrl_.joint_.setCommand(0.); From 37e06fc32418e5bc5531bbbca63092906286e1ea Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 7 Jan 2023 16:39:21 +0800 Subject: [PATCH 042/130] Take some joints' name as params. --- rm_chassis_controllers/src/balance.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 3e525c99..40bfd4d0 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -21,14 +21,23 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan imu_handle_ = robot_hw->get()->getHandle( getParam(controller_nh, "imu_name", std::string("base_imu"))); - left_wheel_joint_handle_ = robot_hw->get()->getHandle("left_wheel_joint"); - right_wheel_joint_handle_ = robot_hw->get()->getHandle("right_wheel_joint"); + std::string left_wheel_joint, right_wheel_joint, left_momentum_block_joint, right_momentum_block_joint; + if (!controller_nh.getParam("left/wheel_joint", left_wheel_joint) || + !controller_nh.getParam("left/block_joint", left_momentum_block_joint) || + !controller_nh.getParam("right/wheel_joint", right_wheel_joint) || + !controller_nh.getParam("right/block_joint", right_momentum_block_joint)) + { + ROS_ERROR("Some Joints' name doesn't given. (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + left_wheel_joint_handle_ = robot_hw->get()->getHandle(left_wheel_joint); + right_wheel_joint_handle_ = robot_hw->get()->getHandle(right_wheel_joint); joint_handles_.push_back(left_wheel_joint_handle_); joint_handles_.push_back(right_wheel_joint_handle_); left_momentum_block_joint_handle_ = - robot_hw->get()->getHandle("left_momentum_block_joint"); + robot_hw->get()->getHandle(left_momentum_block_joint); right_momentum_block_joint_handle_ = - robot_hw->get()->getHandle("right_momentum_block_joint"); + robot_hw->get()->getHandle(right_momentum_block_joint); // i_b is moment of inertia of the pendulum body around the pivot point, // i_w is the moment of inertia of the wheel around the rotational axis of the motor From 1335087653b735754e619e5deda1bf750eebbdc9 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 7 Jan 2023 18:21:15 +0800 Subject: [PATCH 043/130] Modify the expected location update policy. --- rm_chassis_controllers/src/balance.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 40bfd4d0..98ac046f 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -327,10 +327,8 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe x_[7] = angular_vel_base.y; x_[8] = left_momentum_block_joint_handle_.getVelocity(); x_[9] = right_momentum_block_joint_handle_.getVelocity(); - if (vel_cmd_.z != 0.) - yaw_des_ = x_[1] + vel_cmd_.z * period.toSec(); - if (vel_cmd_.x != 0.) - position_des_ = x_[0] + vel_cmd_.x * period.toSec(); + yaw_des_ += vel_cmd_.z * period.toSec(); + position_des_ += vel_cmd_.x * period.toSec(); Eigen::Matrix u; auto x = x_; x(0) -= position_des_; From 3febc608fa024ea6de92b0a7bf095c10f8c09d82 Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Mon, 9 Jan 2023 14:11:36 +0800 Subject: [PATCH 044/130] Fixed the bug of gimbal warning --- rm_gimbal_controllers/src/gimbal_base.cpp | 33 ++++++++++++----------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 62ca8077..9f6b0de9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -362,21 +362,24 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - - yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); + if (!data_track_.header.frame_id.empty()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + + yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); + transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); + } } catch (tf2::TransformException& ex) { From 3bae865e3e81af0427b356a2c36976793e02b716 Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Mon, 9 Jan 2023 18:07:55 +0800 Subject: [PATCH 045/130] Run pre-commit. --- .../src/joint_calibration_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index 76a54a2c..e97225d8 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -130,8 +130,8 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati state_ = CALIBRATED; if (is_return_) { - position_ctrl_.setCommand(target_position_); - returned_ = true; + position_ctrl_.setCommand(target_position_); + returned_ = true; } else { From 5534d5cf7a08107ed993a96e831294c7a9e22c74 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 9 Jan 2023 19:37:57 +0800 Subject: [PATCH 046/130] Update controller description. --- rm_chassis_controllers/rm_chassis_controllers_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 07abcd48..6319ebc3 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -28,7 +28,7 @@ type="rm_chassis_controllers::BalanceController" base_class_type="controller_interface::ControllerBase"> - The ReactionWheelController is RoboMaster balance standard robot Balance controller. It expects a + The BalanceController is RoboMaster balance standard robot Balance controller. It expects a EffortJointInterface type of hardware interface. From 5ac065a1e1e081d241799aacd87104bbea9b9486 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 9 Jan 2023 19:55:52 +0800 Subject: [PATCH 047/130] Update test. --- ...plate_reaction_wheel.yaml => balance.yaml} | 31 ++++++++++++------- .../test/load_controllers.launch | 2 +- .../test/{template_omni.yaml => omni.yaml} | 0 .../{template_swerve.yaml => swerve.yaml} | 0 4 files changed, 21 insertions(+), 12 deletions(-) rename rm_chassis_controllers/test/{template_reaction_wheel.yaml => balance.yaml} (51%) rename rm_chassis_controllers/test/{template_omni.yaml => omni.yaml} (100%) rename rm_chassis_controllers/test/{template_swerve.yaml => swerve.yaml} (100%) diff --git a/rm_chassis_controllers/test/template_reaction_wheel.yaml b/rm_chassis_controllers/test/balance.yaml similarity index 51% rename from rm_chassis_controllers/test/template_reaction_wheel.yaml rename to rm_chassis_controllers/test/balance.yaml index 9dc3ef51..55a2336b 100644 --- a/rm_chassis_controllers/test/template_reaction_wheel.yaml +++ b/rm_chassis_controllers/test/balance.yaml @@ -1,8 +1,10 @@ controllers: chassis_controller: type: rm_chassis_controllers/BalanceController + # ChassisBase publish_rate: 100 enable_odom_tf: true + publish_odom_tf: false power: effort_coeff: 12.0 vel_coeff: 0.0048 @@ -10,20 +12,27 @@ controllers: twist_angular: 0.5233 timeout: 0.1 pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - pid_block_error: { p: 0, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] + # BalanceController imu_name: "base_imu" - joint: "reaction_wheel_joint" - - m_b: 1.854 - m_c: 5.2 - m_p: 10.098 - l_p: 0.039 - l_b: 0.1389 - g: 9.81 + m_w: 0.72 + m: 11.48 + m_b: 1.29 + i_w: 0.01683 + l: 0.0587 + y_b: 0.16 + z_b: 0.0468 + g: 9.8 + i_m: 0.1982 wheel_radius: 0.125 wheel_base: 0.42 + left: + wheel_joint: "left_wheel_joint" + block_joint: "left_momentum_block_joint" + right: + wheel_joint: "right_wheel_joint" + block_joint: "right_momentum_block_joint" - q: [ 0.5, 0.5, 40, 51, 4, 0.2, 0.1, 1 ] - r: [ 0.01, 0.01, 0.0001 ] + q: [ 0.5, 0.5, 40, 51, 51, 10, 0.2, 0.1, 1, 1 ] + r: [ 0.5, 0.5, 0.002, 0.002 ] diff --git a/rm_chassis_controllers/test/load_controllers.launch b/rm_chassis_controllers/test/load_controllers.launch index 03fa717e..8caa327b 100644 --- a/rm_chassis_controllers/test/load_controllers.launch +++ b/rm_chassis_controllers/test/load_controllers.launch @@ -3,7 +3,7 @@ - + false Date: Mon, 9 Jan 2023 20:16:05 +0800 Subject: [PATCH 048/130] Update note. --- rm_chassis_controllers/src/balance.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 98ac046f..5256aa80 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -39,10 +39,14 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan right_momentum_block_joint_handle_ = robot_hw->get()->getHandle(right_momentum_block_joint); - // i_b is moment of inertia of the pendulum body around the pivot point, + // m_w is mass of single wheel + // m is mass of the robot except wheels and momentum_blocks + // m_b is mass of single momentum_block // i_w is the moment of inertia of the wheel around the rotational axis of the motor - // l is the distance between the motor axis and the pivot point - // l_b is the distance between the center of mass of the pendulum body and the pivot point + // l is the vertical component of the distance between the wheel center and the center of mass of robot + // y_b is the y-axis component of the coordinates of the momentum block in the base_link coordinate system + // z_b is the vertical component of the distance between the momentum block and the center of mass of robot + // i_m is the moment of inertia of the robot around the y-axis of base_link coordinate. double m_w, m, m_b, i_w, l, y_b, z_b, g, i_m; if (!controller_nh.getParam("m_w", m_w)) From 0eb67fda38270da2d8ab63f137f66ed1be07de99 Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Mon, 9 Jan 2023 20:31:02 +0800 Subject: [PATCH 049/130] Add the else to judge mode TRACK --- rm_gimbal_controllers/src/gimbal_base.cpp | 35 +++++++++++------------ 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9f6b0de9..f7546052 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -354,7 +354,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) yaw_vel_des = cmd_gimbal_.rate_yaw; pitch_vel_des = cmd_gimbal_.rate_pitch; } - else + else if (state_ == TRACK) { geometry_msgs::Point target_pos = data_track_.target_pos; geometry_msgs::Vector3 target_vel = data_track_.target_vel; @@ -362,24 +362,21 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { - if (!data_track_.header.frame_id.empty()) - { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - - yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); - } + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + + yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); + transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); } catch (tf2::TransformException& ex) { From 9cd2b75c78e939d052a7ff5b6fa81de914eb14d6 Mon Sep 17 00:00:00 2001 From: lsy <2028238109@qq.com> Date: Tue, 10 Jan 2023 11:05:53 +0800 Subject: [PATCH 050/130] Fix bug. --- .../src/joint_calibration_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index e97225d8..68724635 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -131,7 +131,6 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati if (is_return_) { position_ctrl_.setCommand(target_position_); - returned_ = true; } else { @@ -179,7 +178,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati { if (is_return_) { - if ((std::abs(position_ctrl_.joint_.getPosition()) - target_position_) < position_threshold_) + if ((std::abs(position_ctrl_.joint_.getPosition() - target_position_)) < position_threshold_) returned_ = true; position_ctrl_.update(time, period); } From c83eae5a7273f126a3da9f89a22aa97c36eadc6c Mon Sep 17 00:00:00 2001 From: StarHeart Date: Sat, 21 Jan 2023 10:07:32 +0800 Subject: [PATCH 051/130] ci: remove duplicate initialization --- .github/workflows/format.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 0e1c4fe8..45f0cd92 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -26,7 +26,6 @@ jobs: sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt-get -q update sudo apt-get -q install python3-rosdep - sudo rosdep init rosdep update sudo apt-get -q install catkin-lint export ROS_DISTRO=noetic From f6d807df80ebd9af9c2921f0b577f0532662a704 Mon Sep 17 00:00:00 2001 From: StarHeart Date: Sat, 21 Jan 2023 10:10:59 +0800 Subject: [PATCH 052/130] ci: remove default apt list as demand --- .github/workflows/format.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 45f0cd92..ff7149e6 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -26,6 +26,8 @@ jobs: sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt-get -q update sudo apt-get -q install python3-rosdep + rm -f /etc/ros/rosdep/sources.list.d/*.list + sudo rosdep init rosdep update sudo apt-get -q install catkin-lint export ROS_DISTRO=noetic From 852530ee3e727aac3b53dc1ae5017e7285720576 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 7 Feb 2023 20:31:36 +0800 Subject: [PATCH 053/130] Fix bug. --- rm_gimbal_controllers/src/gimbal_base.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 62ca8077..3974fc8e 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -42,6 +42,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { From 33ea83dcb349c0d738810a32f28d43ceb09e46ae Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 11 Feb 2023 03:42:50 +0800 Subject: [PATCH 054/130] Clear position when position_des minus position_act bigger than threshold. --- .../include/rm_chassis_controllers/balance.h | 2 ++ rm_chassis_controllers/src/balance.cpp | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 51a0f85c..cc1fdd0e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -33,6 +33,8 @@ class BalanceController : public ChassisBase x_; double wheel_radius_, wheel_base_; double position_des_ = 0; + double position_offset_ = 0.; + double position_clear_threshold_ = 0.; double yaw_des_ = 0; hardware_interface::ImuSensorHandle imu_handle_; diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 5256aa80..edbc66e2 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -104,6 +104,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ROS_ERROR("Params wheel_base_ doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } + controller_nh.getParam("position_offset", position_offset_); + controller_nh.getParam("position_clear_threshold", position_clear_threshold_); q_.setZero(); r_.setZero(); @@ -339,6 +341,11 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); x(5) -= vel_cmd_.x; x(6) -= vel_cmd_.z; + if (std::abs(x(0) + position_offset_) > position_clear_threshold_) + { + x_[0] = 0.; + position_des_ = position_offset_; + } u = k_ * (-x); rm_msgs::BalanceState state; state.header.stamp = time; From c65bf4c69a913dcceb8bc59f22e496223a9d6766 Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Mon, 20 Feb 2023 21:23:37 +0800 Subject: [PATCH 055/130] Add follow gimbal's chassis control --- rm_chassis_controllers/src/chassis_base.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 4a20b2dd..2d72f6e7 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -217,12 +217,12 @@ void ChassisBase::twist(const ros::Time& time, const ros::Duration& period recovery(); pid_follow_.reset(); } - tfVelToBase("yaw"); + tfVelToBase(command_source_frame_); try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", "yaw", ros::Time(0)).transform.rotation, roll, pitch, - yaw); + quatToRPY(robot_state_handle_.lookupTransform("base_link", command_source_frame_, ros::Time(0)).transform.rotation, + roll, pitch, yaw); double angle[4] = { -0.785, 0.785, 2.355, -2.355 }; double off_set = 0.0; @@ -256,7 +256,7 @@ void ChassisBase::gyro() recovery(); } - tfVelToBase(follow_source_frame_); + tfVelToBase(command_source_frame_); } template @@ -269,6 +269,7 @@ void ChassisBase::raw() recovery(); } + tfVelToBase(command_source_frame_); } template From 23f5979744566f6f1850fa1c729382c39613dc88 Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Mon, 20 Feb 2023 22:04:36 +0800 Subject: [PATCH 056/130] Add follow gimbal's chassis control --- rm_chassis_controllers/src/chassis_base.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 2d72f6e7..5c30da5e 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -269,7 +269,6 @@ void ChassisBase::raw() recovery(); } - tfVelToBase(command_source_frame_); } template From c11cbc14ed31045a743974ee9cc0dd990bc0198b Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 21 Feb 2023 00:47:07 +0800 Subject: [PATCH 057/130] Add gpio calibration controller. --- rm_calibration_controllers/CMakeLists.txt | 3 +- .../gpio_calibration_controller.h | 46 ++++++++ .../rm_calibration_controllers_plugins.xml | 3 + .../src/gpio_calibration_controller.cpp | 106 ++++++++++++++++++ 4 files changed, 157 insertions(+), 1 deletion(-) create mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h create mode 100644 rm_calibration_controllers/src/gpio_calibration_controller.cpp diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 4a680f4b..022dbddd 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -51,7 +51,8 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/joint_calibration_controller.cpp) +add_library(${PROJECT_NAME} src/joint_calibration_controller.cpp + src/gpio_calibration_controller.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h new file mode 100644 index 00000000..c486b962 --- /dev/null +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -0,0 +1,46 @@ +// +// Created by guanlin on 22-11-7. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +namespace rm_calibration_controllers +{ +class GpioCalibrationController + : public controller_interface::MultiInterfaceController +{ +public: + GpioCalibrationController() = 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 gpioStateCB(const rm_msgs::GpioDataConstPtr& msg); + bool isCalibrated(control_msgs::QueryCalibrationState::Request& req, + control_msgs::QueryCalibrationState::Response& resp); + enum + { + INITIALIZED, + MOVING_TO_CENTER, + CALIBRATED + }; + int state_{}; + double velocity_search_{}, vel_gain_{}, vel_threshold_{}; + bool on_center_, gpio_state_change_, initial_gpio_state_ = false; + rm_control::ActuatorExtraHandle actuator_; + effort_controllers::JointVelocityController velocity_ctrl_; + + ros::ServiceServer is_calibrated_srv_; + ros::Subscriber gpio_sub_; + rm_msgs::GpioData gpio_data_{}; +}; +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml index 38aae700..0d8b0e8d 100755 --- a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml +++ b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml @@ -3,4 +3,7 @@ + diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp new file mode 100644 index 00000000..4027425e --- /dev/null +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -0,0 +1,106 @@ +// +// Created by guanlin on 22-11-7. +// + +#include "rm_calibration_controllers/gpio_calibration_controller.h" +#include + +namespace rm_calibration_controllers +{ +bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + velocity_ctrl_.init(robot_hw->get(), controller_nh); + gpio_sub_ = + controller_nh.subscribe("gpio_states", 1, &GpioCalibrationController::gpioStateCB, this); + XmlRpc::XmlRpcValue actuator; + if (!controller_nh.getParam("actuator", actuator)) + { + ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + actuator_ = robot_hw->get()->getHandle(actuator[0]); + if (!controller_nh.getParam("search_velocity", velocity_search_)) + { + ROS_ERROR("Search velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("vel_gain", vel_gain_)) + { + ROS_ERROR("Velocity gain was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("vel_threshold", vel_threshold_)) + { + ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + // advertise service to check calibration + is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &GpioCalibrationController::isCalibrated, this); + return true; +} + +void GpioCalibrationController::starting(const ros::Time& time) +{ + actuator_.setCalibrated(false); + state_ = INITIALIZED; + if (actuator_.getCalibrated()) + ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", + velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); +} + +void GpioCalibrationController::update(const ros::Time& time, const ros::Duration& period) +{ + switch (state_) + { + case INITIALIZED: + { + velocity_ctrl_.setCommand(velocity_search_); + state_ = MOVING_TO_CENTER; + break; + } + case MOVING_TO_CENTER: + { + while (std::abs(velocity_ctrl_.joint_.getVelocity()) > vel_threshold_) + { + if (gpio_state_change_) + { + velocity_ctrl_.setCommand(-velocity_search_ * vel_gain_); + } + } + if (gpio_state_change_) + { + velocity_ctrl_.setCommand(0.); + actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); + actuator_.setCalibrated(true); + ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); + state_ = CALIBRATED; + } + break; + } + case CALIBRATED: + { + velocity_ctrl_.update(time, period); + break; + } + } + velocity_ctrl_.update(time, period); +} + +bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState::Request& req, + control_msgs::QueryCalibrationState::Response& resp) +{ + ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED && on_center_); + resp.is_calibrated = (state_ == CALIBRATED && on_center_); + return true; +} + +void GpioCalibrationController::gpioStateCB(const rm_msgs::GpioDataConstPtr& msg) +{ + gpio_data_ = *msg; + if (gpio_data_.gpio_state[0] != initial_gpio_state_) + gpio_state_change_ = true; +} +} // namespace rm_calibration_controllers + +PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::GpioCalibrationController, controller_interface::ControllerBase) From 40a6a0846a2f8435e954fdd2743cf5752c48de9a Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 21 Feb 2023 20:32:15 +0800 Subject: [PATCH 058/130] 0.1.9 --- gpio_controller/CHANGELOG.rst | 7 ++++++ mimic_joint_controller/CHANGELOG.rst | 7 ++++++ rm_calibration_controllers/CHANGELOG.rst | 18 ++++++++++++++++ rm_chassis_controllers/CHANGELOG.rst | 27 ++++++++++++++++++++++++ rm_controllers/CHANGELOG.rst | 7 ++++++ rm_gimbal_controllers/CHANGELOG.rst | 14 ++++++++++++ rm_orientation_controller/CHANGELOG.rst | 7 ++++++ rm_shooter_controllers/CHANGELOG.rst | 9 ++++++++ robot_state_controller/CHANGELOG.rst | 7 ++++++ tof_radar_controller/CHANGELOG.rst | 7 ++++++ 10 files changed, 110 insertions(+) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 62f41add..93224761 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index b95df227..b11df960 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 62ce9044..52ddeae5 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#103 `_ from L-SY/fix_return + Fix calibration controller no return true. +* Fix bug. +* Run pre-commit. +* Merge branch 'master' into balance_standard +* Add engineer trigger ui . +* Merge pull request `#96 `_ from L-SY/fix_return + Fix joint_calibration_controller +* Delete repetitive set. +* delete useless target_velocity\_. +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* eyes is lost. +* Update fix can not return . +* Contributors: lsy, ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge branch 'master' into target_velocity_correction diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 219133af..16fb29a9 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#105 `_ from ljq-lv/gimbal_toward + Add follow gimbal's chassis control +* Add follow gimbal's chassis control +* Add follow gimbal's chassis control +* Merge pull request `#100 `_ from ye-luo-xi-tui/balance_standard + Add balance chassis controller +* Update note. +* Update test. +* Update controller description. +* Merge branch 'master' into balance_standard +* Modify the expected location update policy. +* Take some joints' name as params. +* Fix warning. +* Add power limit. +* Compute state matrix and control matrix with given params. +* Add balance_chassis's odometry. +* Transform IMU's data to base_link. +* Try to use PID controller to fix the problem that one block's position is differ from another. +* Revised dynamics model and remove gazebo dependency. +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Fix mistake of orientation data. +* Add some test codes for balance chassis. +* Contributors: ljq-lv, ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge branch 'master' into target_velocity_correction diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index 4f86c071..fd17e805 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#85 `_ from ye-luo-xi-tui/master diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index f9a9c58b..09611b44 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#104 `_ from ye-luo-xi-tui/balance_standard + Fix bug. +* Fix bug. +* Merge pull request `#102 `_ from ljq-lv/fix_warning + Fixed the bug of gimbal warning +* Add the else to judge mode TRACK +* Fixed the bug of gimbal warning +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ljq-lv, ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#92 `_ from ye-luo-xi-tui/target_velocity_correction diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 49e7bc60..eb103106 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#91 `_ from ye-luo-xi-tui/imu_offline diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index 1084b2b5..047070a9 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index 1e65c265..303d4608 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#86 `_ from NaHCO3bc/Readme diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 8176479a..e692b95b 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'master' into balance_standard +* Merge pull request `#97 `_ from ye-luo-xi-tui/master + 0.1.8 +* Contributors: ye-luo-xi-tui, yezi + 0.1.8 (2022-11-24) ------------------ * Merge pull request `#86 `_ from NaHCO3bc/Readme From 6a99d6f2c8c08b89b86e7f16df3c68adb79ae102 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 21 Feb 2023 20:32:36 +0800 Subject: [PATCH 059/130] 0.1.9 --- gpio_controller/CHANGELOG.rst | 4 ++-- gpio_controller/package.xml | 2 +- mimic_joint_controller/CHANGELOG.rst | 4 ++-- mimic_joint_controller/package.xml | 2 +- rm_calibration_controllers/CHANGELOG.rst | 4 ++-- rm_calibration_controllers/package.xml | 2 +- rm_chassis_controllers/CHANGELOG.rst | 4 ++-- rm_chassis_controllers/package.xml | 2 +- rm_controllers/CHANGELOG.rst | 4 ++-- rm_controllers/package.xml | 2 +- rm_gimbal_controllers/CHANGELOG.rst | 4 ++-- rm_gimbal_controllers/package.xml | 2 +- rm_orientation_controller/CHANGELOG.rst | 4 ++-- rm_orientation_controller/package.xml | 2 +- rm_shooter_controllers/CHANGELOG.rst | 4 ++-- rm_shooter_controllers/package.xml | 2 +- robot_state_controller/CHANGELOG.rst | 4 ++-- robot_state_controller/package.xml | 2 +- tof_radar_controller/CHANGELOG.rst | 4 ++-- tof_radar_controller/package.xml | 2 +- 20 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 93224761..cb28ae52 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index acb17bed..e8dc48ee 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -1,7 +1,7 @@ gpio_controller - 0.1.8 + 0.1.9 The gpio_controller package muyuexin diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index b11df960..5ac073c3 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 141dd696..29ecf2de 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -1,7 +1,7 @@ mimic_joint_controller - 0.1.8 + 0.1.9 The mimic_joint_controller package ljq diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 52ddeae5..2e46f00d 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge pull request `#103 `_ from L-SY/fix_return Fix calibration controller no return true. * Fix bug. diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 78125c9b..1af87d6f 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -1,7 +1,7 @@ rm_calibration_controllers - 0.1.8 + 0.1.9 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 16fb29a9..4aebf534 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge pull request `#105 `_ from ljq-lv/gimbal_toward Add follow gimbal's chassis control * Add follow gimbal's chassis control diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 59d6ff90..e361d35d 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -1,7 +1,7 @@ rm_chassis_controllers - 0.1.8 + 0.1.9 RoboMaster standard robot Chassis controller Qiayuan Liao BSD diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index fd17e805..1c2c6986 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/rm_controllers/package.xml b/rm_controllers/package.xml index dbe5f065..e555326d 100644 --- a/rm_controllers/package.xml +++ b/rm_controllers/package.xml @@ -1,7 +1,7 @@ rm_controllers - 0.1.8 + 0.1.9 Meta package that contains package for RoboMaster. Qiayuan Liao diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 09611b44..5551ce62 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge pull request `#104 `_ from ye-luo-xi-tui/balance_standard Fix bug. * Fix bug. diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index ff1b5155..962d0ba3 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -1,7 +1,7 @@ rm_gimbal_controllers - 0.1.8 + 0.1.9 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index eb103106..5515ce7f 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index 4ac9774e..fb3418eb 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -1,7 +1,7 @@ rm_orientation_controller - 0.1.8 + 0.1.9 RoboMaster standard robot orientation controller Qiayuan Liao BSD diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index 047070a9..6297cd0b 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index 49ef1b4b..32d6bef7 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -1,7 +1,7 @@ rm_shooter_controllers - 0.1.8 + 0.1.9 RoboMaster standard robot Shooter controller Qiayuan Liao BSD diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index 303d4608..c7200f9a 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index 7c577b09..2d33e4a4 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -1,7 +1,7 @@ robot_state_controller - 0.1.8 + 0.1.9 A template for ROS packages. Qiayuan Liao BSD diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index e692b95b..05470e50 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.9 (2023-02-21) +------------------ * Merge branch 'master' into balance_standard * Merge pull request `#97 `_ from ye-luo-xi-tui/master 0.1.8 diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index fdecfad4..c1978973 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -1,7 +1,7 @@ tof_radar_controller - 0.1.8 + 0.1.9 The tof radar controller package luotinkai From 6edfb8ec9cf3fcfc6e6ec8559c6db347e7be3dfe Mon Sep 17 00:00:00 2001 From: Aung-xiao <2557160927@qq.com> Date: Thu, 23 Feb 2023 21:48:35 +0800 Subject: [PATCH 060/130] add mecanum.yaml --- rm_chassis_controllers/test/mecanum.yaml | 40 ++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 rm_chassis_controllers/test/mecanum.yaml diff --git a/rm_chassis_controllers/test/mecanum.yaml b/rm_chassis_controllers/test/mecanum.yaml new file mode 100644 index 00000000..0c9ae872 --- /dev/null +++ b/rm_chassis_controllers/test/mecanum.yaml @@ -0,0 +1,40 @@ +controllers: + chassis_controller: + type: rm_chassis_controllers/OmniController + # ChassisBase + publish_rate: 100 + enable_odom_tf: true + publish_odom_tf: false + power: + effort_coeff: 3.9 + vel_coeff: 0.00855 + power_offset: -9.8 + twist_angular: 0.5233 + timeout: 0.1 + pid_follow: { p: 5, i: 0, d: 0.8, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] + + # MecanumController + wheels: + left_front: + pose: [ 0.2, 0.185, 0. ] + roller_angle: -0.785 + joint: left_front_wheel_joint + <<: &wheel_setting + radius: 0.07625 + pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + right_front: + pose: [ 0.2, -0.185, 0. ] + roller_angle: 0.785 + joint: right_front_wheel_joint + <<: *wheel_setting + left_back: + pose: [ -0.2, 0.185, 0. ] + roller_angle: 0.785 + joint: left_back_wheel_joint + <<: *wheel_setting + right_back: + pose: [ -0.2, -0.185, 0. ] + roller_angle: -0.785 + joint: right_back_wheel_joint + <<: *wheel_setting From c7e31b70ebcb701b6b513c278241059aba4af5fc Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Fri, 24 Feb 2023 18:10:58 +0800 Subject: [PATCH 061/130] Modifier default value of forward_push_threshold and exit_push_threshold. --- rm_shooter_controllers/src/standard.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 365961e0..4bd2f71e 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -50,8 +50,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .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.), - .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.), + .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), + .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), .qd_10 = getParam(controller_nh, "qd_10", 0.), .qd_15 = getParam(controller_nh, "qd_15", 0.), .qd_16 = getParam(controller_nh, "qd_16", 0.), From 7891f061903b87945e0b9dfdd76fc3d41f035e1e Mon Sep 17 00:00:00 2001 From: Aung-xiao <2557160927@qq.com> Date: Fri, 24 Feb 2023 19:45:29 +0800 Subject: [PATCH 062/130] change the name of the controller --- rm_chassis_controllers/test/mecanum.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/test/mecanum.yaml b/rm_chassis_controllers/test/mecanum.yaml index 0c9ae872..de7606c1 100644 --- a/rm_chassis_controllers/test/mecanum.yaml +++ b/rm_chassis_controllers/test/mecanum.yaml @@ -14,7 +14,7 @@ controllers: pid_follow: { p: 5, i: 0, d: 0.8, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - # MecanumController + # OmniController wheels: left_front: pose: [ 0.2, 0.185, 0. ] From c0fa2b069ad5dd99cecde3fc592a3c035e2c6699 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Thu, 23 Feb 2023 20:02:49 +0800 Subject: [PATCH 063/130] Modify queue length of gpio subscriber. --- .../src/gpio_calibration_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index 4027425e..09211143 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -12,7 +12,7 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: { velocity_ctrl_.init(robot_hw->get(), controller_nh); gpio_sub_ = - controller_nh.subscribe("gpio_states", 1, &GpioCalibrationController::gpioStateCB, this); + controller_nh.subscribe("gpio_states", 100, &GpioCalibrationController::gpioStateCB, this); XmlRpc::XmlRpcValue actuator; if (!controller_nh.getParam("actuator", actuator)) { @@ -67,6 +67,8 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio { velocity_ctrl_.setCommand(-velocity_search_ * vel_gain_); } + else + velocity_ctrl_.setCommand(velocity_search_); } if (gpio_state_change_) { From b542685dc5af927205f6c41a18f34661d7945859 Mon Sep 17 00:00:00 2001 From: d0h0s <3328432363@qq.com> Date: Sun, 26 Feb 2023 11:08:50 +0800 Subject: [PATCH 064/130] Updated README.md of rm_chassis_controllers. --- rm_chassis_controllers/README.md | 74 +++++++++++++++++++++++++++----- 1 file changed, 63 insertions(+), 11 deletions(-) diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index dd9b910d..df6f8849 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -36,8 +36,10 @@ sudo rosdep install --from-paths src * [Robot Operating System (ROS)](http://wiki.ros.org/) (middleware for robotics), * roscpp * rm_common +* controller_interface * effort_controllers * tf2_geometry_msgs +* angles * robot_localization ## ROS API @@ -57,10 +59,16 @@ sudo rosdep install --from-paths src Set the speed of the chassis. #### Published Topics + * **`odom`**([nav_msgs/Odometry](http://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html)) Chassis odometer information (speed, position, covariance). +* **`state`**([rm_msgs::BalanceState](http://docs.ros.org/en/api/rm_msgs/html/msg/BalanceState.html)) + + Contains information about the Balance at a certain point in time, stores the vector of joint position, and optional + speed and acceleration. + #### Parameters ##### common @@ -83,7 +91,11 @@ sudo rosdep install --from-paths src * **`enable_odom_tf`** (bool, default: true) - Option.If set this param true, it will send Transform from odom to base. + Option.If it is set to true, publish_odom_tf is also true, it will send Transform from odom to base. + +* **`publish_odom_tf_`** (bool, default: false) + + Option.If it is set to true, enable_odom_tf is also true, it will send Transform from odom to base. * **`twist_covariance_diagonal`** (double[6]) @@ -105,27 +117,67 @@ sudo rosdep install --from-paths src Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0. +* **`power_offset`** (double) + + Difference between actual power and set power. + ##### Balance -* **`joint_left_name`** (string, default: "joint_left") +* **`imu_name`** (string, default: "base_imu") + + imu joint name or list of joint names. + +* **`left/wheel_joint`** (string, default: "left_wheel_joint") + + left wheel joint name or list of joint names. + +* **`left/block_joint`** (string, default: "left_momentum_block_joint") + + left momentum block joint name or list of joint names. + +* **`right/wheel_joint`** (string, default: "right_wheel_joint") + + right wheel joint name or list of joint names. + +* **`right/block_joint`** (string, default: "right_momentum_block_joint") + + right momentum block joint name or list of joint names. + +* **`m_w`** (double, default: 0.72) + + m_w is mass of single wheel. + +* **`m`** (double, default: 11.48) + + m is mass of the robot except wheels and momentum_blocks. + +* **`m_b`** (double, default: 1.13) + + m_b is mass of single momentum_block. + +* **`i_w`** (double, default: 0.01683) + + i_w is the moment of inertia of the wheel around the rotational axis of the motor. + +* **`l`** (double, default: 0.0587) - Left wheel joint name or list of joint names. + l is the vertical component of the distance between the wheel center and the center of mass of robot. -* **`joint_right_name`** (string, default: "joint_right") +* **`y_b`** (double, default: 0.16) - Right wheel joint name or list of joint names. + y_b is the y-axis component of the coordinates of the momentum block in the base_link coordinate system. -* **`com_pitch_offset`** (double, default: 0) +* **`z_b`** (double[4], default: 0.0468) - The reduction ratio of pitch. + z_b is the vertical component of the distance between the momentum block and the center of mass of robot. -* **`a`** (double[16]) +* **`g`** (double, default: 9.8) - State space expression. + Gravity constant. -* **`b`** (double[8]) +* **`i_m`** (double, default: 0.1982) - State space expression. + i_m is the moment of inertia of the robot around the y-axis of base_link coordinate. * **`q`** (double[16]) From 2cd0d06a1a1273b3b35ac039dd59f4e17e2f87a9 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 26 Feb 2023 16:04:29 +0800 Subject: [PATCH 065/130] Modify logic and callback function of gpio calibration controller. --- .../gpio_calibration_controller.h | 5 ++-- .../src/gpio_calibration_controller.cpp | 25 +++++++++++++------ 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h index c486b962..8ac8df67 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -33,14 +33,13 @@ class GpioCalibrationController MOVING_TO_CENTER, CALIBRATED }; - int state_{}; + int state_{}, countdown_{}; double velocity_search_{}, vel_gain_{}, vel_threshold_{}; - bool on_center_, gpio_state_change_, initial_gpio_state_ = false; + bool on_center_ = false, gpio_state_change_ = false, initial_gpio_state_ = false; rm_control::ActuatorExtraHandle actuator_; effort_controllers::JointVelocityController velocity_ctrl_; ros::ServiceServer is_calibrated_srv_; ros::Subscriber gpio_sub_; - rm_msgs::GpioData gpio_data_{}; }; } // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index 09211143..90fbd3b4 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -35,6 +35,11 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } + if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) + { + ROS_ERROR("Initial gpio state was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } // advertise service to check calibration is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &GpioCalibrationController::isCalibrated, this); return true; @@ -56,26 +61,34 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio case INITIALIZED: { velocity_ctrl_.setCommand(velocity_search_); + velocity_ctrl_.update(time, period); + countdown_ = 100; state_ = MOVING_TO_CENTER; break; } case MOVING_TO_CENTER: { - while (std::abs(velocity_ctrl_.joint_.getVelocity()) > vel_threshold_) + if (std::abs(velocity_ctrl_.joint_.getVelocity()) < vel_threshold_) + countdown_--; + else + countdown_ = 100; + if (countdown_ != 0) { if (gpio_state_change_) { - velocity_ctrl_.setCommand(-velocity_search_ * vel_gain_); + velocity_ctrl_.setCommand(velocity_ctrl_.command_ * vel_gain_ * -1.); + velocity_ctrl_.update(time, period); } else - velocity_ctrl_.setCommand(velocity_search_); + velocity_ctrl_.update(time, period); } - if (gpio_state_change_) + if (countdown_ < 0) { velocity_ctrl_.setCommand(0.); actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); actuator_.setCalibrated(true); ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); + velocity_ctrl_.update(time, period); state_ = CALIBRATED; } break; @@ -86,7 +99,6 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio break; } } - velocity_ctrl_.update(time, period); } bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState::Request& req, @@ -99,8 +111,7 @@ bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState void GpioCalibrationController::gpioStateCB(const rm_msgs::GpioDataConstPtr& msg) { - gpio_data_ = *msg; - if (gpio_data_.gpio_state[0] != initial_gpio_state_) + if (msg->gpio_state[0] != initial_gpio_state_) gpio_state_change_ = true; } } // namespace rm_calibration_controllers From e78bbbf384414841d0087a559c369c9b6144af98 Mon Sep 17 00:00:00 2001 From: d0h0s <3328432363@qq.com> Date: Mon, 27 Feb 2023 17:12:12 +0800 Subject: [PATCH 066/130] Added the parameters of Omni and fixed the inappropriate description. --- rm_chassis_controllers/README.md | 83 ++++++++++++++++++++------------ 1 file changed, 51 insertions(+), 32 deletions(-) diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index df6f8849..fe622d77 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -66,8 +66,7 @@ sudo rosdep install --from-paths src * **`state`**([rm_msgs::BalanceState](http://docs.ros.org/en/api/rm_msgs/html/msg/BalanceState.html)) - Contains information about the Balance at a certain point in time, stores the vector of joint position, and optional - speed and acceleration. + Contains quantities of state and control about the Balance. #### Parameters @@ -91,7 +90,7 @@ sudo rosdep install --from-paths src * **`enable_odom_tf`** (bool, default: true) - Option.If it is set to true, publish_odom_tf is also true, it will send Transform from odom to base. + Option.If it is set to true, it will not send Transform, but will store Transform in tf_buffer. * **`publish_odom_tf_`** (bool, default: false) @@ -119,57 +118,57 @@ sudo rosdep install --from-paths src * **`power_offset`** (double) - Difference between actual power and set power. + Fix the difference between theoretical power and actual power. ##### Balance * **`imu_name`** (string, default: "base_imu") - imu joint name or list of joint names. + Chassis imu name. * **`left/wheel_joint`** (string, default: "left_wheel_joint") - left wheel joint name or list of joint names. + left wheel joint name. * **`left/block_joint`** (string, default: "left_momentum_block_joint") - left momentum block joint name or list of joint names. + left momentum block joint name. * **`right/wheel_joint`** (string, default: "right_wheel_joint") - right wheel joint name or list of joint names. + right wheel joint name. * **`right/block_joint`** (string, default: "right_momentum_block_joint") - right momentum block joint name or list of joint names. + right momentum block joint name. * **`m_w`** (double, default: 0.72) - m_w is mass of single wheel. + mass of single wheel. * **`m`** (double, default: 11.48) - m is mass of the robot except wheels and momentum_blocks. + mass of the robot except wheels and momentum_blocks. * **`m_b`** (double, default: 1.13) - m_b is mass of single momentum_block. + mass of single momentum_block. * **`i_w`** (double, default: 0.01683) - i_w is the moment of inertia of the wheel around the rotational axis of the motor. + The moment of inertia of the wheel around the rotational axis of the motor. * **`l`** (double, default: 0.0587) - l is the vertical component of the distance between the wheel center and the center of mass of robot. + The vertical component of the distance between the wheel center and the center of mass of robot. * **`y_b`** (double, default: 0.16) - y_b is the y-axis component of the coordinates of the momentum block in the base_link coordinate system. + The y-axis component of the coordinates of the momentum block in the base_link coordinate system. * **`z_b`** (double[4], default: 0.0468) - z_b is the vertical component of the distance between the momentum block and the center of mass of robot. + The vertical component of the distance between the momentum block and the center of mass of robot. * **`g`** (double, default: 9.8) @@ -177,7 +176,7 @@ sudo rosdep install --from-paths src * **`i_m`** (double, default: 0.1982) - i_m is the moment of inertia of the robot around the y-axis of base_link coordinate. + The moment of inertia of the robot around the y-axis of base_link coordinate. * **`q`** (double[16]) @@ -209,37 +208,57 @@ sudo rosdep install --from-paths src The radius of wheel. +##### Omni + +* **`/wheels//pose`** (double[3]) + + The pose of wheel. + +* **`/wheels//joint`** (string) + + wheel joint name. + +* **`/wheels/left_front/roller_angle`** (double) + + The roller angle of wheel. + +* **`/wheels/left_front/radius`** (double) + + The radius of wheel. + ## Controller configuration examples ### Complete description ``` -chassis_controller: - type: rm_chassis_controllers/MecanumController + chassis_controller: + type: rm_chassis_controllers/OmniController publish_rate: 100 enable_odom_tf: true + publish_odom_tf: false + power: + effort_coeff: 10.0 + vel_coeff: 0.0060 + power_offset: -8.41 + twist_angular: 0.5233 + timeout: 0.1 + pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] + + chassis_radius: 0.208 wheel_radius: 0.07625 left_front: joint: "left_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } right_front: joint: "right_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } left_back: joint: "left_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } right_back: joint: "right_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - wheel_base: 0.395 - wheel_track: 0.374 - power: - coeff: 0.535 - min_vel: 4.4 - twist_angular: 0.5233 - timeout: 0.1 - pid_follow: { p: 5, i: 0, d: 0.8, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } ``` ## Bugs & Feature Requests From c736444a66285a02a949cf9b2d03049f1bd2b581 Mon Sep 17 00:00:00 2001 From: d0h0s <3328432363@qq.com> Date: Sat, 4 Mar 2023 01:08:27 +0800 Subject: [PATCH 067/130] Repaired the example of chassis_controller. --- rm_chassis_controllers/README.md | 38 ++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index fe622d77..1ebb81ac 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -90,7 +90,7 @@ sudo rosdep install --from-paths src * **`enable_odom_tf`** (bool, default: true) - Option.If it is set to true, it will not send Transform, but will store Transform in tf_buffer. + Option.If it is set to true, it will store Transform in tf_buffer. * **`publish_odom_tf_`** (bool, default: false) @@ -238,27 +238,33 @@ sudo rosdep install --from-paths src publish_odom_tf: false power: effort_coeff: 10.0 - vel_coeff: 0.0060 + vel_coeff: 0.003 power_offset: -8.41 twist_angular: 0.5233 timeout: 0.1 pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - chassis_radius: 0.208 - wheel_radius: 0.07625 - left_front: - joint: "left_front_wheel_joint" - pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_front: - joint: "right_front_wheel_joint" - pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - left_back: - joint: "left_back_wheel_joint" - pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_back: - joint: "right_back_wheel_joint" - pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + wheels: + left_front: + pose: [ 0.147, 0.147, 2.356 ] + joint: left_front_wheel_joint + <<: &wheel_setting + roller_angle: 0. + radius: 0.07625 + pid: { p: 0.41, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + right_front: + pose: [ 0.147, -0.147, 0.785 ] + joint: right_front_wheel_joint + <<: *wheel_setting + left_back: + pose: [ -0.147, 0.147, -2.356 ] + joint: left_back_wheel_joint + <<: *wheel_setting + right_back: + pose: [ -0.147, -0.147, -0.785 ] + joint: right_back_wheel_joint + <<: *wheel_setting ``` ## Bugs & Feature Requests From 62ccd6a34169263b765360883512366bd384617e Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Sun, 5 Mar 2023 18:26:32 +0800 Subject: [PATCH 068/130] Delete the chassis mode "GYRO" --- .../rm_chassis_controllers/chassis_base.h | 10 ++-------- rm_chassis_controllers/src/chassis_base.cpp | 17 +---------------- 2 files changed, 3 insertions(+), 24 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 629ca5c0..bbedaffb 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -78,7 +78,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * * Receive real_time command from manual and check whether it is normally, if can not receive command from manual * for a while, chassis's velocity will be set zero to avoid out of control. Execute different action according - * to current mode such as RAW, FOLLOW, GYRO, TWIST.(Their specific usage will be explain in the next). UpdateOdom,Set + * to current mode such as RAW, FOLLOW, TWIST.(Their specific usage will be explain in the next). UpdateOdom,Set * necessary params such as Acc and vel_tfed. Execute moving action and powerlimit. * * @param time The current time. @@ -108,11 +108,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @param period The time passed since the last call to update. */ void twist(const ros::Time& time, const ros::Duration& period); - /** @brief The mode GYRO: Moving like a top. - * - * The mode GYRO: Chassis will rotate around itself. - */ - void gyro(); virtual void moveJoint(const ros::Time& time, const ros::Duration& period) = 0; virtual geometry_msgs::Twist odometry() = 0; /** @brief Init frame on base_link. Integral vel to pos and angle. @@ -158,10 +153,9 @@ class ChassisBase : public controller_interface::MultiInterfaceController { RAW, FOLLOW, - GYRO, TWIST }; - int state_ = GYRO; + int state_ = RAW; RampFilter*ramp_x_{}, *ramp_y_{}, *ramp_w_{}; std::string follow_source_frame_{}, command_source_frame_{}; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 5c30da5e..28c56917 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -162,9 +162,6 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio case FOLLOW: follow(time, period); break; - case GYRO: - gyro(); - break; case TWIST: twist(time, period); break; @@ -246,19 +243,6 @@ void ChassisBase::twist(const ros::Time& time, const ros::Duration& period } } -template -void ChassisBase::gyro() -{ - if (state_changed_) - { - state_changed_ = false; - ROS_INFO("[Chassis] Enter GYRO"); - - recovery(); - } - tfVelToBase(command_source_frame_); -} - template void ChassisBase::raw() { @@ -269,6 +253,7 @@ void ChassisBase::raw() recovery(); } + tfVelToBase(command_source_frame_); } template From 160e3f16c16e0c948db0e417f776cd58023555a6 Mon Sep 17 00:00:00 2001 From: ljq-lv <939468378@qq.com> Date: Sun, 5 Mar 2023 18:38:50 +0800 Subject: [PATCH 069/130] Modified the chassis's README --- rm_chassis_controllers/README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index 1ebb81ac..0b6a81ae 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -2,7 +2,10 @@ ## Overview -There are four states: raw, follow, gyro and twist. The output torque and speed of each motor of the chassis can be calculated according to the current state of the control, the received speed and pose of the pan/tilt, and the speed and acceleration commands, and the data is returned by the motor to calculate The speed and posture of the chassis are released. The control algorithm involved in the chassis controller is PID algorithm. +There are three states: raw, follow and twist. The output torque and speed of each motor of the chassis can be +calculated according to the current state of the control, the received speed and pose of the pan/tilt, and the speed and +acceleration commands, and the data is returned by the motor to calculate The speed and posture of the chassis are +released. The control algorithm involved in the chassis controller is PID algorithm. **Keywords:** mecanum, swerve, balance, chassis, ROS, RoboMaster From 8303ae20b03d997894d18a614158f8ed5c46a345 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 6 Mar 2023 14:33:24 +0800 Subject: [PATCH 070/130] Use Vector3WithFilter in rm_common instead. --- .../rm_gimbal_controllers/gimbal_base.h | 54 +++++-------------- 1 file changed, 13 insertions(+), 41 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 003d9dca..bad6aa42 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -54,41 +54,6 @@ namespace rm_gimbal_controllers { -class Vector3WithFilter -{ -public: - Vector3WithFilter(int num_data) - { - for (int i = 0; i < 3; i++) - filter_vector_.push_back(std::make_shared>(num_data)); - } - void input(double vector[3], double period) - { - for (int i = 0; i < 3; i++) - { - if (period < 0) - return; - if (period > 0.1) - filter_vector_[i]->clear(); - filter_vector_[i]->input(vector[i]); - } - } - double x() - { - return filter_vector_[0]->output(); - } - double y() - { - return filter_vector_[1]->output(); - } - double z() - { - return filter_vector_[2]->output(); - } - -private: - std::vector>> filter_vector_; -}; class ChassisVel { public: @@ -97,20 +62,27 @@ class ChassisVel double num_data; nh.param("num_data", num_data, 20.0); nh.param("debug", is_debug_, true); - linear_ = std::make_shared(num_data); - angular_ = std::make_shared(num_data); + linear_ = std::make_shared>(num_data); + angular_ = std::make_shared>(num_data); if (is_debug_) { real_pub_.reset(new realtime_tools::RealtimePublisher(nh, "real", 1)); filtered_pub_.reset(new realtime_tools::RealtimePublisher(nh, "filtered", 1)); } } - std::shared_ptr linear_; - std::shared_ptr angular_; + std::shared_ptr> linear_; + std::shared_ptr> angular_; void update(double linear_vel[3], double angular_vel[3], double period) { - linear_->input(linear_vel, period); - angular_->input(angular_vel, period); + if (period < 0) + return; + if (period > 0.1) + { + linear_->clear(); + angular_->clear(); + } + linear_->input(linear_vel); + angular_->input(angular_vel); if (is_debug_ && loop_count_ % 10 == 0) { if (real_pub_->trylock()) From 1014e2140d3114349c769ac233109fd1312294c2 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 10 Mar 2023 17:35:09 +0800 Subject: [PATCH 071/130] Modify gpio calibration controller scheme to first use speed control to find a fixed point, and then use position control to reach. --- .../gpio_calibration_controller.h | 13 +++- .../src/gpio_calibration_controller.cpp | 78 +++++++++++++------ 2 files changed, 62 insertions(+), 29 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h index 8ac8df67..553c8b8e 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -30,14 +31,18 @@ class GpioCalibrationController enum { INITIALIZED, - MOVING_TO_CENTER, + MOVING_AROUND, + RETURN, CALIBRATED }; - int state_{}, countdown_{}; - double velocity_search_{}, vel_gain_{}, vel_threshold_{}; - bool on_center_ = false, gpio_state_change_ = false, initial_gpio_state_ = false; + int state_{}; + double velocity_search_{}, vel_gain_{}, vel_threshold_{}, enter_pos_{}, exit_pos_{}; + double position_threshold_ = 0.01; + bool initial_gpio_state_ = false, enter_flag_ = false, exit_flag_ = false, can_returned_ = false, + is_returned_ = false; rm_control::ActuatorExtraHandle actuator_; effort_controllers::JointVelocityController velocity_ctrl_; + effort_controllers::JointPositionController position_ctrl_; ros::ServiceServer is_calibrated_srv_; ros::Subscriber gpio_sub_; diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index 90fbd3b4..84930cd7 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -10,9 +10,12 @@ namespace rm_calibration_controllers bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - velocity_ctrl_.init(robot_hw->get(), controller_nh); - gpio_sub_ = - controller_nh.subscribe("gpio_states", 100, &GpioCalibrationController::gpioStateCB, this); + ros::NodeHandle vel_nh(controller_nh, "velocity"); + ros::NodeHandle pos_nh(controller_nh, "position"); + velocity_ctrl_.init(robot_hw->get(), vel_nh); + position_ctrl_.init(robot_hw->get(), pos_nh); + gpio_sub_ = controller_nh.subscribe("/controllers/gpio_controller/gpio_states", 100, + &GpioCalibrationController::gpioStateCB, this); XmlRpc::XmlRpcValue actuator; if (!controller_nh.getParam("actuator", actuator)) { @@ -62,41 +65,57 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio { velocity_ctrl_.setCommand(velocity_search_); velocity_ctrl_.update(time, period); - countdown_ = 100; - state_ = MOVING_TO_CENTER; + state_ = MOVING_AROUND; break; } - case MOVING_TO_CENTER: + case MOVING_AROUND: { - if (std::abs(velocity_ctrl_.joint_.getVelocity()) < vel_threshold_) - countdown_--; - else - countdown_ = 100; - if (countdown_ != 0) + if (enter_flag_) { - if (gpio_state_change_) - { - velocity_ctrl_.setCommand(velocity_ctrl_.command_ * vel_gain_ * -1.); - velocity_ctrl_.update(time, period); - } - else - velocity_ctrl_.update(time, period); + enter_flag_ = false; + enter_pos_ = velocity_ctrl_.joint_.getPosition(); } - if (countdown_ < 0) + if (exit_flag_) + { + exit_flag_ = false; + exit_pos_ = velocity_ctrl_.joint_.getPosition(); + } + if (enter_pos_ != 0. && exit_pos_ != 0.) { velocity_ctrl_.setCommand(0.); + can_returned_ = true; + } + if (can_returned_) + { + position_ctrl_.setCommand((enter_pos_ + exit_pos_) / 2); + enter_pos_ = 0; + exit_pos_ = 0; + can_returned_ = false; + state_ = RETURN; + } + velocity_ctrl_.update(time, period); + break; + } + case RETURN: + { + is_returned_ = true; + position_ctrl_.update(time, period); + if (((std::abs(position_ctrl_.joint_.getPosition() - position_ctrl_.command_struct_.position_)) < + position_threshold_) && + (position_ctrl_.joint_.getVelocity() < vel_threshold_)) + { actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); actuator_.setCalibrated(true); ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); - velocity_ctrl_.update(time, period); state_ = CALIBRATED; } break; } case CALIBRATED: { - velocity_ctrl_.update(time, period); - break; + is_returned_ = false; + position_ctrl_.setCommand(position_ctrl_.joint_.getPosition()); + position_ctrl_.update(time, period); } } } @@ -104,15 +123,24 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState::Request& req, control_msgs::QueryCalibrationState::Response& resp) { - ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED && on_center_); - resp.is_calibrated = (state_ == CALIBRATED && on_center_); + ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED); + resp.is_calibrated = (state_ == CALIBRATED); return true; } void GpioCalibrationController::gpioStateCB(const rm_msgs::GpioDataConstPtr& msg) { if (msg->gpio_state[0] != initial_gpio_state_) - gpio_state_change_ = true; + { + if (!initial_gpio_state_ && !is_returned_) + { + enter_flag_ = true; + } + + if (initial_gpio_state_ && enter_pos_ != 0) + exit_flag_ = true; + initial_gpio_state_ = !initial_gpio_state_; + } } } // namespace rm_calibration_controllers From 1c0f021a3eac9e7f4c3e3e596e719b2b5d905838 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 11 Mar 2023 15:41:21 +0800 Subject: [PATCH 072/130] Solved the problem of not being in the detection range of the hall switch when starting the calibration. --- .../gpio_calibration_controller.h | 13 ++++++----- .../src/gpio_calibration_controller.cpp | 22 ++++++++++++++----- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h index 553c8b8e..2503ac6c 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -12,11 +12,13 @@ #include #include #include +#include + namespace rm_calibration_controllers { class GpioCalibrationController - : public controller_interface::MultiInterfaceController + : public controller_interface::MultiInterfaceController< + hardware_interface::EffortJointInterface, rm_control::ActuatorExtraInterface, rm_control::GpioStateInterface> { public: GpioCalibrationController() = default; @@ -36,11 +38,12 @@ class GpioCalibrationController CALIBRATED }; int state_{}; - double velocity_search_{}, vel_gain_{}, vel_threshold_{}, enter_pos_{}, exit_pos_{}; - double position_threshold_ = 0.01; + double velocity_search_{}, vel_gain_{}, vel_threshold_{}, position_threshold_{}, enter_pos_{}, exit_pos_{}; bool initial_gpio_state_ = false, enter_flag_ = false, exit_flag_ = false, can_returned_ = false, - is_returned_ = false; + is_returned_ = false, skip_ = false; rm_control::ActuatorExtraHandle actuator_; + rm_control::GpioStateHandle gpio_state_handle_; + effort_controllers::JointVelocityController velocity_ctrl_; effort_controllers::JointPositionController position_ctrl_; diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index 84930cd7..d2a5bb07 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -14,6 +14,7 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: ros::NodeHandle pos_nh(controller_nh, "position"); velocity_ctrl_.init(robot_hw->get(), vel_nh); position_ctrl_.init(robot_hw->get(), pos_nh); + gpio_state_handle_ = robot_hw->get()->getHandle("calibration"); gpio_sub_ = controller_nh.subscribe("/controllers/gpio_controller/gpio_states", 100, &GpioCalibrationController::gpioStateCB, this); XmlRpc::XmlRpcValue actuator; @@ -38,6 +39,11 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } + if (!controller_nh.getParam("pos_threshold", position_threshold_)) + { + ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) { ROS_ERROR("Initial gpio state was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); @@ -65,7 +71,16 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio { velocity_ctrl_.setCommand(velocity_search_); velocity_ctrl_.update(time, period); - state_ = MOVING_AROUND; + if (gpio_state_handle_.getValue()) + { + velocity_ctrl_.update(time, period); + skip_ = true; + } + else + { + state_ = MOVING_AROUND; + skip_ = false; + } break; } case MOVING_AROUND: @@ -132,11 +147,8 @@ void GpioCalibrationController::gpioStateCB(const rm_msgs::GpioDataConstPtr& msg { if (msg->gpio_state[0] != initial_gpio_state_) { - if (!initial_gpio_state_ && !is_returned_) - { + if (!initial_gpio_state_ && !is_returned_ && !skip_) enter_flag_ = true; - } - if (initial_gpio_state_ && enter_pos_ != 0) exit_flag_ = true; initial_gpio_state_ = !initial_gpio_state_; From fd7700e44fc961e77d934b1fda830e2b17b4386e Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 12 Mar 2023 16:59:54 +0800 Subject: [PATCH 073/130] Add resistance compensation on yaw. --- .../include/rm_gimbal_controllers/gimbal_base.h | 4 ++++ rm_gimbal_controllers/src/gimbal_base.cpp | 10 +++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index bad6aa42..2e5719cc 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -169,6 +169,10 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ae9a4237..a1eba8c0 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -62,6 +62,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; + ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation"); + yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.); + yaw_error_tolerance_ = getParam(resistance_compensation_nh, "error_tolerance", 0.); + k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); chassis_vel_ = std::make_shared(chassis_vel_nh); @@ -389,7 +393,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z()); + double resistance_compensation = 0.; + if (std::abs(yaw_des - ctrl_yaw_.getPosition()) > yaw_error_tolerance_) + resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; + ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + + resistance_compensation); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } From ac3d42441cb690376eaf9e4fc772be3ab54487ac Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 14 Mar 2023 15:14:22 +0800 Subject: [PATCH 074/130] Use gpio handle to replace gpio call back function. --- .../gpio_calibration_controller.h | 5 +-- .../src/gpio_calibration_controller.cpp | 43 +++++++------------ 2 files changed, 17 insertions(+), 31 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h index 2503ac6c..34156755 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -39,8 +39,8 @@ class GpioCalibrationController }; int state_{}; double velocity_search_{}, vel_gain_{}, vel_threshold_{}, position_threshold_{}, enter_pos_{}, exit_pos_{}; - bool initial_gpio_state_ = false, enter_flag_ = false, exit_flag_ = false, can_returned_ = false, - is_returned_ = false, skip_ = false; + bool initial_gpio_state_ = false, can_returned_ = false, is_returned_ = false, skip_ = false, + last_gpio_state_ = false; rm_control::ActuatorExtraHandle actuator_; rm_control::GpioStateHandle gpio_state_handle_; @@ -48,6 +48,5 @@ class GpioCalibrationController effort_controllers::JointPositionController position_ctrl_; ros::ServiceServer is_calibrated_srv_; - ros::Subscriber gpio_sub_; }; } // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index d2a5bb07..59a0364b 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -15,8 +15,6 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: velocity_ctrl_.init(robot_hw->get(), vel_nh); position_ctrl_.init(robot_hw->get(), pos_nh); gpio_state_handle_ = robot_hw->get()->getHandle("calibration"); - gpio_sub_ = controller_nh.subscribe("/controllers/gpio_controller/gpio_states", 100, - &GpioCalibrationController::gpioStateCB, this); XmlRpc::XmlRpcValue actuator; if (!controller_nh.getParam("actuator", actuator)) { @@ -71,13 +69,15 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio { velocity_ctrl_.setCommand(velocity_search_); velocity_ctrl_.update(time, period); - if (gpio_state_handle_.getValue()) + if (gpio_state_handle_.getValue() != initial_gpio_state_) { - velocity_ctrl_.update(time, period); + last_gpio_state_ = gpio_state_handle_.getValue(); skip_ = true; + velocity_ctrl_.update(time, period); } else { + last_gpio_state_ = gpio_state_handle_.getValue(); state_ = MOVING_AROUND; skip_ = false; } @@ -85,27 +85,25 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio } case MOVING_AROUND: { - if (enter_flag_) + if (gpio_state_handle_.getValue() != last_gpio_state_) { - enter_flag_ = false; - enter_pos_ = velocity_ctrl_.joint_.getPosition(); - } - if (exit_flag_) - { - exit_flag_ = false; - exit_pos_ = velocity_ctrl_.joint_.getPosition(); + if (gpio_state_handle_.getValue() != initial_gpio_state_ && !is_returned_) + { + enter_pos_ = velocity_ctrl_.joint_.getPosition(); + last_gpio_state_ = gpio_state_handle_.getValue(); + } + if (gpio_state_handle_.getValue() == initial_gpio_state_ && enter_pos_ != 0) + { + exit_pos_ = velocity_ctrl_.joint_.getPosition(); + last_gpio_state_ = gpio_state_handle_.getValue(); + } } if (enter_pos_ != 0. && exit_pos_ != 0.) { velocity_ctrl_.setCommand(0.); - can_returned_ = true; - } - if (can_returned_) - { position_ctrl_.setCommand((enter_pos_ + exit_pos_) / 2); enter_pos_ = 0; exit_pos_ = 0; - can_returned_ = false; state_ = RETURN; } velocity_ctrl_.update(time, period); @@ -143,17 +141,6 @@ bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState return true; } -void GpioCalibrationController::gpioStateCB(const rm_msgs::GpioDataConstPtr& msg) -{ - if (msg->gpio_state[0] != initial_gpio_state_) - { - if (!initial_gpio_state_ && !is_returned_ && !skip_) - enter_flag_ = true; - if (initial_gpio_state_ && enter_pos_ != 0) - exit_flag_ = true; - initial_gpio_state_ = !initial_gpio_state_; - } -} } // namespace rm_calibration_controllers PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::GpioCalibrationController, controller_interface::ControllerBase) From 877c46986359ef978e6738e5eab6306bc3263742 Mon Sep 17 00:00:00 2001 From: Edwinlinks <1026085014@qq.com> Date: Thu, 16 Mar 2023 00:25:00 +0800 Subject: [PATCH 075/130] Add manner to fix odom2base by adding outside odometry. --- .../rm_chassis_controllers/chassis_base.h | 8 +++ rm_chassis_controllers/src/chassis_base.cpp | 58 ++++++++++++++++++- 2 files changed, 65 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 629ca5c0..ae82cb83 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -47,6 +47,9 @@ #include #include #include +#include +#include +#include namespace rm_chassis_controllers { @@ -144,6 +147,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @param msg This expresses velocity in free space broken into its linear and angular parts. */ void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg); + void outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; @@ -152,6 +156,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double wheel_base_{}, wheel_track_{}, wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, power_offset_{}; bool enable_odom_tf_ = false; + bool topic_update_ = false; bool publish_odom_tf_ = false; bool state_changed_ = true; enum @@ -167,15 +172,18 @@ class ChassisBase : public controller_interface::MultiInterfaceController ros::Time last_publish_time_; geometry_msgs::TransformStamped odom2base_{}; + tf2::Transform world2odom_; geometry_msgs::Vector3 vel_cmd_{}; // x, y control_toolbox::Pid pid_follow_; std::shared_ptr > odom_pub_; rm_common::TfRtBroadcaster tf_broadcaster_{}; + ros::Subscriber outside_odom_sub_; ros::Subscriber cmd_chassis_sub_; ros::Subscriber cmd_vel_sub_; Command cmd_struct_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; + realtime_tools::RealtimeBuffer odom_buffer_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 4a20b2dd..224c1983 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -103,7 +103,10 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan tf_broadcaster_.init(root_nh); tf_broadcaster_.sendTransform(odom2base_); } + world2odom_.setRotation(tf2::Quaternion::getIdentity()); + outside_odom_sub_ = + controller_nh.subscribe("/Odometry", 10, &ChassisBase::outsideOdomCallback, this); cmd_chassis_sub_ = controller_nh.subscribe("command", 1, &ChassisBase::cmdChassisCallback, this); cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); @@ -308,9 +311,55 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p odom2base_quat.normalize(); odom2base_.transform.rotation = tf2::toMsg(odom2base_quat); } - robot_state_handle_.setTransform(odom2base_, "rm_chassis_controllers"); } + if (topic_update_) + { + auto* odom_msg = odom_buffer_.readFromRT(); + + tf2::Transform world2sensor; + world2sensor.setOrigin( + tf2::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); + world2sensor.setRotation(tf2::Quaternion(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w)); + + if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) + { + tf2::Transform odom2sensor; + try + { + geometry_msgs::TransformStamped tf_msg = + robot_state_handle_.lookupTransform("odom", "livox_frame", odom_msg->header.stamp); + tf2::fromMsg(tf_msg.transform, odom2sensor); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + world2odom_ = world2sensor * odom2sensor.inverse(); + } + tf2::Transform base2sensor; + try + { + geometry_msgs::TransformStamped tf_msg = + robot_state_handle_.lookupTransform("base_link", "livox_frame", odom_msg->header.stamp); + tf2::fromMsg(tf_msg.transform, base2sensor); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); + odom2base_.transform.translation.x = odom2base.getOrigin().x(); + odom2base_.transform.translation.y = odom2base.getOrigin().y(); + odom2base_.transform.translation.z = odom2base.getOrigin().z(); + topic_update_ = false; + } + + robot_state_handle_.setTransform(odom2base_, "rm_chassis_controllers"); + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { if (odom_pub_->trylock()) @@ -391,4 +440,11 @@ void ChassisBase::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg cmd_rt_buffer_.writeFromNonRT(cmd_struct_); } +template +void ChassisBase::outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg) +{ + odom_buffer_.writeFromNonRT(*msg); + topic_update_ = true; +} + } // namespace rm_chassis_controllers From f7bdd803ff6217bd5c1d7d32079901b7c0819a58 Mon Sep 17 00:00:00 2001 From: Edwinlinks <1026085014@qq.com> Date: Thu, 16 Mar 2023 23:28:43 +0800 Subject: [PATCH 076/130] Delete the unused header. --- .../include/rm_chassis_controllers/chassis_base.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index ae82cb83..86943191 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,8 +48,6 @@ #include #include #include -#include -#include namespace rm_chassis_controllers { From 1ca768473a3e2fbd8dc662e5dc4285f477ea60c0 Mon Sep 17 00:00:00 2001 From: Edwinlinks <1026085014@qq.com> Date: Sat, 18 Mar 2023 19:46:50 +0800 Subject: [PATCH 077/130] Modify topic name and add comment. --- rm_chassis_controllers/src/chassis_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 224c1983..73e66e77 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -106,7 +106,7 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan world2odom_.setRotation(tf2::Quaternion::getIdentity()); outside_odom_sub_ = - controller_nh.subscribe("/Odometry", 10, &ChassisBase::outsideOdomCallback, this); + controller_nh.subscribe("/odometry", 10, &ChassisBase::outsideOdomCallback, this); cmd_chassis_sub_ = controller_nh.subscribe("command", 1, &ChassisBase::cmdChassisCallback, this); cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); @@ -323,7 +323,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p world2sensor.setRotation(tf2::Quaternion(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w)); - if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) + if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received { tf2::Transform odom2sensor; try From 06d02088472842411916beeb4f34ab68b39a6874 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 18 Mar 2023 20:23:27 +0800 Subject: [PATCH 078/130] Factor out the calibration controller into a form derived from a base class and modify the controller appropriately. --- rm_calibration_controllers/CMakeLists.txt | 7 +- .../calibration_base.h | 79 ++++++++++ .../gpio_calibration_base.h | 26 ++++ .../gpio_calibration_controller.h | 52 ------- .../hall_switch_calibration_controller.h | 30 ++++ .../joint_calibration_controller.h | 26 +--- .../rm_calibration_controllers_plugins.xml | 4 +- .../src/calibration_base.cpp | 54 +++++++ .../src/gpio_calibration_base.cpp | 41 +++++ .../src/gpio_calibration_controller.cpp | 146 ------------------ .../hall_switch_calibration_controller.cpp | 96 ++++++++++++ .../src/joint_calibration_controller.cpp | 47 +----- 12 files changed, 343 insertions(+), 265 deletions(-) create mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h create mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h delete mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h create mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h create mode 100644 rm_calibration_controllers/src/calibration_base.cpp create mode 100644 rm_calibration_controllers/src/gpio_calibration_base.cpp delete mode 100644 rm_calibration_controllers/src/gpio_calibration_controller.cpp create mode 100644 rm_calibration_controllers/src/hall_switch_calibration_controller.cpp diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 022dbddd..870297f3 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -51,8 +51,11 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/joint_calibration_controller.cpp - src/gpio_calibration_controller.cpp) +add_library(${PROJECT_NAME} + src/joint_calibration_controller.cpp + src/hall_switch_calibration_controller.cpp + src/calibration_base.cpp + src/gpio_calibration_base.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h new file mode 100644 index 00000000..f4e3c9a7 --- /dev/null +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -0,0 +1,79 @@ +// +// Created by guanlin on 23-3-14. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rm_calibration_controllers +{ +template +class CalibrationBase : public controller_interface::MultiInterfaceController +{ +public: + CalibrationBase() = default; + /** @brief Get necessary params from param server. Init joint_calibration_controller. + * + * Get params from param server and check whether these params are set.Init JointVelocityController.Check + * whether threshold is set correctly. + * + * @param robot_hw The robot hardware abstraction. + * @param root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are + * setup (publishers, subscribers, services). + * @param controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific + * configuration resides. + * @return True if init successful, false when failed. + */ + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + /** @brief Execute corresponding action according to current calibration controller state. + * + * Execute corresponding action according to current joint state. If INITIALIZED, target joint will be set + * a vel_search_ and countdown_ to move, and switch state to MOVING. If MOVING, target joint will move until + * current velocity lower than threshold last for a while, and switch state to CALIBRATED. If CALIBRATED, + * target joint velocity will be set to zero and wait for next command. + * + * @param time The current time. + * @param period The time passed since the last call to update. + */ + void update(const ros::Time& time, const ros::Duration& period) override; + /** @brief Switch all of the actuators state to INITIALIZED. + * + * Switch all of the actuator state to INITIALIZED in order to restart the calibration. + * + * @param time The current time. + */ + void starting(const ros::Time& time) override; + +protected: + /** @brief Provide a service to know the state of target actuators. + * + * When requesting to this server, it will return respond about whether target actuators has been calibrated. + * + * @param req The request of knowing the state of target actuators. + * @param resp The respond included the state of target actuators. + * @return True if get respond successfully, false when failed. + */ + bool isCalibrated(control_msgs::QueryCalibrationState::Request& req, + control_msgs::QueryCalibrationState::Response& resp); + ros::ServiceServer is_calibrated_srv_; + enum state + { + INITIALIZED, + CALIBRATED + }; + int state_{}; + double velocity_search_{}; + bool calibration_success_ = false; + rm_control::ActuatorExtraHandle actuator_; + effort_controllers::JointVelocityController velocity_ctrl_; +}; + +} // namespace rm_calibration_controllers \ No newline at end of file diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h new file mode 100644 index 00000000..22c6534a --- /dev/null +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h @@ -0,0 +1,26 @@ +// +// Created by guanlin on 23-3-16. +// + +#pragma once + +#include +#include +#include + +namespace rm_calibration_controllers +{ +class GpioCalibrationBase : public CalibrationBase +{ +public: + GpioCalibrationBase() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + double vel_threshold_{}; + bool last_gpio_state_ = false; + std::vector gpio_state_handles_; + std::vector initial_gpio_states_; +}; +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h deleted file mode 100644 index 34156755..00000000 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ /dev/null @@ -1,52 +0,0 @@ -// -// Created by guanlin on 22-11-7. -// - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rm_calibration_controllers -{ -class GpioCalibrationController - : public controller_interface::MultiInterfaceController< - hardware_interface::EffortJointInterface, rm_control::ActuatorExtraInterface, rm_control::GpioStateInterface> -{ -public: - GpioCalibrationController() = 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 gpioStateCB(const rm_msgs::GpioDataConstPtr& msg); - bool isCalibrated(control_msgs::QueryCalibrationState::Request& req, - control_msgs::QueryCalibrationState::Response& resp); - enum - { - INITIALIZED, - MOVING_AROUND, - RETURN, - CALIBRATED - }; - int state_{}; - double velocity_search_{}, vel_gain_{}, vel_threshold_{}, position_threshold_{}, enter_pos_{}, exit_pos_{}; - bool initial_gpio_state_ = false, can_returned_ = false, is_returned_ = false, skip_ = false, - last_gpio_state_ = false; - rm_control::ActuatorExtraHandle actuator_; - rm_control::GpioStateHandle gpio_state_handle_; - - effort_controllers::JointVelocityController velocity_ctrl_; - effort_controllers::JointPositionController position_ctrl_; - - ros::ServiceServer is_calibrated_srv_; -}; -} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h new file mode 100644 index 00000000..6e5323af --- /dev/null +++ b/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h @@ -0,0 +1,30 @@ +// +// Created by guanlin on 22-11-7. +// + +#pragma once + +#include "rm_calibration_controllers/gpio_calibration_base.h" +#include + +namespace rm_calibration_controllers +{ +class HallSwitchCalibrationController : public GpioCalibrationBase +{ +public: + HallSwitchCalibrationController() = 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; + +private: + enum state + { + MOVING_AROUND = 3, + RETURN + }; + double position_threshold_{}, enter_pos_{}, exit_pos_{}; + bool is_returned_ = false; + + effort_controllers::JointPositionController position_ctrl_; +}; +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 5cf4cd4a..728c3f69 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -37,19 +37,13 @@ #pragma once -#include -#include -#include -#include -#include #include -#include +#include "rm_calibration_controllers/calibration_base.h" namespace rm_calibration_controllers { class JointCalibrationController - : public controller_interface::MultiInterfaceController + : public CalibrationBase { public: JointCalibrationController() = default; @@ -83,7 +77,6 @@ class JointCalibrationController * * @param time The current time. */ - void starting(const ros::Time& time) override; private: /** @brief Provide a service to know the state of target actuators. @@ -94,24 +87,15 @@ class JointCalibrationController * @param resp The respond included the state of target actuators. * @return True if get respond successfully, false when failed. */ - bool isCalibrated(control_msgs::QueryCalibrationState::Request& req, - control_msgs::QueryCalibrationState::Response& resp); - ros::Time last_publish_time_; - ros::ServiceServer is_calibrated_srv_; - // enum { INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED }; for GPIO switch - enum + enum state { - INITIALIZED, - MOVING_POSITIVE, + MOVING_POSITIVE = 3, MOVING_NEGATIVE, - CALIBRATED }; int state_{}, countdown_{}; double velocity_search_{}, target_position_{}, velocity_threshold_{}, position_threshold_{}; double positive_position_{}, negative_position_{}; - bool is_return_{}, is_center_{}, returned_{}; - rm_control::ActuatorExtraHandle actuator_; - effort_controllers::JointVelocityController velocity_ctrl_; + bool is_return_{}, is_center_{}; effort_controllers::JointPositionController position_ctrl_; }; diff --git a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml index 0d8b0e8d..18d7f4cd 100755 --- a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml +++ b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml @@ -3,7 +3,7 @@ - diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp new file mode 100644 index 00000000..fdd68999 --- /dev/null +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -0,0 +1,54 @@ +// +// Created by guanlin on 23-3-14. +// + +#include "rm_calibration_controllers/calibration_base.h" + +namespace rm_calibration_controllers +{ +template class CalibrationBase; +template class CalibrationBase; + +template +bool CalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + ros::NodeHandle vel_nh(controller_nh, "velocity"); + velocity_ctrl_.init(robot_hw->get(), vel_nh); + XmlRpc::XmlRpcValue actuator; + if (!controller_nh.getParam("actuator", actuator)) + { + ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + actuator_ = robot_hw->get()->getHandle(actuator[0]); + if (!controller_nh.getParam("search_velocity", velocity_search_)) + { + ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + // advertise service to check calibration + is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &CalibrationBase::isCalibrated, this); + return true; +} + +template +void CalibrationBase::starting(const ros::Time& time) +{ + actuator_.setCalibrated(false); + state_ = INITIALIZED; + if (actuator_.getCalibrated()) + ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", + velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); +} + +template +bool CalibrationBase::isCalibrated(control_msgs::QueryCalibrationState::Request& req, + control_msgs::QueryCalibrationState::Response& resp) +{ + ROS_DEBUG("Is calibrated service %d", calibration_success_); + resp.is_calibrated = calibration_success_; + return true; +} +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/gpio_calibration_base.cpp b/rm_calibration_controllers/src/gpio_calibration_base.cpp new file mode 100644 index 00000000..f2167050 --- /dev/null +++ b/rm_calibration_controllers/src/gpio_calibration_base.cpp @@ -0,0 +1,41 @@ +// +// Created by guanlin on 23-3-16. +// + +#include "rm_calibration_controllers/gpio_calibration_base.h" + +namespace rm_calibration_controllers +{ +bool GpioCalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + CalibrationBase::init(robot_hw, root_nh, controller_nh); + + XmlRpc::XmlRpcValue gpios, initial_gpio_states; + if (!controller_nh.getParam("gpios", gpios)) + { + ROS_ERROR("No gpios given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("initial_gpio_states", initial_gpio_states)) + { + ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("vel_threshold", vel_threshold_)) + { + ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + for (int i = 0; i < gpios.size(); ++i) + { + ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeString); + ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); + std::string gpio_name = gpios[i]; + rm_control::GpioStateHandle state_handle_ = robot_hw->get()->getHandle(gpio_name); + gpio_state_handles_.push_back(state_handle_); + initial_gpio_states_.push_back(initial_gpio_states[i]); + } + return true; +} +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp deleted file mode 100644 index 59a0364b..00000000 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ /dev/null @@ -1,146 +0,0 @@ -// -// Created by guanlin on 22-11-7. -// - -#include "rm_calibration_controllers/gpio_calibration_controller.h" -#include - -namespace rm_calibration_controllers -{ -bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) -{ - ros::NodeHandle vel_nh(controller_nh, "velocity"); - ros::NodeHandle pos_nh(controller_nh, "position"); - velocity_ctrl_.init(robot_hw->get(), vel_nh); - position_ctrl_.init(robot_hw->get(), pos_nh); - gpio_state_handle_ = robot_hw->get()->getHandle("calibration"); - XmlRpc::XmlRpcValue actuator; - if (!controller_nh.getParam("actuator", actuator)) - { - ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - actuator_ = robot_hw->get()->getHandle(actuator[0]); - if (!controller_nh.getParam("search_velocity", velocity_search_)) - { - ROS_ERROR("Search velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("vel_gain", vel_gain_)) - { - ROS_ERROR("Velocity gain was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("vel_threshold", vel_threshold_)) - { - ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("pos_threshold", position_threshold_)) - { - ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) - { - ROS_ERROR("Initial gpio state was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - // advertise service to check calibration - is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &GpioCalibrationController::isCalibrated, this); - return true; -} - -void GpioCalibrationController::starting(const ros::Time& time) -{ - actuator_.setCalibrated(false); - state_ = INITIALIZED; - if (actuator_.getCalibrated()) - ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", - velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); -} - -void GpioCalibrationController::update(const ros::Time& time, const ros::Duration& period) -{ - switch (state_) - { - case INITIALIZED: - { - velocity_ctrl_.setCommand(velocity_search_); - velocity_ctrl_.update(time, period); - if (gpio_state_handle_.getValue() != initial_gpio_state_) - { - last_gpio_state_ = gpio_state_handle_.getValue(); - skip_ = true; - velocity_ctrl_.update(time, period); - } - else - { - last_gpio_state_ = gpio_state_handle_.getValue(); - state_ = MOVING_AROUND; - skip_ = false; - } - break; - } - case MOVING_AROUND: - { - if (gpio_state_handle_.getValue() != last_gpio_state_) - { - if (gpio_state_handle_.getValue() != initial_gpio_state_ && !is_returned_) - { - enter_pos_ = velocity_ctrl_.joint_.getPosition(); - last_gpio_state_ = gpio_state_handle_.getValue(); - } - if (gpio_state_handle_.getValue() == initial_gpio_state_ && enter_pos_ != 0) - { - exit_pos_ = velocity_ctrl_.joint_.getPosition(); - last_gpio_state_ = gpio_state_handle_.getValue(); - } - } - if (enter_pos_ != 0. && exit_pos_ != 0.) - { - velocity_ctrl_.setCommand(0.); - position_ctrl_.setCommand((enter_pos_ + exit_pos_) / 2); - enter_pos_ = 0; - exit_pos_ = 0; - state_ = RETURN; - } - velocity_ctrl_.update(time, period); - break; - } - case RETURN: - { - is_returned_ = true; - position_ctrl_.update(time, period); - if (((std::abs(position_ctrl_.joint_.getPosition() - position_ctrl_.command_struct_.position_)) < - position_threshold_) && - (position_ctrl_.joint_.getVelocity() < vel_threshold_)) - { - actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); - actuator_.setCalibrated(true); - ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); - state_ = CALIBRATED; - } - break; - } - case CALIBRATED: - { - is_returned_ = false; - position_ctrl_.setCommand(position_ctrl_.joint_.getPosition()); - position_ctrl_.update(time, period); - } - } -} - -bool GpioCalibrationController::isCalibrated(control_msgs::QueryCalibrationState::Request& req, - control_msgs::QueryCalibrationState::Response& resp) -{ - ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED); - resp.is_calibrated = (state_ == CALIBRATED); - return true; -} - -} // namespace rm_calibration_controllers - -PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::GpioCalibrationController, controller_interface::ControllerBase) diff --git a/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp b/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp new file mode 100644 index 00000000..14a734a2 --- /dev/null +++ b/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp @@ -0,0 +1,96 @@ +// +// Created by guanlin on 22-11-7. +// + +#include "rm_calibration_controllers/hall_switch_calibration_controller.h" +#include + +namespace rm_calibration_controllers +{ +bool HallSwitchCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + GpioCalibrationBase::init(robot_hw, root_nh, controller_nh); + ros::NodeHandle pos_nh(controller_nh, "position"); + position_ctrl_.init(robot_hw->get(), pos_nh); + if (!controller_nh.getParam("pos_threshold", position_threshold_)) + { + ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + return true; +} + +void HallSwitchCalibrationController::update(const ros::Time& time, const ros::Duration& period) +{ + switch (state_) + { + case INITIALIZED: + { + velocity_ctrl_.setCommand(velocity_search_); + velocity_ctrl_.update(time, period); + if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) + { + last_gpio_state_ = gpio_state_handles_[0].getValue(); + velocity_ctrl_.update(time, period); + } + else + { + last_gpio_state_ = gpio_state_handles_[0].getValue(); + state_ = MOVING_AROUND; + } + break; + } + case MOVING_AROUND: + { + if (gpio_state_handles_[0].getValue() != last_gpio_state_) + { + if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0] && !is_returned_) + { + enter_pos_ = velocity_ctrl_.joint_.getPosition(); + last_gpio_state_ = gpio_state_handles_[0].getValue(); + } + if (gpio_state_handles_[0].getValue() == initial_gpio_states_[0] && enter_pos_ != 0) + { + exit_pos_ = velocity_ctrl_.joint_.getPosition(); + last_gpio_state_ = gpio_state_handles_[0].getValue(); + } + } + if (enter_pos_ != 0. && exit_pos_ != 0.) + { + velocity_ctrl_.setCommand(0.); + position_ctrl_.setCommand((enter_pos_ + exit_pos_) / 2); + enter_pos_ = 0; + exit_pos_ = 0; + state_ = RETURN; + } + velocity_ctrl_.update(time, period); + break; + } + case RETURN: + { + is_returned_ = true; + position_ctrl_.update(time, period); + if (((std::abs(position_ctrl_.joint_.getPosition() - position_ctrl_.command_struct_.position_)) < + position_threshold_) && + (position_ctrl_.joint_.getVelocity() < vel_threshold_)) + { + actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); + actuator_.setCalibrated(true); + ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); + state_ = CALIBRATED; + } + break; + } + case CALIBRATED: + { + is_returned_ = false; + calibration_success_ = true; + position_ctrl_.setCommand(position_ctrl_.joint_.getPosition()); + position_ctrl_.update(time, period); + } + } +} +} // namespace rm_calibration_controllers + +PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::HallSwitchCalibrationController, controller_interface::ControllerBase) diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index 68724635..ac0d9cac 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -44,26 +44,9 @@ namespace rm_calibration_controllers bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - velocity_ctrl_.init(robot_hw->get(), controller_nh); - XmlRpc::XmlRpcValue actuator; + CalibrationBase::init(robot_hw, root_nh, controller_nh); is_return_ = is_center_ = false; controller_nh.getParam("center", is_center_); - if (!controller_nh.getParam("actuator", actuator)) - { - ROS_ERROR("No actuator given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - actuator_ = robot_hw->get()->getHandle(actuator[0]); - if (!controller_nh.getParam("search_velocity", velocity_search_)) - { - ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("threshold", velocity_threshold_)) - { - ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } if (velocity_threshold_ < 0) { velocity_threshold_ *= -1.; @@ -85,22 +68,11 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros return false; } is_return_ = true; - returned_ = false; + calibration_success_ = false; } - // advertise service to check calibration - is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &JointCalibrationController::isCalibrated, this); return true; } -void JointCalibrationController::starting(const ros::Time& time) -{ - actuator_.setCalibrated(false); - state_ = INITIALIZED; - if (actuator_.getCalibrated()) - ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", - velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); -} - void JointCalibrationController::update(const ros::Time& time, const ros::Duration& period) { // TODO: Add GPIO switch support @@ -135,7 +107,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati else { velocity_ctrl_.joint_.setCommand(0.); - returned_ = true; + calibration_success_ = true; } } else @@ -168,7 +140,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati else { velocity_ctrl_.joint_.setCommand(0.); - returned_ = true; + calibration_success_ = true; } } velocity_ctrl_.update(time, period); @@ -179,7 +151,7 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati if (is_return_) { if ((std::abs(position_ctrl_.joint_.getPosition() - target_position_)) < position_threshold_) - returned_ = true; + calibration_success_ = true; position_ctrl_.update(time, period); } else @@ -188,15 +160,6 @@ void JointCalibrationController::update(const ros::Time& time, const ros::Durati } } } - -bool JointCalibrationController::isCalibrated(control_msgs::QueryCalibrationState::Request& req, - control_msgs::QueryCalibrationState::Response& resp) -{ - ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED && returned_); - resp.is_calibrated = (state_ == CALIBRATED && returned_); - return true; -} - } // namespace rm_calibration_controllers PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::JointCalibrationController, controller_interface::ControllerBase) From 2f8b4506acfba0ac30d3964da831e613e65f1c96 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 18 Mar 2023 21:55:13 +0800 Subject: [PATCH 079/130] Add a new line at the end and delete update function of calibration_base.h file. --- .../rm_calibration_controllers/calibration_base.h | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index f4e3c9a7..95f7fb0e 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -33,17 +33,6 @@ class CalibrationBase : public controller_interface::MultiInterfaceController Date: Sun, 19 Mar 2023 13:18:28 +0800 Subject: [PATCH 080/130] Publish shoot state. --- .../include/rm_shooter_controllers/standard.h | 2 ++ rm_shooter_controllers/src/standard.cpp | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index f451f743..d37f4a39 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -103,6 +104,7 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer; realtime_tools::RealtimeBuffer cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; + std::shared_ptr> shoot_state_pub_; ros::Subscriber cmd_subscriber_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 4bd2f71e..5ee310f1 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -63,6 +63,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro push_qd_threshold_ = getParam(controller_nh, "push_qd_threshold", 0.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); + 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) { @@ -119,6 +120,12 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) block(time, period); break; } + if (shoot_state_pub_->trylock()) + { + shoot_state_pub_->msg_.stamp = time; + shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->unlockAndPublish(); + } ctrl_friction_l_.update(time, period); ctrl_friction_r_.update(time, period); ctrl_trigger_.update(time, period); From 88b0065b2b518dbcae1176c354cd818ad1a3b34e Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 19 Mar 2023 14:44:36 +0800 Subject: [PATCH 081/130] Modified hardware interface for instantiating template classes. --- .../rm_calibration_controllers/gpio_calibration_base.h | 2 +- .../rm_calibration_controllers/joint_calibration_controller.h | 4 ++-- rm_calibration_controllers/src/calibration_base.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h index 22c6534a..289f4b54 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h @@ -10,7 +10,7 @@ namespace rm_calibration_controllers { -class GpioCalibrationBase : public CalibrationBase { public: diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 728c3f69..96d7b847 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -43,7 +43,7 @@ namespace rm_calibration_controllers { class JointCalibrationController - : public CalibrationBase + : public CalibrationBase { public: JointCalibrationController() = default; @@ -93,7 +93,7 @@ class JointCalibrationController MOVING_NEGATIVE, }; int state_{}, countdown_{}; - double velocity_search_{}, target_position_{}, velocity_threshold_{}, position_threshold_{}; + double target_position_{}, velocity_threshold_{}, position_threshold_{}; double positive_position_{}, negative_position_{}; bool is_return_{}, is_center_{}; effort_controllers::JointPositionController position_ctrl_; diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp index fdd68999..7197dd06 100644 --- a/rm_calibration_controllers/src/calibration_base.cpp +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -6,8 +6,8 @@ namespace rm_calibration_controllers { -template class CalibrationBase; -template class CalibrationBase; +template class CalibrationBase; template From 6ecd5b965570e6758e8467c3be05f70ed97471e3 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 19 Mar 2023 17:18:16 +0800 Subject: [PATCH 082/130] Delete some comments, modify the initialization function of the gpio calibration controller base class. --- .../joint_calibration_controller.h | 14 -------------- .../src/gpio_calibration_base.cpp | 4 ++-- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 96d7b847..9913b361 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -71,22 +71,8 @@ class JointCalibrationController * @param period The time passed since the last call to update. */ void update(const ros::Time& time, const ros::Duration& period) override; - /** @brief Switch all of the actuators state to INITIALIZED. - * - * Switch all of the actuator state to INITIALIZED in order to restart the calibration. - * - * @param time The current time. - */ private: - /** @brief Provide a service to know the state of target actuators. - * - * When requesting to this server, it will return respond about whether target actuators has been calibrated. - * - * @param req The request of knowing the state of target actuators. - * @param resp The respond included the state of target actuators. - * @return True if get respond successfully, false when failed. - */ enum state { MOVING_POSITIVE = 3, diff --git a/rm_calibration_controllers/src/gpio_calibration_base.cpp b/rm_calibration_controllers/src/gpio_calibration_base.cpp index f2167050..a1dfb232 100644 --- a/rm_calibration_controllers/src/gpio_calibration_base.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_base.cpp @@ -27,9 +27,9 @@ bool GpioCalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeH ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - for (int i = 0; i < gpios.size(); ++i) + for (int i = 0; i < gpios.size(); i++) { - ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeString); + ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); std::string gpio_name = gpios[i]; rm_control::GpioStateHandle state_handle_ = robot_hw->get()->getHandle(gpio_name); From 15fd8bb41a4f8105708cf5b4989f1cf793ba5c53 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 19 Mar 2023 17:20:05 +0800 Subject: [PATCH 083/130] Modify variable name. --- rm_calibration_controllers/src/gpio_calibration_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_calibration_controllers/src/gpio_calibration_base.cpp b/rm_calibration_controllers/src/gpio_calibration_base.cpp index a1dfb232..8c23dd62 100644 --- a/rm_calibration_controllers/src/gpio_calibration_base.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_base.cpp @@ -32,8 +32,8 @@ bool GpioCalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeH ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); std::string gpio_name = gpios[i]; - rm_control::GpioStateHandle state_handle_ = robot_hw->get()->getHandle(gpio_name); - gpio_state_handles_.push_back(state_handle_); + rm_control::GpioStateHandle state_handle = robot_hw->get()->getHandle(gpio_name); + gpio_state_handles_.push_back(state_handle); initial_gpio_states_.push_back(initial_gpio_states[i]); } return true; From cc042abdf69ddae06b492f299c4fa9e984728bb5 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 19 Mar 2023 17:21:54 +0800 Subject: [PATCH 084/130] Modify the name of an enumeration type. --- .../include/rm_calibration_controllers/calibration_base.h | 2 +- .../hall_switch_calibration_controller.h | 2 +- .../rm_calibration_controllers/joint_calibration_controller.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index 95f7fb0e..556403db 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -53,7 +53,7 @@ class CalibrationBase : public controller_interface::MultiInterfaceController Date: Sun, 19 Mar 2023 17:24:17 +0800 Subject: [PATCH 085/130] Modify the name of an enumeration type. --- .../include/rm_calibration_controllers/calibration_base.h | 2 +- .../hall_switch_calibration_controller.h | 2 +- .../rm_calibration_controllers/joint_calibration_controller.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index 556403db..8e914f80 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -53,7 +53,7 @@ class CalibrationBase : public controller_interface::MultiInterfaceController Date: Mon, 20 Mar 2023 15:58:46 +0800 Subject: [PATCH 086/130] Write the velocity threshold in the base class. --- .../rm_calibration_controllers/calibration_base.h | 2 +- .../gpio_calibration_base.h | 1 - .../joint_calibration_controller.h | 2 +- rm_calibration_controllers/src/calibration_base.cpp | 11 +++++++++++ .../src/gpio_calibration_base.cpp | 5 ----- .../src/hall_switch_calibration_controller.cpp | 2 +- .../src/joint_calibration_controller.cpp | 8 +------- 7 files changed, 15 insertions(+), 16 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index 8e914f80..b20b09af 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -59,7 +59,7 @@ class CalibrationBase : public controller_interface::MultiInterfaceController gpio_state_handles_; std::vector initial_gpio_states_; diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h index 31965d52..013920fa 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h @@ -79,7 +79,7 @@ class JointCalibrationController MOVING_NEGATIVE, }; int state_{}, countdown_{}; - double target_position_{}, velocity_threshold_{}, position_threshold_{}; + double target_position_{}, position_threshold_{}; double positive_position_{}, negative_position_{}; bool is_return_{}, is_center_{}; effort_controllers::JointPositionController position_ctrl_; diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp index 7197dd06..2577140c 100644 --- a/rm_calibration_controllers/src/calibration_base.cpp +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -28,6 +28,17 @@ bool CalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::Nod ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } + if (!controller_nh.getParam("vel_threshold", velocity_threshold_)) + { + ROS_ERROR("Position value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (velocity_threshold_ < 0) + { + velocity_threshold_ *= -1.; + ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", + velocity_ctrl_.getJointName().c_str()); + } // advertise service to check calibration is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &CalibrationBase::isCalibrated, this); return true; diff --git a/rm_calibration_controllers/src/gpio_calibration_base.cpp b/rm_calibration_controllers/src/gpio_calibration_base.cpp index 8c23dd62..5948c4e9 100644 --- a/rm_calibration_controllers/src/gpio_calibration_base.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_base.cpp @@ -22,11 +22,6 @@ bool GpioCalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeH ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("vel_threshold", vel_threshold_)) - { - ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } for (int i = 0; i < gpios.size(); i++) { ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); diff --git a/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp b/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp index 14a734a2..60119e25 100644 --- a/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp +++ b/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp @@ -73,7 +73,7 @@ void HallSwitchCalibrationController::update(const ros::Time& time, const ros::D position_ctrl_.update(time, period); if (((std::abs(position_ctrl_.joint_.getPosition() - position_ctrl_.command_struct_.position_)) < position_threshold_) && - (position_ctrl_.joint_.getVelocity() < vel_threshold_)) + (position_ctrl_.joint_.getVelocity() < velocity_threshold_)) { actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); actuator_.setCalibrated(true); diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/joint_calibration_controller.cpp index ac0d9cac..584cfac6 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/joint_calibration_controller.cpp @@ -47,12 +47,6 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros CalibrationBase::init(robot_hw, root_nh, controller_nh); is_return_ = is_center_ = false; controller_nh.getParam("center", is_center_); - if (velocity_threshold_ < 0) - { - velocity_threshold_ *= -1.; - ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", - velocity_ctrl_.getJointName().c_str()); - } if (controller_nh.hasParam("return")) { ros::NodeHandle nh_return(controller_nh, "return"); @@ -62,7 +56,7 @@ bool JointCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("threshold", position_threshold_)) + if (!controller_nh.getParam("pos_threshold", position_threshold_)) { ROS_ERROR("Position value was not specified (namespace: %s)", nh_return.getNamespace().c_str()); return false; From f0c0e56d6f52efcdfffde8637f259cae0758caf3 Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Mon, 20 Mar 2023 21:58:36 +0800 Subject: [PATCH 087/130] Add balance auto exit block. --- .../include/rm_chassis_controllers/balance.h | 11 + rm_chassis_controllers/src/balance.cpp | 188 +++++++++++++----- 2 files changed, 154 insertions(+), 45 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index cc1fdd0e..e7234995 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -17,6 +17,12 @@ using Eigen::Matrix; class BalanceController : public ChassisBase { + enum balanceState + { + NORMAL, + BLOCK + }; + public: BalanceController() = default; bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; @@ -37,6 +43,11 @@ class BalanceController : public ChassisBase(q[i]); + } else if (q[i].getType() == XmlRpc::XmlRpcValue::TypeInt) + { q_(i, i) = static_cast(q[i]); + } } // Check and get R ROS_ASSERT(r.getType() == XmlRpc::XmlRpcValue::TypeArray); @@ -130,9 +159,13 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan { ROS_ASSERT(r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble || r[i].getType() == XmlRpc::XmlRpcValue::TypeInt); if (r[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) + { r_(i, i) = static_cast(r[i]); + } else if (r[i].getType() == XmlRpc::XmlRpcValue::TypeInt) + { r_(i, i) = static_cast(r[i]); + } } // Continuous model \dot{x} = A x + B u @@ -276,6 +309,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ROS_INFO_STREAM("K of LQR:" << k_); state_pub_ = root_nh.advertise("/state", 10); + balance_state_ = balanceState::NORMAL; + return true; } @@ -319,56 +354,119 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe odom2imu.setOrigin(odom2imu_origin); odom2imu.setRotation(odom2imu_quaternion); odom2base = odom2imu * imu2base; + double roll, pitch, yaw; quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); - x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - - imu_handle_.getAngularVelocity()[1]) * - wheel_radius_; - x_[0] += x_[5] * period.toSec(); - x_[1] = yaw; - x_[2] = pitch; - x_[3] = left_momentum_block_joint_handle_.getPosition(); - x_[4] = right_momentum_block_joint_handle_.getPosition(); - x_[6] = angular_vel_base.z; - x_[7] = angular_vel_base.y; - x_[8] = left_momentum_block_joint_handle_.getVelocity(); - x_[9] = right_momentum_block_joint_handle_.getVelocity(); - yaw_des_ += vel_cmd_.z * period.toSec(); - position_des_ += vel_cmd_.x * period.toSec(); - Eigen::Matrix u; - auto x = x_; - x(0) -= position_des_; - x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); - x(5) -= vel_cmd_.x; - x(6) -= vel_cmd_.z; - if (std::abs(x(0) + position_offset_) > position_clear_threshold_) + + // Check block + if (balance_state_ != balanceState::BLOCK) { - x_[0] = 0.; - position_des_ = position_offset_; + if ((std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > + block_effort_ && + (left_wheel_joint_handle_.getVelocity() < 0.5 || right_wheel_joint_handle_.getVelocity() < 0.5)) + { + if (!maybe_block_) + { + block_time_ = time; + maybe_block_ = true; + } + if ((time - block_time_).toSec() >= block_duration_) + { + balance_state_ = balanceState::BLOCK; + balance_state_changed_ = true; + ROS_INFO("[balance] Exit NOMAl"); + } + } + else + { + maybe_block_ = false; + } } - u = k_ * (-x); - rm_msgs::BalanceState state; - state.header.stamp = time; - state.x = x(0); - state.phi = x(1); - state.theta = x(2); - state.x_b_l = x(3); - state.x_b_r = x(4); - state.x_dot = x(5); - state.phi_dot = x(6); - state.theta_dot = x(7); - state.x_b_l_dot = x(8); - state.x_b_r_dot = x(9); - state.T_l = u(0); - state.T_r = u(1); - state.f_b_l = u(2); - state.f_b_r = u(3); - state_pub_.publish(state); - left_wheel_joint_handle_.setCommand(u(0)); - right_wheel_joint_handle_.setCommand(u(1)); - left_momentum_block_joint_handle_.setCommand(u(2)); - right_momentum_block_joint_handle_.setCommand(u(3)); + switch (balance_state_) + { + case balanceState::NORMAL: + { + if (balance_state_changed_) + { + ROS_INFO("[balance] Enter NOMAl"); + balance_state_changed_ = false; + } + x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - + imu_handle_.getAngularVelocity()[1]) * + wheel_radius_; + x_[0] += x_[5] * period.toSec(); + x_[1] = yaw; + x_[2] = pitch; + x_[3] = left_momentum_block_joint_handle_.getPosition(); + x_[4] = right_momentum_block_joint_handle_.getPosition(); + x_[6] = angular_vel_base.z; + x_[7] = angular_vel_base.y; + x_[8] = left_momentum_block_joint_handle_.getVelocity(); + x_[9] = right_momentum_block_joint_handle_.getVelocity(); + yaw_des_ += vel_cmd_.z * period.toSec(); + position_des_ += vel_cmd_.x * period.toSec(); + Eigen::Matrix u; + auto x = x_; + x(0) -= position_des_; + x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); + x(5) -= vel_cmd_.x; + x(6) -= vel_cmd_.z; + if (std::abs(x(0) + position_offset_) > position_clear_threshold_) + { + x_[0] = 0.; + position_des_ = position_offset_; + } + u = k_ * (-x); + rm_msgs::BalanceState state; + state.header.stamp = time; + state.x = x(0); + state.phi = x(1); + state.theta = x(2); + state.x_b_l = x(3); + state.x_b_r = x(4); + state.x_dot = x(5); + state.phi_dot = x(6); + state.theta_dot = x(7); + state.x_b_l_dot = x(8); + state.x_b_r_dot = x(9); + state.T_l = u(0); + state.T_r = u(1); + state.f_b_l = u(2); + state.f_b_r = u(3); + state_pub_.publish(state); + + left_wheel_joint_handle_.setCommand(u(0)); + right_wheel_joint_handle_.setCommand(u(1)); + left_momentum_block_joint_handle_.setCommand(u(2)); + right_momentum_block_joint_handle_.setCommand(u(3)); + break; + } + case balanceState::BLOCK: + { + if (balance_state_changed_) + { + ROS_INFO("[balance] Enter BLOCK"); + balance_state_changed_ = false; + + last_block_time_ = ros::Time::now(); + } + if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) + { + balance_state_ = balanceState::NORMAL; + balance_state_changed_ = true; + ROS_INFO("[balance] Exit BLOCK"); + } + else + { + left_momentum_block_joint_handle_.setCommand(pitch > 0 ? -80 : 80); + right_momentum_block_joint_handle_.setCommand(pitch > 0 ? -80 : 80); + left_wheel_joint_handle_.setCommand(pitch > 0 ? -anti_block_effort_ : anti_block_effort_); + right_wheel_joint_handle_.setCommand(pitch > 0 ? -anti_block_effort_ : anti_block_effort_); + } + break; + } + } } geometry_msgs::Twist BalanceController::odometry() From 6a2dae5bfda5df5f6e72be69d796679c30c19854 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 21 Mar 2023 14:56:40 +0800 Subject: [PATCH 088/130] Rename joint calibration controller. --- rm_calibration_controllers/CMakeLists.txt | 2 +- ...bration_controller.h => mechanical_calibration_controller.h} | 0 ...ion_controller.cpp => mechanical_calibration_controller.cpp} | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename rm_calibration_controllers/include/rm_calibration_controllers/{joint_calibration_controller.h => mechanical_calibration_controller.h} (100%) rename rm_calibration_controllers/src/{joint_calibration_controller.cpp => mechanical_calibration_controller.cpp} (98%) diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 870297f3..b2b3ac7b 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -52,7 +52,7 @@ include_directories( ## Declare a cpp library add_library(${PROJECT_NAME} - src/joint_calibration_controller.cpp + src/mechanical_calibration_controller.cpp src/hall_switch_calibration_controller.cpp src/calibration_base.cpp src/gpio_calibration_base.cpp) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h similarity index 100% rename from rm_calibration_controllers/include/rm_calibration_controllers/joint_calibration_controller.h rename to rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h diff --git a/rm_calibration_controllers/src/joint_calibration_controller.cpp b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp similarity index 98% rename from rm_calibration_controllers/src/joint_calibration_controller.cpp rename to rm_calibration_controllers/src/mechanical_calibration_controller.cpp index 584cfac6..2db3a734 100644 --- a/rm_calibration_controllers/src/joint_calibration_controller.cpp +++ b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp @@ -35,7 +35,7 @@ // Created by qiayuan on 5/16/21. // -#include "rm_calibration_controllers/joint_calibration_controller.h" +#include "rm_calibration_controllers/mechanical_calibration_controller.h" #include From fca2b17238ae01f3cc826c47bff2823e8790a0a1 Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Tue, 21 Mar 2023 15:17:41 +0800 Subject: [PATCH 089/130] Update auto exit block. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index e7234995..be6b635a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -45,7 +45,7 @@ class BalanceController : public ChassisBase block_effort_ && - (left_wheel_joint_handle_.getVelocity() < 0.5 || right_wheel_joint_handle_.getVelocity() < 0.5)) + (left_wheel_joint_handle_.getVelocity() < block_velocity_ || + right_wheel_joint_handle_.getVelocity() < block_velocity_)) { if (!maybe_block_) { @@ -410,7 +411,8 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe auto x = x_; x(0) -= position_des_; x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); - x(5) -= vel_cmd_.x; + if (state_ != GYRO) + x(5) -= vel_cmd_.x; x(6) -= vel_cmd_.z; if (std::abs(x(0) + position_offset_) > position_clear_threshold_) { From aea3e6800940deeea3023442dd6f1f8eb994666c Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 21 Mar 2023 15:46:32 +0800 Subject: [PATCH 090/130] Modify the unmodified name in the joint calibration controller. --- .../mechanical_calibration_controller.h | 4 ++-- .../rm_calibration_controllers_plugins.xml | 4 ++-- .../src/mechanical_calibration_controller.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h index 013920fa..4876430f 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h @@ -42,11 +42,11 @@ namespace rm_calibration_controllers { -class JointCalibrationController +class MechanicalCalibrationController : public CalibrationBase { public: - JointCalibrationController() = default; + MechanicalCalibrationController() = default; /** @brief Get necessary params from param server. Init joint_calibration_controller. * * Get params from param server and check whether these params are set.Init JointVelocityController.Check diff --git a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml index 18d7f4cd..9679200c 100755 --- a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml +++ b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml @@ -1,7 +1,7 @@ - Date: Fri, 24 Mar 2023 14:00:02 +0800 Subject: [PATCH 091/130] Balance auto exit block add pitch limit. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index be6b635a..bdc3ef85 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -45,7 +45,7 @@ class BalanceController : public ChassisBase + if (std::abs(pitch) > block_angle_ && + (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > block_effort_ && (left_wheel_joint_handle_.getVelocity() < block_velocity_ || right_wheel_joint_handle_.getVelocity() < block_velocity_)) From 2ed512aa1842e3c54dcf359b066373441895f22e Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Fri, 24 Mar 2023 14:04:06 +0800 Subject: [PATCH 092/130] Update GYRO to RAW and enum rename. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index bdc3ef85..730c48d9 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -17,7 +17,7 @@ using Eigen::Matrix; class BalanceController : public ChassisBase { - enum balanceState + enum BalanceState { NORMAL, BLOCK diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 33269544..eb547847 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -314,7 +314,7 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ROS_INFO_STREAM("K of LQR:" << k_); state_pub_ = root_nh.advertise("/state", 10); - balance_state_ = balanceState::NORMAL; + balance_state_ = BalanceState::NORMAL; return true; } @@ -364,7 +364,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); // Check block - if (balance_state_ != balanceState::BLOCK) + if (balance_state_ != BalanceState::BLOCK) { if (std::abs(pitch) > block_angle_ && (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > @@ -379,7 +379,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe } if ((time - block_time_).toSec() >= block_duration_) { - balance_state_ = balanceState::BLOCK; + balance_state_ = BalanceState::BLOCK; balance_state_changed_ = true; ROS_INFO("[balance] Exit NOMAl"); } @@ -392,7 +392,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe switch (balance_state_) { - case balanceState::NORMAL: + case BalanceState::NORMAL: { if (balance_state_changed_) { @@ -417,7 +417,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe auto x = x_; x(0) -= position_des_; x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); - if (state_ != GYRO) + if (state_ != RAW) x(5) -= vel_cmd_.x; x(6) -= vel_cmd_.z; if (std::abs(x(0) + position_offset_) > position_clear_threshold_) @@ -450,7 +450,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe right_momentum_block_joint_handle_.setCommand(u(3)); break; } - case balanceState::BLOCK: + case BalanceState::BLOCK: { if (balance_state_changed_) { @@ -461,7 +461,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe } if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) { - balance_state_ = balanceState::NORMAL; + balance_state_ = BalanceState::NORMAL; balance_state_changed_ = true; ROS_INFO("[balance] Exit BLOCK"); } From f5116eb81cef5c008d80b80781863122075be2e0 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 25 Mar 2023 01:10:23 +0800 Subject: [PATCH 093/130] 0.1.10 --- gpio_controller/CHANGELOG.rst | 6 +++++ mimic_joint_controller/CHANGELOG.rst | 6 +++++ rm_calibration_controllers/CHANGELOG.rst | 6 +++++ rm_chassis_controllers/CHANGELOG.rst | 28 ++++++++++++++++++++++++ rm_controllers/CHANGELOG.rst | 6 +++++ rm_gimbal_controllers/CHANGELOG.rst | 13 +++++++++++ rm_orientation_controller/CHANGELOG.rst | 6 +++++ rm_shooter_controllers/CHANGELOG.rst | 12 ++++++++++ robot_state_controller/CHANGELOG.rst | 6 +++++ tof_radar_controller/CHANGELOG.rst | 6 +++++ 10 files changed, 95 insertions(+) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index cb28ae52..09a8f9ce 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 5ac073c3..093e8ada 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 2e46f00d..98375adc 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge pull request `#103 `_ from L-SY/fix_return diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 4aebf534..a9e58a55 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#112 `_ from ljq-lv/Delete + Delete the chassis mode "GYRO" +* Merge pull request `#115 `_ from Edwinlinks/fix-odom + Add manner to fix odom2base by adding outside odometry. +* Merge pull request `#119 `_ from ye-luo-xi-tui/balance_standard + Clear position when position_des minus position_act bigger than threshold +* Modify topic name and add comment. +* Delete the unused header. +* Add manner to fix odom2base by adding outside odometry. +* Merge branch 'rm-controls:master' into master +* Modified the chassis's README +* Delete the chassis mode "GYRO" +* Merge pull request `#111 `_ from d0h0s/master + Updated README.md of rm_chassis_controllers. +* Repaired the example of chassis_controller. +* Added the parameters of Omni and fixed the inappropriate description. +* Updated README.md of rm_chassis_controllers. +* Merge pull request `#108 `_ from Aung-xiao/master + add mecanum.yaml +* change the name of the controller +* add mecanum.yaml +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Clear position when position_des minus position_act bigger than threshold. +* Contributors: Aung-xiao, Edwinlinks, QiayuanLiao, d0h0s, ljq-lv, ye-luo-xi-tui, yezi + 0.1.9 (2023-02-21) ------------------ * Merge pull request `#105 `_ from ljq-lv/gimbal_toward diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index 1c2c6986..81759afb 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 5551ce62..a9f871de 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'rm-controls:master' into master +* Merge pull request `#114 `_ from ye-luo-xi-tui/resistance_compensation + Add resistance compensation on yaw +* Add resistance compensation on yaw. +* Merge pull request `#113 `_ from ye-luo-xi-tui/master + Use Vector3WithFilter in rm_common instead +* Use Vector3WithFilter in rm_common instead. +* 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 pull request `#104 `_ from ye-luo-xi-tui/balance_standard diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 5515ce7f..dbb9f4e2 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index 6297cd0b..8c9b0906 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index c7200f9a..c905de0b 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 05470e50..80d4f6dd 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#106 `_ from ye-luo-xi-tui/master + 0.1.9 +* Contributors: ye-luo-xi-tui + 0.1.9 (2023-02-21) ------------------ * Merge branch 'master' into balance_standard From 7d82545ec251cb28b5727abe94132d9e8fa3878f Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sat, 25 Mar 2023 01:10:40 +0800 Subject: [PATCH 094/130] 0.1.10 --- gpio_controller/CHANGELOG.rst | 4 ++-- gpio_controller/package.xml | 2 +- mimic_joint_controller/CHANGELOG.rst | 4 ++-- mimic_joint_controller/package.xml | 2 +- rm_calibration_controllers/CHANGELOG.rst | 4 ++-- rm_calibration_controllers/package.xml | 2 +- rm_chassis_controllers/CHANGELOG.rst | 4 ++-- rm_chassis_controllers/package.xml | 2 +- rm_controllers/CHANGELOG.rst | 4 ++-- rm_controllers/package.xml | 2 +- rm_gimbal_controllers/CHANGELOG.rst | 4 ++-- rm_gimbal_controllers/package.xml | 2 +- rm_orientation_controller/CHANGELOG.rst | 4 ++-- rm_orientation_controller/package.xml | 2 +- rm_shooter_controllers/CHANGELOG.rst | 4 ++-- rm_shooter_controllers/package.xml | 2 +- robot_state_controller/CHANGELOG.rst | 4 ++-- robot_state_controller/package.xml | 2 +- tof_radar_controller/CHANGELOG.rst | 4 ++-- tof_radar_controller/package.xml | 2 +- 20 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 09a8f9ce..45acb927 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index e8dc48ee..da7ace44 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -1,7 +1,7 @@ gpio_controller - 0.1.9 + 0.1.10 The gpio_controller package muyuexin diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 093e8ada..574cf3cd 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 29ecf2de..7857d1c6 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -1,7 +1,7 @@ mimic_joint_controller - 0.1.9 + 0.1.10 The mimic_joint_controller package ljq diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index 98375adc..accbd53c 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index 1af87d6f..ef2e985f 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -1,7 +1,7 @@ rm_calibration_controllers - 0.1.9 + 0.1.10 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index a9e58a55..9e8e27ee 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#112 `_ from ljq-lv/Delete Delete the chassis mode "GYRO" * Merge pull request `#115 `_ from Edwinlinks/fix-odom diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index e361d35d..59876d7a 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -1,7 +1,7 @@ rm_chassis_controllers - 0.1.9 + 0.1.10 RoboMaster standard robot Chassis controller Qiayuan Liao BSD diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index 81759afb..e64bc710 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/rm_controllers/package.xml b/rm_controllers/package.xml index e555326d..ecba2ff1 100644 --- a/rm_controllers/package.xml +++ b/rm_controllers/package.xml @@ -1,7 +1,7 @@ rm_controllers - 0.1.9 + 0.1.10 Meta package that contains package for RoboMaster. Qiayuan Liao diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index a9f871de..40e590fb 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge branch 'rm-controls:master' into master * Merge pull request `#114 `_ from ye-luo-xi-tui/resistance_compensation Add resistance compensation on yaw diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index 962d0ba3..b9ac9e1e 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -1,7 +1,7 @@ rm_gimbal_controllers - 0.1.9 + 0.1.10 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index dbb9f4e2..9767a42c 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index fb3418eb..a9a4fdd9 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -1,7 +1,7 @@ rm_orientation_controller - 0.1.9 + 0.1.10 RoboMaster standard robot orientation controller Qiayuan Liao BSD diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index 8c9b0906..bd8424e0 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#118 `_ from ye-luo-xi-tui/master Publish shoot state * Publish shoot state. diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index 32d6bef7..33dc0a7f 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -1,7 +1,7 @@ rm_shooter_controllers - 0.1.9 + 0.1.10 RoboMaster standard robot Shooter controller Qiayuan Liao BSD diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index c905de0b..cff27340 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index 2d33e4a4..edfb6bf1 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -1,7 +1,7 @@ robot_state_controller - 0.1.9 + 0.1.10 A template for ROS packages. Qiayuan Liao BSD diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index 80d4f6dd..e924acda 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.10 (2023-03-25) +------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master 0.1.9 * Contributors: ye-luo-xi-tui diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index c1978973..25b5556b 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -1,7 +1,7 @@ tof_radar_controller - 0.1.9 + 0.1.10 The tof radar controller package luotinkai From 756302b5ba5bec2d746798aa34422fba6b573c43 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Wed, 29 Mar 2023 14:04:38 +0800 Subject: [PATCH 095/130] Change gpio calibration controller to the same one and modify gpio calibration logic. --- rm_calibration_controllers/CMakeLists.txt | 4 +- .../gpio_calibration_base.h | 25 ----- .../gpio_calibration_controller.h | 33 ++++++ .../hall_switch_calibration_controller.h | 30 ----- .../rm_calibration_controllers_plugins.xml | 4 +- .../src/calibration_base.cpp | 4 +- .../src/gpio_calibration_base.cpp | 36 ------ .../src/gpio_calibration_controller.cpp | 103 ++++++++++++++++++ .../hall_switch_calibration_controller.cpp | 96 ---------------- 9 files changed, 142 insertions(+), 193 deletions(-) delete mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h create mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h delete mode 100644 rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h delete mode 100644 rm_calibration_controllers/src/gpio_calibration_base.cpp create mode 100644 rm_calibration_controllers/src/gpio_calibration_controller.cpp delete mode 100644 rm_calibration_controllers/src/hall_switch_calibration_controller.cpp diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index b2b3ac7b..38c1d79d 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -53,9 +53,9 @@ include_directories( ## Declare a cpp library add_library(${PROJECT_NAME} src/mechanical_calibration_controller.cpp - src/hall_switch_calibration_controller.cpp + src/gpio_calibration_controller.cpp src/calibration_base.cpp - src/gpio_calibration_base.cpp) + ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h deleted file mode 100644 index 95fc5f00..00000000 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_base.h +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by guanlin on 23-3-16. -// - -#pragma once - -#include -#include -#include - -namespace rm_calibration_controllers -{ -class GpioCalibrationBase : public CalibrationBase -{ -public: - GpioCalibrationBase() = default; - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - -protected: - bool last_gpio_state_ = false; - std::vector gpio_state_handles_; - std::vector initial_gpio_states_; -}; -} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h new file mode 100644 index 00000000..359237b4 --- /dev/null +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -0,0 +1,33 @@ +// +// Created by guanlin on 22-11-7. +// + +#pragma once + +#include "rm_calibration_controllers/calibration_base.h" +#include + +namespace rm_calibration_controllers +{ +class GpioCalibrationController + : public CalibrationBase +{ +public: + GpioCalibrationController() = 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; + +private: + enum State + { + RETREAT = 3, + FORWARD + }; + double position_threshold_{}, backward_radius, start_retreat_pos_{}, forward_velocity_{}; + std::vector gpio_state_handles_; + std::vector initial_gpio_states_; + + effort_controllers::JointPositionController position_ctrl_; +}; +} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h deleted file mode 100644 index 003557cf..00000000 --- a/rm_calibration_controllers/include/rm_calibration_controllers/hall_switch_calibration_controller.h +++ /dev/null @@ -1,30 +0,0 @@ -// -// Created by guanlin on 22-11-7. -// - -#pragma once - -#include "rm_calibration_controllers/gpio_calibration_base.h" -#include - -namespace rm_calibration_controllers -{ -class HallSwitchCalibrationController : public GpioCalibrationBase -{ -public: - HallSwitchCalibrationController() = 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; - -private: - enum State - { - MOVING_AROUND = 3, - RETURN - }; - double position_threshold_{}, enter_pos_{}, exit_pos_{}; - bool is_returned_ = false; - - effort_controllers::JointPositionController position_ctrl_; -}; -} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml index 9679200c..d3f197b3 100755 --- a/rm_calibration_controllers/rm_calibration_controllers_plugins.xml +++ b/rm_calibration_controllers/rm_calibration_controllers_plugins.xml @@ -3,7 +3,7 @@ - diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp index 2577140c..957fbc98 100644 --- a/rm_calibration_controllers/src/calibration_base.cpp +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -23,12 +23,12 @@ bool CalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::Nod return false; } actuator_ = robot_hw->get()->getHandle(actuator[0]); - if (!controller_nh.getParam("search_velocity", velocity_search_)) + if (!vel_nh.getParam("search_velocity", velocity_search_)) { ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("vel_threshold", velocity_threshold_)) + if (!vel_nh.getParam("vel_threshold", velocity_threshold_)) { ROS_ERROR("Position value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; diff --git a/rm_calibration_controllers/src/gpio_calibration_base.cpp b/rm_calibration_controllers/src/gpio_calibration_base.cpp deleted file mode 100644 index 5948c4e9..00000000 --- a/rm_calibration_controllers/src/gpio_calibration_base.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by guanlin on 23-3-16. -// - -#include "rm_calibration_controllers/gpio_calibration_base.h" - -namespace rm_calibration_controllers -{ -bool GpioCalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) -{ - CalibrationBase::init(robot_hw, root_nh, controller_nh); - - XmlRpc::XmlRpcValue gpios, initial_gpio_states; - if (!controller_nh.getParam("gpios", gpios)) - { - ROS_ERROR("No gpios given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (!controller_nh.getParam("initial_gpio_states", initial_gpio_states)) - { - ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - for (int i = 0; i < gpios.size(); i++) - { - ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); - ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); - std::string gpio_name = gpios[i]; - rm_control::GpioStateHandle state_handle = robot_hw->get()->getHandle(gpio_name); - gpio_state_handles_.push_back(state_handle); - initial_gpio_states_.push_back(initial_gpio_states[i]); - } - return true; -} -} // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp new file mode 100644 index 00000000..b6460d07 --- /dev/null +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -0,0 +1,103 @@ +// +// Created by guanlin on 22-11-7. +// + +#include "rm_calibration_controllers/gpio_calibration_controller.h" +#include + +namespace rm_calibration_controllers +{ +bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + CalibrationBase::init(robot_hw, root_nh, controller_nh); + ros::NodeHandle pos_nh(controller_nh, "position"); + position_ctrl_.init(robot_hw->get(), pos_nh); + if (!pos_nh.getParam("pos_threshold", position_threshold_)) + { + ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!pos_nh.getParam("backward_radius", backward_radius)) + { + ROS_ERROR("Backward radius was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("velocity/forward_velocity", forward_velocity_)) + { + ROS_ERROR("Forward velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + XmlRpc::XmlRpcValue gpios, initial_gpio_states; + if (!controller_nh.getParam("gpios", gpios)) + { + ROS_ERROR("No gpios given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (!controller_nh.getParam("initial_gpio_states", initial_gpio_states)) + { + ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + for (int i = 0; i < gpios.size(); i++) + { + ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); + ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); + std::string gpio_name = gpios[i]; + rm_control::GpioStateHandle state_handle = robot_hw->get()->getHandle(gpio_name); + gpio_state_handles_.push_back(state_handle); + initial_gpio_states_.push_back(initial_gpio_states[i]); + } + return true; +} + +void GpioCalibrationController::update(const ros::Time& time, const ros::Duration& period) +{ + switch (state_) + { + case INITIALIZED: + { + velocity_ctrl_.setCommand(velocity_search_); + if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) + { + start_retreat_pos_ = velocity_ctrl_.joint_.getPosition(); + velocity_ctrl_.setCommand(0); + state_ = RETREAT; + } + else + velocity_ctrl_.update(time, period); + break; + } + case RETREAT: + { + position_ctrl_.setCommand(start_retreat_pos_ - backward_radius); + position_ctrl_.update(time, period); + if (std::abs(position_ctrl_.command_struct_.position_ - position_ctrl_.joint_.getPosition()) < position_threshold_) + { + state_ = FORWARD; + position_ctrl_.stopping(time); + } + break; + } + case FORWARD: + { + velocity_ctrl_.setCommand(forward_velocity_); + if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) + velocity_ctrl_.setCommand(0); + if (std::abs(velocity_ctrl_.joint_.getVelocity()) < velocity_threshold_) + { + actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); + actuator_.setCalibrated(true); + ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); + state_ = CALIBRATED; + } + velocity_ctrl_.update(time, period); + break; + } + case CALIBRATED: + calibration_success_ = true; + } +} +} // namespace rm_calibration_controllers + +PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::GpioCalibrationController, controller_interface::ControllerBase) diff --git a/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp b/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp deleted file mode 100644 index 60119e25..00000000 --- a/rm_calibration_controllers/src/hall_switch_calibration_controller.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// -// Created by guanlin on 22-11-7. -// - -#include "rm_calibration_controllers/hall_switch_calibration_controller.h" -#include - -namespace rm_calibration_controllers -{ -bool HallSwitchCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) -{ - GpioCalibrationBase::init(robot_hw, root_nh, controller_nh); - ros::NodeHandle pos_nh(controller_nh, "position"); - position_ctrl_.init(robot_hw->get(), pos_nh); - if (!controller_nh.getParam("pos_threshold", position_threshold_)) - { - ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - return true; -} - -void HallSwitchCalibrationController::update(const ros::Time& time, const ros::Duration& period) -{ - switch (state_) - { - case INITIALIZED: - { - velocity_ctrl_.setCommand(velocity_search_); - velocity_ctrl_.update(time, period); - if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) - { - last_gpio_state_ = gpio_state_handles_[0].getValue(); - velocity_ctrl_.update(time, period); - } - else - { - last_gpio_state_ = gpio_state_handles_[0].getValue(); - state_ = MOVING_AROUND; - } - break; - } - case MOVING_AROUND: - { - if (gpio_state_handles_[0].getValue() != last_gpio_state_) - { - if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0] && !is_returned_) - { - enter_pos_ = velocity_ctrl_.joint_.getPosition(); - last_gpio_state_ = gpio_state_handles_[0].getValue(); - } - if (gpio_state_handles_[0].getValue() == initial_gpio_states_[0] && enter_pos_ != 0) - { - exit_pos_ = velocity_ctrl_.joint_.getPosition(); - last_gpio_state_ = gpio_state_handles_[0].getValue(); - } - } - if (enter_pos_ != 0. && exit_pos_ != 0.) - { - velocity_ctrl_.setCommand(0.); - position_ctrl_.setCommand((enter_pos_ + exit_pos_) / 2); - enter_pos_ = 0; - exit_pos_ = 0; - state_ = RETURN; - } - velocity_ctrl_.update(time, period); - break; - } - case RETURN: - { - is_returned_ = true; - position_ctrl_.update(time, period); - if (((std::abs(position_ctrl_.joint_.getPosition() - position_ctrl_.command_struct_.position_)) < - position_threshold_) && - (position_ctrl_.joint_.getVelocity() < velocity_threshold_)) - { - actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); - actuator_.setCalibrated(true); - ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); - state_ = CALIBRATED; - } - break; - } - case CALIBRATED: - { - is_returned_ = false; - calibration_success_ = true; - position_ctrl_.setCommand(position_ctrl_.joint_.getPosition()); - position_ctrl_.update(time, period); - } - } -} -} // namespace rm_calibration_controllers - -PLUGINLIB_EXPORT_CLASS(rm_calibration_controllers::HallSwitchCalibrationController, controller_interface::ControllerBase) From 75aab34a37e302c2a72a136bdbd04e1af2987612 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Wed, 29 Mar 2023 20:28:41 +0800 Subject: [PATCH 096/130] Add velocity_dead_zone and effort_dead_zone. --- .../include/rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/gimbal_base.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 2e5719cc..a0bbab9d 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -171,7 +171,7 @@ class Controller : public controller_interface::MultiInterfaceController yaw_error_tolerance_) + if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_dead_zone_) resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; + else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_dead_zone_) + resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + resistance_compensation); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); From 227f8d44267feff3e6b42af214983a53b3686def Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Thu, 30 Mar 2023 13:41:54 +0800 Subject: [PATCH 097/130] Use realtime pub in balance state. --- .../include/rm_chassis_controllers/balance.h | 4 +- rm_chassis_controllers/src/balance.cpp | 38 ++++++++++--------- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 730c48d9..5af60c73 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -51,7 +52,8 @@ class BalanceController : public ChassisBase> RtpublisherPtr; + RtpublisherPtr state_pub_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index eb547847..91aec30d 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -313,7 +313,7 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan k_ = lqr.getK(); ROS_INFO_STREAM("K of LQR:" << k_); - state_pub_ = root_nh.advertise("/state", 10); + state_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/state", 100)); balance_state_ = BalanceState::NORMAL; return true; @@ -426,23 +426,25 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe position_des_ = position_offset_; } u = k_ * (-x); - rm_msgs::BalanceState state; - state.header.stamp = time; - state.x = x(0); - state.phi = x(1); - state.theta = x(2); - state.x_b_l = x(3); - state.x_b_r = x(4); - state.x_dot = x(5); - state.phi_dot = x(6); - state.theta_dot = x(7); - state.x_b_l_dot = x(8); - state.x_b_r_dot = x(9); - state.T_l = u(0); - state.T_r = u(1); - state.f_b_l = u(2); - state.f_b_r = u(3); - state_pub_.publish(state); + if (state_pub_->trylock()) + { + state_pub_->msg_.header.stamp = time; + state_pub_->msg_.x = x(0); + state_pub_->msg_.phi = x(1); + state_pub_->msg_.theta = x(2); + state_pub_->msg_.x_b_l = x(3); + state_pub_->msg_.x_b_r = x(4); + state_pub_->msg_.x_dot = x(5); + state_pub_->msg_.phi_dot = x(6); + state_pub_->msg_.theta_dot = x(7); + state_pub_->msg_.x_b_l_dot = x(8); + state_pub_->msg_.x_b_r_dot = x(9); + state_pub_->msg_.T_l = u(0); + state_pub_->msg_.T_r = u(1); + state_pub_->msg_.f_b_l = u(2); + state_pub_->msg_.f_b_r = u(3); + state_pub_->unlockAndPublish(); + } left_wheel_joint_handle_.setCommand(u(0)); right_wheel_joint_handle_.setCommand(u(1)); From cad6d43ec0efdc09493f8a5760e2b04fd4429f82 Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Thu, 30 Mar 2023 13:51:57 +0800 Subject: [PATCH 098/130] Separate balance model into functions. --- .../include/rm_chassis_controllers/balance.h | 5 + rm_chassis_controllers/src/balance.cpp | 172 +++++++++--------- 2 files changed, 96 insertions(+), 81 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 5af60c73..75db6071 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -30,6 +30,8 @@ class BalanceController : public ChassisBase> RtpublisherPtr; RtpublisherPtr state_pub_; + geometry_msgs::Vector3 angular_vel_base_; + double roll_, pitch_, yaw_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 91aec30d..a0c0aa27 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -321,13 +321,13 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Vector3 gyro, angular_vel_base; + geometry_msgs::Vector3 gyro; gyro.x = imu_handle_.getAngularVelocity()[0]; gyro.y = imu_handle_.getAngularVelocity()[1]; gyro.z = imu_handle_.getAngularVelocity()[2]; try { - tf2::doTransform(gyro, angular_vel_base, + tf2::doTransform(gyro, angular_vel_base_, robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -360,13 +360,12 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe odom2imu.setRotation(odom2imu_quaternion); odom2base = odom2imu * imu2base; - double roll, pitch, yaw; - quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); + quatToRPY(toMsg(odom2base).rotation, roll_, pitch_, yaw_); // Check block if (balance_state_ != BalanceState::BLOCK) { - if (std::abs(pitch) > block_angle_ && + if (std::abs(pitch_) > block_angle_ && (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > block_effort_ && (left_wheel_joint_handle_.getVelocity() < block_velocity_ || @@ -394,91 +393,102 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe { case BalanceState::NORMAL: { - if (balance_state_changed_) - { - ROS_INFO("[balance] Enter NOMAl"); - balance_state_changed_ = false; - } - x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - - imu_handle_.getAngularVelocity()[1]) * - wheel_radius_; - x_[0] += x_[5] * period.toSec(); - x_[1] = yaw; - x_[2] = pitch; - x_[3] = left_momentum_block_joint_handle_.getPosition(); - x_[4] = right_momentum_block_joint_handle_.getPosition(); - x_[6] = angular_vel_base.z; - x_[7] = angular_vel_base.y; - x_[8] = left_momentum_block_joint_handle_.getVelocity(); - x_[9] = right_momentum_block_joint_handle_.getVelocity(); - yaw_des_ += vel_cmd_.z * period.toSec(); - position_des_ += vel_cmd_.x * period.toSec(); - Eigen::Matrix u; - auto x = x_; - x(0) -= position_des_; - x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); - if (state_ != RAW) - x(5) -= vel_cmd_.x; - x(6) -= vel_cmd_.z; - if (std::abs(x(0) + position_offset_) > position_clear_threshold_) - { - x_[0] = 0.; - position_des_ = position_offset_; - } - u = k_ * (-x); - if (state_pub_->trylock()) - { - state_pub_->msg_.header.stamp = time; - state_pub_->msg_.x = x(0); - state_pub_->msg_.phi = x(1); - state_pub_->msg_.theta = x(2); - state_pub_->msg_.x_b_l = x(3); - state_pub_->msg_.x_b_r = x(4); - state_pub_->msg_.x_dot = x(5); - state_pub_->msg_.phi_dot = x(6); - state_pub_->msg_.theta_dot = x(7); - state_pub_->msg_.x_b_l_dot = x(8); - state_pub_->msg_.x_b_r_dot = x(9); - state_pub_->msg_.T_l = u(0); - state_pub_->msg_.T_r = u(1); - state_pub_->msg_.f_b_l = u(2); - state_pub_->msg_.f_b_r = u(3); - state_pub_->unlockAndPublish(); - } - - left_wheel_joint_handle_.setCommand(u(0)); - right_wheel_joint_handle_.setCommand(u(1)); - left_momentum_block_joint_handle_.setCommand(u(2)); - right_momentum_block_joint_handle_.setCommand(u(3)); + normal(time, period); break; } case BalanceState::BLOCK: { - if (balance_state_changed_) - { - ROS_INFO("[balance] Enter BLOCK"); - balance_state_changed_ = false; - - last_block_time_ = ros::Time::now(); - } - if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) - { - balance_state_ = BalanceState::NORMAL; - balance_state_changed_ = true; - ROS_INFO("[balance] Exit BLOCK"); - } - else - { - left_momentum_block_joint_handle_.setCommand(pitch > 0 ? -80 : 80); - right_momentum_block_joint_handle_.setCommand(pitch > 0 ? -80 : 80); - left_wheel_joint_handle_.setCommand(pitch > 0 ? -anti_block_effort_ : anti_block_effort_); - right_wheel_joint_handle_.setCommand(pitch > 0 ? -anti_block_effort_ : anti_block_effort_); - } + block(time, period); break; } } } +void BalanceController::normal(const ros::Time& time, const ros::Duration& period) +{ + if (balance_state_changed_) + { + ROS_INFO("[balance] Enter NOMAl"); + balance_state_changed_ = false; + } + + x_[5] = ((left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2 - + imu_handle_.getAngularVelocity()[1]) * + wheel_radius_; + x_[0] += x_[5] * period.toSec(); + x_[1] = yaw_; + x_[2] = pitch_; + x_[3] = left_momentum_block_joint_handle_.getPosition(); + x_[4] = right_momentum_block_joint_handle_.getPosition(); + x_[6] = angular_vel_base_.z; + x_[7] = angular_vel_base_.y; + x_[8] = left_momentum_block_joint_handle_.getVelocity(); + x_[9] = right_momentum_block_joint_handle_.getVelocity(); + yaw_des_ += vel_cmd_.z * period.toSec(); + position_des_ += vel_cmd_.x * period.toSec(); + Eigen::Matrix u; + auto x = x_; + x(0) -= position_des_; + x(1) = angles::shortest_angular_distance(yaw_des_, x_(1)); + if (state_ != RAW) + x(5) -= vel_cmd_.x; + x(6) -= vel_cmd_.z; + if (std::abs(x(0) + position_offset_) > position_clear_threshold_) + { + x_[0] = 0.; + position_des_ = position_offset_; + } + u = k_ * (-x); + if (state_pub_->trylock()) + { + state_pub_->msg_.header.stamp = time; + state_pub_->msg_.x = x(0); + state_pub_->msg_.phi = x(1); + state_pub_->msg_.theta = x(2); + state_pub_->msg_.x_b_l = x(3); + state_pub_->msg_.x_b_r = x(4); + state_pub_->msg_.x_dot = x(5); + state_pub_->msg_.phi_dot = x(6); + state_pub_->msg_.theta_dot = x(7); + state_pub_->msg_.x_b_l_dot = x(8); + state_pub_->msg_.x_b_r_dot = x(9); + state_pub_->msg_.T_l = u(0); + state_pub_->msg_.T_r = u(1); + state_pub_->msg_.f_b_l = u(2); + state_pub_->msg_.f_b_r = u(3); + state_pub_->unlockAndPublish(); + } + + left_wheel_joint_handle_.setCommand(u(0)); + right_wheel_joint_handle_.setCommand(u(1)); + left_momentum_block_joint_handle_.setCommand(u(2)); + right_momentum_block_joint_handle_.setCommand(u(3)); +} + +void BalanceController::block(const ros::Time& time, const ros::Duration& period) +{ + if (balance_state_changed_) + { + ROS_INFO("[balance] Enter BLOCK"); + balance_state_changed_ = false; + + last_block_time_ = ros::Time::now(); + } + if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) + { + balance_state_ = BalanceState::NORMAL; + balance_state_changed_ = true; + ROS_INFO("[balance] Exit BLOCK"); + } + else + { + left_momentum_block_joint_handle_.setCommand(pitch_ > 0 ? -80 : 80); + right_momentum_block_joint_handle_.setCommand(pitch_ > 0 ? -80 : 80); + left_wheel_joint_handle_.setCommand(pitch_ > 0 ? -anti_block_effort_ : anti_block_effort_); + right_wheel_joint_handle_.setCommand(pitch_ > 0 ? -anti_block_effort_ : anti_block_effort_); + } +} + geometry_msgs::Twist BalanceController::odometry() { geometry_msgs::Twist twist; From f4745710f1e203a925003ab1b557276caf019882 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 3 Apr 2023 18:44:12 +0800 Subject: [PATCH 099/130] Delete vector and some unnecessary code. --- .../calibration_base.h | 4 +- .../gpio_calibration_controller.h | 13 +++-- .../mechanical_calibration_controller.h | 5 +- .../src/calibration_base.cpp | 13 +---- .../src/gpio_calibration_controller.cpp | 49 ++++++++----------- .../src/mechanical_calibration_controller.cpp | 11 +++++ 6 files changed, 44 insertions(+), 51 deletions(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index b20b09af..5dd9887e 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace rm_calibration_controllers @@ -59,10 +60,11 @@ class CalibrationBase : public controller_interface::MultiInterfaceController gpio_state_handles_; - std::vector initial_gpio_states_; - - effort_controllers::JointPositionController position_ctrl_; + double position_threshold_{}, backward_angle_, start_retreat_position_{}, slow_forward_velocity_{}; + rm_control::GpioStateHandle gpio_state_handle_; + bool initial_gpio_state_; }; } // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h index 4876430f..18f0413c 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h @@ -79,10 +79,9 @@ class MechanicalCalibrationController MOVING_NEGATIVE, }; int state_{}, countdown_{}; - double target_position_{}, position_threshold_{}; - double positive_position_{}, negative_position_{}; + double velocity_threshold_{}, position_threshold_{}; + double positive_position_{}, negative_position_{}, target_position_{}; bool is_return_{}, is_center_{}; - effort_controllers::JointPositionController position_ctrl_; }; } // namespace rm_calibration_controllers diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp index 957fbc98..73d1e71c 100644 --- a/rm_calibration_controllers/src/calibration_base.cpp +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -25,20 +25,9 @@ bool CalibrationBase::init(hardware_interface::RobotHW* robot_hw, ros::Nod actuator_ = robot_hw->get()->getHandle(actuator[0]); if (!vel_nh.getParam("search_velocity", velocity_search_)) { - ROS_ERROR("Velocity value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Search velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!vel_nh.getParam("vel_threshold", velocity_threshold_)) - { - ROS_ERROR("Position value was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } - if (velocity_threshold_ < 0) - { - velocity_threshold_ *= -1.; - ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", - velocity_ctrl_.getJointName().c_str()); - } // advertise service to check calibration is_calibrated_srv_ = controller_nh.advertiseService("is_calibrated", &CalibrationBase::isCalibrated, this); return true; diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index b6460d07..9727dbfc 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -18,36 +18,28 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: ROS_ERROR("Position threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!pos_nh.getParam("backward_radius", backward_radius)) + if (!pos_nh.getParam("backward_angle", backward_angle_)) { - ROS_ERROR("Backward radius was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Backward angle was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("velocity/forward_velocity", forward_velocity_)) + if (!controller_nh.getParam("velocity/slow_forward_velocity", slow_forward_velocity_)) { - ROS_ERROR("Forward velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("Slow forward velocity was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - XmlRpc::XmlRpcValue gpios, initial_gpio_states; - if (!controller_nh.getParam("gpios", gpios)) + std::string gpio{}; + if (!controller_nh.getParam("gpio", gpio)) { ROS_ERROR("No gpios given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - if (!controller_nh.getParam("initial_gpio_states", initial_gpio_states)) + if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) { ROS_ERROR("No initial gpio states given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } - for (int i = 0; i < gpios.size(); i++) - { - ROS_ASSERT(initial_gpio_states[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean); - ROS_ASSERT(gpios[i].getType() == XmlRpc::XmlRpcValue::TypeString); - std::string gpio_name = gpios[i]; - rm_control::GpioStateHandle state_handle = robot_hw->get()->getHandle(gpio_name); - gpio_state_handles_.push_back(state_handle); - initial_gpio_states_.push_back(initial_gpio_states[i]); - } + gpio_state_handle_ = robot_hw->get()->getHandle(gpio); return true; } @@ -58,9 +50,14 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio case INITIALIZED: { velocity_ctrl_.setCommand(velocity_search_); - if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) + state_ = FAST_FORWARD; + break; + } + case FAST_FORWARD: + { + if (gpio_state_handle_.getValue() != initial_gpio_state_) { - start_retreat_pos_ = velocity_ctrl_.joint_.getPosition(); + start_retreat_position_ = velocity_ctrl_.joint_.getPosition(); velocity_ctrl_.setCommand(0); state_ = RETREAT; } @@ -70,22 +67,18 @@ void GpioCalibrationController::update(const ros::Time& time, const ros::Duratio } case RETREAT: { - position_ctrl_.setCommand(start_retreat_pos_ - backward_radius); + position_ctrl_.setCommand(start_retreat_position_ - backward_angle_); position_ctrl_.update(time, period); if (std::abs(position_ctrl_.command_struct_.position_ - position_ctrl_.joint_.getPosition()) < position_threshold_) - { - state_ = FORWARD; - position_ctrl_.stopping(time); - } + state_ = SLOW_FORWARD; break; } - case FORWARD: + case SLOW_FORWARD: { - velocity_ctrl_.setCommand(forward_velocity_); - if (gpio_state_handles_[0].getValue() != initial_gpio_states_[0]) - velocity_ctrl_.setCommand(0); - if (std::abs(velocity_ctrl_.joint_.getVelocity()) < velocity_threshold_) + velocity_ctrl_.setCommand(slow_forward_velocity_); + if (gpio_state_handle_.getValue() != initial_gpio_state_) { + velocity_ctrl_.setCommand(0); actuator_.setOffset(-actuator_.getPosition() + actuator_.getOffset()); actuator_.setCalibrated(true); ROS_INFO("Joint %s calibrated", velocity_ctrl_.getJointName().c_str()); diff --git a/rm_calibration_controllers/src/mechanical_calibration_controller.cpp b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp index b5cd4560..80898fa7 100644 --- a/rm_calibration_controllers/src/mechanical_calibration_controller.cpp +++ b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp @@ -47,6 +47,17 @@ bool MechanicalCalibrationController::init(hardware_interface::RobotHW* robot_hw CalibrationBase::init(robot_hw, root_nh, controller_nh); is_return_ = is_center_ = false; controller_nh.getParam("center", is_center_); + if (!controller_nh.getParam("velocity/vel_threshold", velocity_threshold_)) + { + ROS_ERROR("Velocity threshold was not specified (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + if (velocity_threshold_ < 0) + { + velocity_threshold_ *= -1.; + ROS_ERROR("Negative velocity threshold is not supported for joint %s. Making the velocity threshold positive.", + velocity_ctrl_.getJointName().c_str()); + } if (controller_nh.hasParam("return")) { ros::NodeHandle nh_return(controller_nh, "return"); From 5b58277976d797751506b32feb70e9348a853648 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 3 Apr 2023 18:55:59 +0800 Subject: [PATCH 100/130] Modified to get the error message when gpio is obtained. --- rm_calibration_controllers/src/gpio_calibration_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_calibration_controllers/src/gpio_calibration_controller.cpp b/rm_calibration_controllers/src/gpio_calibration_controller.cpp index 9727dbfc..b54a98b6 100644 --- a/rm_calibration_controllers/src/gpio_calibration_controller.cpp +++ b/rm_calibration_controllers/src/gpio_calibration_controller.cpp @@ -31,7 +31,7 @@ bool GpioCalibrationController::init(hardware_interface::RobotHW* robot_hw, ros: std::string gpio{}; if (!controller_nh.getParam("gpio", gpio)) { - ROS_ERROR("No gpios given (namespace: %s)", controller_nh.getNamespace().c_str()); + ROS_ERROR("No gpio given (namespace: %s)", controller_nh.getNamespace().c_str()); return false; } if (!controller_nh.getParam("initial_gpio_state", initial_gpio_state_)) From b1c85f0cbb0eea2da47b7b5ade0cc0d58bd79dc3 Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Mon, 3 Apr 2023 19:15:17 +0800 Subject: [PATCH 101/130] Rename BalanceMode. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 75db6071..694bed4c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -18,7 +18,7 @@ using Eigen::Matrix; class BalanceController : public ChassisBase { - enum BalanceState + enum BalanceMode { NORMAL, BLOCK diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index a0c0aa27..2ec7d1cf 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -314,7 +314,7 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ROS_INFO_STREAM("K of LQR:" << k_); state_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/state", 100)); - balance_state_ = BalanceState::NORMAL; + balance_state_ = BalanceMode::NORMAL; return true; } @@ -363,7 +363,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe quatToRPY(toMsg(odom2base).rotation, roll_, pitch_, yaw_); // Check block - if (balance_state_ != BalanceState::BLOCK) + if (balance_state_ != BalanceMode::BLOCK) { if (std::abs(pitch_) > block_angle_ && (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > @@ -378,7 +378,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe } if ((time - block_time_).toSec() >= block_duration_) { - balance_state_ = BalanceState::BLOCK; + balance_state_ = BalanceMode::BLOCK; balance_state_changed_ = true; ROS_INFO("[balance] Exit NOMAl"); } @@ -391,12 +391,12 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe switch (balance_state_) { - case BalanceState::NORMAL: + case BalanceMode::NORMAL: { normal(time, period); break; } - case BalanceState::BLOCK: + case BalanceMode::BLOCK: { block(time, period); break; @@ -476,7 +476,7 @@ void BalanceController::block(const ros::Time& time, const ros::Duration& period } if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) { - balance_state_ = BalanceState::NORMAL; + balance_state_ = BalanceMode::NORMAL; balance_state_changed_ = true; ROS_INFO("[balance] Exit BLOCK"); } From 4936f02fa3b652c708ce1ed625c1aa28d1b3a8ee Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Mon, 3 Apr 2023 20:44:37 +0800 Subject: [PATCH 102/130] Update balance model value. --- .../include/rm_chassis_controllers/balance.h | 2 +- rm_chassis_controllers/src/balance.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h index 694bed4c..048a3fd6 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/balance.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/balance.h @@ -46,7 +46,7 @@ class BalanceController : public ChassisBase(root_nh, "/state", 100)); - balance_state_ = BalanceMode::NORMAL; + balance_mode_ = BalanceMode::NORMAL; return true; } @@ -363,7 +363,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe quatToRPY(toMsg(odom2base).rotation, roll_, pitch_, yaw_); // Check block - if (balance_state_ != BalanceMode::BLOCK) + if (balance_mode_ != BalanceMode::BLOCK) { if (std::abs(pitch_) > block_angle_ && (std::abs(left_wheel_joint_handle_.getEffort()) + std::abs(right_wheel_joint_handle_.getEffort())) / 2. > @@ -378,7 +378,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe } if ((time - block_time_).toSec() >= block_duration_) { - balance_state_ = BalanceMode::BLOCK; + balance_mode_ = BalanceMode::BLOCK; balance_state_changed_ = true; ROS_INFO("[balance] Exit NOMAl"); } @@ -389,7 +389,7 @@ void BalanceController::moveJoint(const ros::Time& time, const ros::Duration& pe } } - switch (balance_state_) + switch (balance_mode_) { case BalanceMode::NORMAL: { @@ -476,7 +476,7 @@ void BalanceController::block(const ros::Time& time, const ros::Duration& period } if ((ros::Time::now() - last_block_time_).toSec() > block_overtime_) { - balance_state_ = BalanceMode::NORMAL; + balance_mode_ = BalanceMode::NORMAL; balance_state_changed_ = true; ROS_INFO("[balance] Exit BLOCK"); } From a3ef7044952269f4ca14e738368c783983712d13 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Thu, 6 Apr 2023 09:07:10 +0800 Subject: [PATCH 103/130] Modefy CMakeLists, delete TODO and initialize a variable. --- rm_calibration_controllers/CMakeLists.txt | 4 ++-- .../rm_calibration_controllers/gpio_calibration_controller.h | 2 +- .../src/mechanical_calibration_controller.cpp | 1 - 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rm_calibration_controllers/CMakeLists.txt b/rm_calibration_controllers/CMakeLists.txt index 38c1d79d..5e214a97 100755 --- a/rm_calibration_controllers/CMakeLists.txt +++ b/rm_calibration_controllers/CMakeLists.txt @@ -52,9 +52,9 @@ include_directories( ## Declare a cpp library add_library(${PROJECT_NAME} - src/mechanical_calibration_controller.cpp - src/gpio_calibration_controller.cpp src/calibration_base.cpp + src/gpio_calibration_controller.cpp + src/mechanical_calibration_controller.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h index b6616f39..40264c78 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/gpio_calibration_controller.h @@ -25,7 +25,7 @@ class GpioCalibrationController RETREAT, SLOW_FORWARD }; - double position_threshold_{}, backward_angle_, start_retreat_position_{}, slow_forward_velocity_{}; + double position_threshold_{}, backward_angle_{}, start_retreat_position_{}, slow_forward_velocity_{}; rm_control::GpioStateHandle gpio_state_handle_; bool initial_gpio_state_; }; diff --git a/rm_calibration_controllers/src/mechanical_calibration_controller.cpp b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp index 80898fa7..78bfe2f7 100644 --- a/rm_calibration_controllers/src/mechanical_calibration_controller.cpp +++ b/rm_calibration_controllers/src/mechanical_calibration_controller.cpp @@ -80,7 +80,6 @@ bool MechanicalCalibrationController::init(hardware_interface::RobotHW* robot_hw void MechanicalCalibrationController::update(const ros::Time& time, const ros::Duration& period) { - // TODO: Add GPIO switch support switch (state_) { case INITIALIZED: From be14fe61f8729bcc82397db2680a7fccf4d06094 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 8 Apr 2023 10:54:01 +0800 Subject: [PATCH 104/130] Remove unnecessary variables. --- .../mechanical_calibration_controller.h | 2 +- rm_calibration_controllers/src/calibration_base.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h index 18f0413c..54539a45 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/mechanical_calibration_controller.h @@ -78,7 +78,7 @@ class MechanicalCalibrationController MOVING_POSITIVE = 3, MOVING_NEGATIVE, }; - int state_{}, countdown_{}; + int countdown_{}; double velocity_threshold_{}, position_threshold_{}; double positive_position_{}, negative_position_{}, target_position_{}; bool is_return_{}, is_center_{}; diff --git a/rm_calibration_controllers/src/calibration_base.cpp b/rm_calibration_controllers/src/calibration_base.cpp index 73d1e71c..7da27012 100644 --- a/rm_calibration_controllers/src/calibration_base.cpp +++ b/rm_calibration_controllers/src/calibration_base.cpp @@ -38,6 +38,7 @@ void CalibrationBase::starting(const ros::Time& time) { actuator_.setCalibrated(false); state_ = INITIALIZED; + calibration_success_ = false; if (actuator_.getCalibrated()) ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); From fc9119bc6da37a9dce4a3ffc511040d83a075df6 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 11 Apr 2023 15:25:13 +0800 Subject: [PATCH 105/130] Rewrite the stopping function and set calibration success to false in the stopping function. --- .../include/rm_calibration_controllers/calibration_base.h | 1 + rm_calibration_controllers/src/calibration_base.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h index 5dd9887e..64b9f83e 100644 --- a/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h +++ b/rm_calibration_controllers/include/rm_calibration_controllers/calibration_base.h @@ -41,6 +41,7 @@ class CalibrationBase : public controller_interface::MultiInterfaceController::starting(const ros::Time& time) { actuator_.setCalibrated(false); state_ = INITIALIZED; - calibration_success_ = false; if (actuator_.getCalibrated()) ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", velocity_ctrl_.getJointName().c_str(), actuator_.getOffset()); } +template +void CalibrationBase::stopping(const ros::Time& time) +{ + calibration_success_ = false; +} + template bool CalibrationBase::isCalibrated(control_msgs::QueryCalibrationState::Request& req, control_msgs::QueryCalibrationState::Response& resp) From 811ace9f2db4e4905f2b2e160c340e956715deaa Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Mon, 8 May 2023 16:58:29 +0800 Subject: [PATCH 106/130] Shooter_controller would not check block when friction wheel don't rotate. --- rm_shooter_controllers/src/standard.cpp | 39 ++++++++++++------------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 5ee310f1..914f0333 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -176,30 +176,29 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; } - } - else - ROS_DEBUG("[Shooter] Wait for friction wheel"); - - // 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) + // 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)) { - state_ = BLOCK; - state_changed_ = true; - ROS_INFO("[Shooter] Exit PUSH"); + 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 - maybe_block_ = false; + ROS_DEBUG("[Shooter] Wait for friction wheel"); } void Controller::block(const ros::Time& time, const ros::Duration& period) From 07864137d8a3c0b3a4c4c26338e91b174f23c93e Mon Sep 17 00:00:00 2001 From: yuchen <116358@qq> Date: Wed, 31 May 2023 12:50:02 +0800 Subject: [PATCH 107/130] Change chassis command topic. --- rm_chassis_controllers/src/chassis_base.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 2f0b73b5..e6f0d197 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -107,7 +107,8 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan outside_odom_sub_ = controller_nh.subscribe("/odometry", 10, &ChassisBase::outsideOdomCallback, this); - cmd_chassis_sub_ = controller_nh.subscribe("command", 1, &ChassisBase::cmdChassisCallback, this); + cmd_chassis_sub_ = + controller_nh.subscribe("/cmd_chassis", 1, &ChassisBase::cmdChassisCallback, this); cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); if (controller_nh.hasParam("pid_follow")) From 33259009443e89a093fa2215f1cf6d9845c6c23e Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 18 Jun 2023 20:45:11 +0800 Subject: [PATCH 108/130] Add input feedforward and fix a bug in computing desire vel at TRACK mode. --- .../include/rm_gimbal_controllers/gimbal_base.h | 4 ++++ rm_gimbal_controllers/src/gimbal_base.cpp | 10 ++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index a0bbab9d..e09d968f 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -169,6 +169,10 @@ class Controller : public controller_interface::MultiInterfaceControllerget(); if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) @@ -375,14 +377,14 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); - yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2); + yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); - pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2); + pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); } catch (tf2::TransformException& ex) { @@ -400,8 +402,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_dead_zone_) resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + - resistance_compensation); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + yaw_k_v_ * yaw_vel_des + resistance_compensation); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des); } double Controller::feedForward(const ros::Time& time) From b8d90836b1fcf7a254550d2157c0cd69171bb87d Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 20 Jun 2023 16:35:29 +0800 Subject: [PATCH 109/130] 0.1.11 --- gpio_controller/CHANGELOG.rst | 8 +++++ mimic_joint_controller/CHANGELOG.rst | 8 +++++ rm_calibration_controllers/CHANGELOG.rst | 38 ++++++++++++++++++++++++ rm_chassis_controllers/CHANGELOG.rst | 23 ++++++++++++++ rm_controllers/CHANGELOG.rst | 8 +++++ rm_gimbal_controllers/CHANGELOG.rst | 14 +++++++++ rm_orientation_controller/CHANGELOG.rst | 8 +++++ rm_shooter_controllers/CHANGELOG.rst | 11 +++++++ robot_state_controller/CHANGELOG.rst | 8 +++++ tof_radar_controller/CHANGELOG.rst | 8 +++++ 10 files changed, 134 insertions(+) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 45acb927..5ea45321 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 574cf3cd..7d766586 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index accbd53c..b5ff48c5 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#127 `_ from 1moule/gpio_calibration_controller + Rewrite the stopping function and set calibration success to false in stopping function +* Rewrite the stopping function and set calibration success to false in the stopping function. +* Merge pull request `#126 `_ from 1moule/master + Remove unnecessary variables +* Remove unnecessary variables. +* Merge pull request `#125 `_ from rm-controls/calibration + Add gpio calibration controller +* Merge pull request `#116 `_ from 1moule/gpio_calibration_controller + Split the calibration controller and add a controller that uses gpio calibration +* Modefy CMakeLists, delete TODO and initialize a variable. +* Modified to get the error message when gpio is obtained. +* Delete vector and some unnecessary code. +* Merge branch 'master' into dev/balance +* Change gpio calibration controller to the same one and modify gpio calibration logic. +* Merge pull request `#120 `_ from ye-luo-xi-tui/master + 0.1.10 +* Modify the unmodified name in the joint calibration controller. +* Rename joint calibration controller. +* Write the velocity threshold in the base class. +* Modify the name of an enumeration type. +* Modify the name of an enumeration type. +* Modify variable name. +* Delete some comments, modify the initialization function of the gpio calibration controller base class. +* Modified hardware interface for instantiating template classes. +* Add a new line at the end and delete update function of calibration_base.h file. +* Factor out the calibration controller into a form derived from a base class and modify the controller appropriately. +* Use gpio handle to replace gpio call back function. +* Solved the problem of not being in the detection range of the hall switch when starting the calibration. +* Modify gpio calibration controller scheme to first use speed control to find a fixed point, and then use position control to reach. +* Modify logic and callback function of gpio calibration controller. +* Modify queue length of gpio subscriber. +* Merge branch 'rm-controls:master' into gpio_calibration_controller +* Add gpio calibration controller. +* Contributors: 1moule, ye-luo-xi-tui, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index 9e8e27ee..b725ed35 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#132 `_ from chenhuiYu00/change_chassis_topic + Change chassis command topic. +* Change chassis command topic. +* Merge branch 'rm-controls:master' into master +* Merge pull request `#123 `_ from chenhuiYu00/dev/balance + Add balance auto exit block +* Update balance model value. +* Rename BalanceMode. +* Separate balance model into functions. +* Use realtime pub in balance state. +* Merge branch 'master' into dev/balance +* Merge pull request `#120 `_ from ye-luo-xi-tui/master + 0.1.10 +* Update GYRO to RAW and enum rename. +* Merge branch 'master' into dev/balance +* Balance auto exit block add pitch limit. +* Update auto exit block. +* Add balance auto exit block. +* Merge branch 'rm-controls:master' into gpio_calibration_controller +* Contributors: 1moule, ye-luo-xi-tui, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#112 `_ from ljq-lv/Delete diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index e64bc710..19694fb1 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index 40e590fb..ce480503 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#135 `_ from ye-luo-xi-tui/feedforward + Add input feedforward and fix a bug in computing desire vel at TRACK mode +* Add input feedforward and fix a bug in computing desire vel at TRACK mode. +* Merge pull request `#124 `_ from ye-luo-xi-tui/resistance_compensation + Add velocity_dead_zone and effort_dead_zone +* Merge branch 'master' into dev/balance +* Add velocity_dead_zone and effort_dead_zone. +* 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 branch 'rm-controls:master' into master diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 9767a42c..56ff1db1 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index bd8424e0..a0f36500 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index cff27340..0f2ce05e 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index e924acda..fbb48232 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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, yuchen + 0.1.10 (2023-03-25) ------------------- * Merge pull request `#106 `_ from ye-luo-xi-tui/master From 8647b1e0a0fce4bff7a806f5ce38cf321cf37d45 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Tue, 20 Jun 2023 16:35:50 +0800 Subject: [PATCH 110/130] 0.1.11 --- gpio_controller/CHANGELOG.rst | 4 ++-- gpio_controller/package.xml | 2 +- mimic_joint_controller/CHANGELOG.rst | 4 ++-- mimic_joint_controller/package.xml | 2 +- rm_calibration_controllers/CHANGELOG.rst | 4 ++-- rm_calibration_controllers/package.xml | 2 +- rm_chassis_controllers/CHANGELOG.rst | 4 ++-- rm_chassis_controllers/package.xml | 2 +- rm_controllers/CHANGELOG.rst | 4 ++-- rm_controllers/package.xml | 2 +- rm_gimbal_controllers/CHANGELOG.rst | 4 ++-- rm_gimbal_controllers/package.xml | 2 +- rm_orientation_controller/CHANGELOG.rst | 4 ++-- rm_orientation_controller/package.xml | 2 +- rm_shooter_controllers/CHANGELOG.rst | 4 ++-- rm_shooter_controllers/package.xml | 2 +- robot_state_controller/CHANGELOG.rst | 4 ++-- robot_state_controller/package.xml | 2 +- tof_radar_controller/CHANGELOG.rst | 4 ++-- tof_radar_controller/package.xml | 2 +- 20 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gpio_controller/CHANGELOG.rst b/gpio_controller/CHANGELOG.rst index 5ea45321..129ba576 100644 --- a/gpio_controller/CHANGELOG.rst +++ b/gpio_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/gpio_controller/package.xml b/gpio_controller/package.xml index da7ace44..2516287e 100644 --- a/gpio_controller/package.xml +++ b/gpio_controller/package.xml @@ -1,7 +1,7 @@ gpio_controller - 0.1.10 + 0.1.11 The gpio_controller package muyuexin diff --git a/mimic_joint_controller/CHANGELOG.rst b/mimic_joint_controller/CHANGELOG.rst index 7d766586..69c6795c 100644 --- a/mimic_joint_controller/CHANGELOG.rst +++ b/mimic_joint_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mimic_joint_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/mimic_joint_controller/package.xml b/mimic_joint_controller/package.xml index 7857d1c6..aed23f44 100644 --- a/mimic_joint_controller/package.xml +++ b/mimic_joint_controller/package.xml @@ -1,7 +1,7 @@ mimic_joint_controller - 0.1.10 + 0.1.11 The mimic_joint_controller package ljq diff --git a/rm_calibration_controllers/CHANGELOG.rst b/rm_calibration_controllers/CHANGELOG.rst index b5ff48c5..7a1595ab 100644 --- a/rm_calibration_controllers/CHANGELOG.rst +++ b/rm_calibration_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_calibration_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge pull request `#127 `_ from 1moule/gpio_calibration_controller Rewrite the stopping function and set calibration success to false in stopping function * Rewrite the stopping function and set calibration success to false in the stopping function. diff --git a/rm_calibration_controllers/package.xml b/rm_calibration_controllers/package.xml index ef2e985f..01601a61 100755 --- a/rm_calibration_controllers/package.xml +++ b/rm_calibration_controllers/package.xml @@ -1,7 +1,7 @@ rm_calibration_controllers - 0.1.10 + 0.1.11 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_chassis_controllers/CHANGELOG.rst b/rm_chassis_controllers/CHANGELOG.rst index b725ed35..442fc8cc 100644 --- a/rm_chassis_controllers/CHANGELOG.rst +++ b/rm_chassis_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_chassis_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge pull request `#132 `_ from chenhuiYu00/change_chassis_topic Change chassis command topic. * Change chassis command topic. diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 59876d7a..ac4b6d69 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -1,7 +1,7 @@ rm_chassis_controllers - 0.1.10 + 0.1.11 RoboMaster standard robot Chassis controller Qiayuan Liao BSD diff --git a/rm_controllers/CHANGELOG.rst b/rm_controllers/CHANGELOG.rst index 19694fb1..106a8a35 100644 --- a/rm_controllers/CHANGELOG.rst +++ b/rm_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/rm_controllers/package.xml b/rm_controllers/package.xml index ecba2ff1..9548b658 100644 --- a/rm_controllers/package.xml +++ b/rm_controllers/package.xml @@ -1,7 +1,7 @@ rm_controllers - 0.1.10 + 0.1.11 Meta package that contains package for RoboMaster. Qiayuan Liao diff --git a/rm_gimbal_controllers/CHANGELOG.rst b/rm_gimbal_controllers/CHANGELOG.rst index ce480503..267ce59b 100644 --- a/rm_gimbal_controllers/CHANGELOG.rst +++ b/rm_gimbal_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_gimbal_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge pull request `#135 `_ from ye-luo-xi-tui/feedforward Add input feedforward and fix a bug in computing desire vel at TRACK mode * Add input feedforward and fix a bug in computing desire vel at TRACK mode. diff --git a/rm_gimbal_controllers/package.xml b/rm_gimbal_controllers/package.xml index b9ac9e1e..3b1fec7b 100644 --- a/rm_gimbal_controllers/package.xml +++ b/rm_gimbal_controllers/package.xml @@ -1,7 +1,7 @@ rm_gimbal_controllers - 0.1.10 + 0.1.11 RoboMaster standard robot Gimbal controller Qiayuan Liao BSD diff --git a/rm_orientation_controller/CHANGELOG.rst b/rm_orientation_controller/CHANGELOG.rst index 56ff1db1..1c530cd5 100644 --- a/rm_orientation_controller/CHANGELOG.rst +++ b/rm_orientation_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_orientation_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/rm_orientation_controller/package.xml b/rm_orientation_controller/package.xml index a9a4fdd9..472e88b2 100644 --- a/rm_orientation_controller/package.xml +++ b/rm_orientation_controller/package.xml @@ -1,7 +1,7 @@ rm_orientation_controller - 0.1.10 + 0.1.11 RoboMaster standard robot orientation controller Qiayuan Liao BSD diff --git a/rm_shooter_controllers/CHANGELOG.rst b/rm_shooter_controllers/CHANGELOG.rst index a0f36500..ec958138 100644 --- a/rm_shooter_controllers/CHANGELOG.rst +++ b/rm_shooter_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rm_shooter_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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. diff --git a/rm_shooter_controllers/package.xml b/rm_shooter_controllers/package.xml index 33dc0a7f..35b85f15 100644 --- a/rm_shooter_controllers/package.xml +++ b/rm_shooter_controllers/package.xml @@ -1,7 +1,7 @@ rm_shooter_controllers - 0.1.10 + 0.1.11 RoboMaster standard robot Shooter controller Qiayuan Liao BSD diff --git a/robot_state_controller/CHANGELOG.rst b/robot_state_controller/CHANGELOG.rst index 0f2ce05e..1ab8a071 100644 --- a/robot_state_controller/CHANGELOG.rst +++ b/robot_state_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package robot_state_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/robot_state_controller/package.xml b/robot_state_controller/package.xml index edfb6bf1..a526b6ba 100644 --- a/robot_state_controller/package.xml +++ b/robot_state_controller/package.xml @@ -1,7 +1,7 @@ robot_state_controller - 0.1.10 + 0.1.11 A template for ROS packages. Qiayuan Liao BSD diff --git a/tof_radar_controller/CHANGELOG.rst b/tof_radar_controller/CHANGELOG.rst index fbb48232..ce9df95e 100644 --- a/tof_radar_controller/CHANGELOG.rst +++ b/tof_radar_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tof_radar_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.11 (2023-06-20) +------------------- * Merge branch 'master' into dev/balance * Merge pull request `#120 `_ from ye-luo-xi-tui/master 0.1.10 diff --git a/tof_radar_controller/package.xml b/tof_radar_controller/package.xml index 25b5556b..e70d684a 100644 --- a/tof_radar_controller/package.xml +++ b/tof_radar_controller/package.xml @@ -1,7 +1,7 @@ tof_radar_controller - 0.1.10 + 0.1.11 The tof radar controller package luotinkai From 9c0b1c7e5dfd5c45c563e92eabbd6dd9f5ae745d Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 24 Jun 2023 17:15:16 +0800 Subject: [PATCH 111/130] Modify the shooter controller to realize the function of modifying the friction wheel speed by pressing the button. --- .../include/rm_shooter_controllers/standard.h | 4 ++++ rm_shooter_controllers/src/standard.cpp | 12 ++++++++++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index d37f4a39..c1cc5863 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -81,12 +82,14 @@ class Controller : public controller_interface::MultiInterfaceController> shoot_state_pub_; ros::Subscriber cmd_subscriber_; + ros::ServiceServer shoot_speed_srv_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 914f0333..87b33bd1 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -62,6 +62,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_qd_threshold_ = getParam(controller_nh, "push_qd_threshold", 0.); + shoot_speed_srv_ = controller_nh.advertiseService("/shooter_speed", &Controller::changeStatusCB, this); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure @@ -237,8 +238,8 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) qd_des = config_.qd_30; else qd_des = 0.; - ctrl_friction_l_.setCommand(qd_des + config_.lf_extra_rotat_speed); - ctrl_friction_r_.setCommand(-qd_des); + ctrl_friction_l_.setCommand(qd_des + config_.lf_extra_rotat_speed + extra_speed_); + ctrl_friction_r_.setCommand(-qd_des - extra_speed_); } void Controller::normalize() @@ -247,6 +248,13 @@ void Controller::normalize() ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); } +bool Controller::changeStatusCB(rm_msgs::ShooterSpeed::Request& req, rm_msgs::ShooterSpeed::Response& res) +{ + extra_speed_ = req.shooter_speed; + res.is_success = true; + return true; +} + void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); From f1773b9699e73e9801b4910489a3639c711fb98c Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Thu, 29 Jun 2023 14:18:07 +0800 Subject: [PATCH 112/130] Modify callback function name and some param names. --- .../include/rm_shooter_controllers/standard.h | 8 ++++---- rm_shooter_controllers/src/standard.cpp | 11 ++++++----- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index c1cc5863..f985260b 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include @@ -82,14 +82,14 @@ class Controller : public controller_interface::MultiInterfaceController> shoot_state_pub_; ros::Subscriber cmd_subscriber_; - ros::ServiceServer shoot_speed_srv_; + ros::ServiceServer extra_friction_wheel_speed_srv_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 87b33bd1..ea12019b 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -62,7 +62,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_qd_threshold_ = getParam(controller_nh, "push_qd_threshold", 0.); - shoot_speed_srv_ = controller_nh.advertiseService("/shooter_speed", &Controller::changeStatusCB, this); + extra_friction_wheel_speed_srv_ = controller_nh.advertiseService("/extra_friction_wheel_speed", &Controller::extraFrictionWheelSpeedCB, this); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure @@ -238,8 +238,8 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) qd_des = config_.qd_30; else qd_des = 0.; - ctrl_friction_l_.setCommand(qd_des + config_.lf_extra_rotat_speed + extra_speed_); - ctrl_friction_r_.setCommand(-qd_des - extra_speed_); + ctrl_friction_l_.setCommand(qd_des + config_.lf_extra_rotat_speed + extra_friction_wheel_speed_); + ctrl_friction_r_.setCommand(-qd_des - extra_friction_wheel_speed_); } void Controller::normalize() @@ -248,9 +248,10 @@ void Controller::normalize() ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); } -bool Controller::changeStatusCB(rm_msgs::ShooterSpeed::Request& req, rm_msgs::ShooterSpeed::Response& res) +bool Controller::extraFrictionWheelSpeedCB(rm_msgs::ExtraFrictionWheelSpeed::Request& req, + rm_msgs::ExtraFrictionWheelSpeed::Response& res) { - extra_speed_ = req.shooter_speed; + extra_friction_wheel_speed_ = req.extra_speed; res.is_success = true; return true; } From 7eaa248d5ac77e32d6d93f7867d32bc275a97183 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 29 Jun 2023 21:37:57 +0800 Subject: [PATCH 113/130] Adapt new antitop model. --- .../rm_gimbal_controllers/bullet_solver.h | 8 +-- rm_gimbal_controllers/src/bullet_solver.cpp | 54 ++++++++++++++----- rm_gimbal_controllers/src/gimbal_base.cpp | 18 ++++--- 3 files changed, 57 insertions(+), 23 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index c3a6dcc1..f75e466b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -60,9 +60,10 @@ class BulletSolver public: explicit BulletSolver(ros::NodeHandle& controller_nh); - bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed); - double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw_real, double pitch_real, - double bullet_speed); + bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, + double r1, double r2, double z2); + double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, + double r2, double z2, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double bullet_speed) const; double getYaw() const { @@ -85,6 +86,7 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; + int selected_armor_; geometry_msgs::Point target_pos_{}; visualization_msgs::Marker marker_desire_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 1f898017..ec54d3dd 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -98,17 +98,39 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const return resistance_coff; } -bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed) +bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, + double v_yaw, double r1, double r2, double z2) { config_ = *config_rt_buffer_.readFromRT(); - target_pos_ = pos; bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; - int count{}; double temp_z = pos.z; - double target_rho; + double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); + output_yaw_ = std::atan2(pos.y, pos.x); + output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; + selected_armor_ = 0; + double r = r1; + double z = pos.z; + if ((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) + { + selected_armor_ = -1; + r = r2; + z = z2; + } + else if ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4) + { + selected_armor_ = 1; + r = r2; + z = z2; + } + int count{}; double error = 999; + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * M_PI_2); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * M_PI_2); + target_pos_.z = z; while (error >= 0.001) { output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); @@ -120,9 +142,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - target_pos_.x = pos.x + vel.x * (config_.delay + fly_time); - target_pos_.y = pos.y + vel.y * (config_.delay + fly_time); - target_pos_.z = pos.z + vel.z * (config_.delay + fly_time); + target_pos_.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos_.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos_.z = z + vel.z * fly_time; double target_yaw = std::atan2(target_pos_.y, target_pos_.x); double error_theta = target_yaw - output_yaw_; @@ -186,11 +208,19 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } } -double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw_real, - double pitch_real, double bullet_speed) +double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, + double r1, double r2, double z2, double yaw_real, double pitch_real, + double bullet_speed) { config_ = *config_rt_buffer_.readFromRT(); double resistance_coff = getResistanceCoefficient(bullet_speed); + double r = r1; + double z = pos.z; + if (selected_armor_ != 0) + { + r = r2; + z = z2; + } double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; @@ -200,9 +230,9 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec while (std::abs(fly_time - last_fly_time) > 0.01) { last_fly_time = fly_time; - target_pos.x = pos.x + vel.x * (config_.delay + fly_time); - target_pos.y = pos.y + vel.y * (config_.delay + fly_time); - target_pos.z = pos.z + vel.z * (config_.delay + fly_time); + target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos.z = z + vel.z * fly_time; target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2)); fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; count++; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index d1f35c58..19658868 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -232,8 +232,8 @@ void Controller::track(const ros::Time& time) quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); double yaw_compute = yaw_real; double pitch_compute = -pitch_real; - geometry_msgs::Point target_pos = data_track_.target_pos; - geometry_msgs::Vector3 target_vel = data_track_.target_vel; + geometry_msgs::Point target_pos = data_track_.position; + geometry_msgs::Vector3 target_vel = data_track_.velocity; try { if (!data_track_.header.frame_id.empty()) @@ -251,18 +251,20 @@ void Controller::track(const ros::Time& time) target_pos.x -= odom2pitch_.transform.translation.x; target_pos.y -= odom2pitch_.transform.translation.y; target_pos.z -= odom2pitch_.transform.translation.z; + double z2 = data_track_.z_2 - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); - - bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed); + bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, z2); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { if (error_pub_->trylock()) { - double error = - bullet_solver_->getGimbalError(target_pos, target_vel, yaw_compute, pitch_compute, cmd_gimbal_.bullet_speed); + double error = bullet_solver_->getGimbalError(target_pos, target_vel, data_track_.yaw, data_track_.v_yaw, + data_track_.radius_1, data_track_.radius_2, z2, yaw_compute, + pitch_compute, cmd_gimbal_.bullet_speed); error_pub_->msg_.stamp = time; error_pub_->msg_.error = solve_success ? error : 1.0; error_pub_->unlockAndPublish(); @@ -364,8 +366,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos = data_track_.target_pos; - geometry_msgs::Vector3 target_vel = data_track_.target_vel; + geometry_msgs::Point target_pos = data_track_.position; + geometry_msgs::Vector3 target_vel = data_track_.velocity; tf2::Vector3 target_pos_tf, target_vel_tf; try From 99a2059e67633af23424e2c682bba7e9dea8a797 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 2 Jul 2023 06:22:26 +0800 Subject: [PATCH 114/130] Adapt balance and outpost. --- .../rm_gimbal_controllers/bullet_solver.h | 4 +-- rm_gimbal_controllers/src/bullet_solver.cpp | 36 +++++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 13 +++---- 3 files changed, 25 insertions(+), 28 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index f75e466b..6d17d115 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -61,9 +61,9 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double z2); + double r1, double r2, double dz, int armors_num); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double z2, double yaw_real, double pitch_real, double bullet_speed); + double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double bullet_speed) const; double getYaw() const { diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index ec54d3dd..318914cc 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -99,7 +99,7 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double z2) + double v_yaw, double r1, double r2, double dz, int armors_num) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -114,22 +114,16 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - if ((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) + if (((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) || ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4)) { - selected_armor_ = -1; - r = r2; - z = z2; - } - else if ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4) - { - selected_armor_ = 1; - r = r2; - z = z2; + selected_armor_ = (yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4 ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = armors_num == 4 ? pos.z + dz : pos.z; } int count{}; double error = 999; - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * M_PI_2); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * M_PI_2); + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); target_pos_.z = z; while (error >= 0.001) { @@ -142,8 +136,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - target_pos_.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); - target_pos_.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos_.x = + pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = + pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); target_pos_.z = z + vel.z * fly_time; double target_yaw = std::atan2(target_pos_.y, target_pos_.x); @@ -209,7 +205,7 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double z2, double yaw_real, double pitch_real, + double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed) { config_ = *config_rt_buffer_.readFromRT(); @@ -218,8 +214,8 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double z = pos.z; if (selected_armor_ != 0) { - r = r2; - z = z2; + r = armors_num == 4 ? r2 : r1; + z = armors_num == 4 ? pos.z + dz : pos.z; } double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / @@ -230,8 +226,8 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec while (std::abs(fly_time - last_fly_time) > 0.01) { last_fly_time = fly_time; - target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); - target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * M_PI_2); + target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); target_pos.z = z + vel.z * fly_time; target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2)); fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 19658868..835573fe 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -251,20 +251,21 @@ void Controller::track(const ros::Time& time) target_pos.x -= odom2pitch_.transform.translation.x; target_pos.y -= odom2pitch_.transform.translation.y; target_pos.z -= odom2pitch_.transform.translation.z; - double z2 = data_track_.z_2 - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); - bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, z2); + bool solve_success = + bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw, + data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { if (error_pub_->trylock()) { - double error = bullet_solver_->getGimbalError(target_pos, target_vel, data_track_.yaw, data_track_.v_yaw, - data_track_.radius_1, data_track_.radius_2, z2, yaw_compute, - pitch_compute, cmd_gimbal_.bullet_speed); + double error = + bullet_solver_->getGimbalError(target_pos, target_vel, data_track_.yaw, data_track_.v_yaw, + data_track_.radius_1, data_track_.radius_2, data_track_.dz, + data_track_.armors_num, yaw_compute, pitch_compute, cmd_gimbal_.bullet_speed); error_pub_->msg_.stamp = time; error_pub_->msg_.error = solve_success ? error : 1.0; error_pub_->unlockAndPublish(); From 31041d068e3f5fa1550e0e7bff75a699dcc74863 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 2 Jul 2023 22:55:35 +0800 Subject: [PATCH 115/130] Revise selected armor's position and velocity. --- .../rm_gimbal_controllers/bullet_solver.h | 3 +++ rm_gimbal_controllers/src/bullet_solver.cpp | 18 ++++++++++++++++++ rm_gimbal_controllers/src/gimbal_base.cpp | 7 +++++-- 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 6d17d115..218436d6 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -73,6 +73,9 @@ class BulletSolver { return -output_pitch_; } + void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, + geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, + double r1, double r2, double dz, int armors_num); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 318914cc..c915e1a5 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -155,6 +155,24 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d return true; } +void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, + geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, + double v_yaw, double r1, double r2, double dz, int armors_num) +{ + double r = r1, z = pos.z; + if (armors_num == 4 && selected_armor_ != 0) + { + r = r2; + z = pos.z + dz; + } + armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.z = z; + armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.z = vel.z; +} + void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 835573fe..9f03a1a5 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -367,8 +367,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos = data_track_.position; - geometry_msgs::Vector3 target_vel = data_track_.velocity; + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, + data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, + data_track_.radius_2, data_track_.dz, data_track_.armors_num); tf2::Vector3 target_pos_tf, target_vel_tf; try From e71a5e03a8b4492d2e834973fba72b56cecacf46 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 8 Jul 2023 20:08:32 +0800 Subject: [PATCH 116/130] modify_readme --- rm_calibration_controllers/README.md | 54 +++++++++++++++++++++++----- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/rm_calibration_controllers/README.md b/rm_calibration_controllers/README.md index 12b8a16c..d1509066 100644 --- a/rm_calibration_controllers/README.md +++ b/rm_calibration_controllers/README.md @@ -2,7 +2,7 @@ ## Overview -Since the zero point of some motors will change after power off, rm_calibration_controller will move at a certain speed after it is started until motors reach the mechanical limit, and motors position will be reset to zero. +Since the zero point of some motors will change after power off, rm_calibration_controller will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stop moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed before it detects a gpio which differs from the origin one.When the state of gpio changes,it will rotate at a small angle,then restore origin gpio,finally move at a small speed until the state of gpio changes again. **Keywords:** calibration, ROS, position. @@ -20,6 +20,7 @@ The package has been tested under [ROS](https://www.ros.org/) Indigo, Melodic an + `EffortJointInterface` Used to send effort command to target joint to make it reach the calibration speed. + `ActuatorExtraInterface` Used to obtain the information of the target actuators offset, current position, the state of the whether it is stopped and the state of whether it is calibrated. ++ `GpioStateInterface` Used to obtain the state of gpio. ## Installation @@ -39,36 +40,71 @@ Or better, use `rosdep`: * roscpp * rm_common * effort_controllers +* controller_interface ## ROS API -#### Service +### Service * **`is_calibrated`** ([control_msgs/QueryCalibrationState](http://docs.ros.org/en/api/control_msgs/html/srv/QueryCalibrationState.html)) When requesting to this server, it will return response about whether target joint has been calibrated. -#### Parameters +### Parameters + +#### calibration_base * **`search_velocity`** (double) The joint velocity of calibrating. -* **`threshold`** (double) +#### mechanical_calibration_controller + +* **`velocity_threshold_`** (double) This is velocity `threshold`. When the real time velocity of target joint lower than threshold, and last for a while, it can switch CALIBRATED from MOVING. -#### Complete description +#### gpio_calibration_controller + +* **`gpio_state_handle_`** (double) + + The state of gpio. + +### Complete description + +#### mechanical_calibration_controller ```yaml -cover_controller: - type: effort_controllers/JointPositionController - joint: "cover_joint" - pid: { p: 15.0, i: 0.0, d: 1.2, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +trigger_calibration_controller: + type: rm_calibration_controllers/MechanicalCalibrationController + actuator: [ trigger_joint_motor ] + velocity: + search_velocity: 4.0 + vel_threshold: 0.001 + joint: trigger_joint + pid: { p: 0.8, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } ``` +#### gpio_calibration_controller +```yaml +yaw_calibration_controller: + type: rm_calibration_controllers/GpioCalibrationController + actuator: [ yaw_joint_motor ] + gpio: "yaw" + initial_gpio_state: false + velocity: + search_velocity: -4.0 + slow_forward_velocity: -2.0 + joint: yaw_joint + pid: { p: 0.19, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } + position: + pos_threshold: 0.01 + backward_angle: -0.15 + joint: yaw_joint + pid: { p: 7.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true } +``` ## Bugs & Feature Requests From c54c1c3a8ecd78921b43da2654cc0ee9c7861f48 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 8 Jul 2023 21:42:21 +0800 Subject: [PATCH 117/130] modify_readme --- rm_calibration_controllers/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_calibration_controllers/README.md b/rm_calibration_controllers/README.md index d1509066..9255d4f3 100644 --- a/rm_calibration_controllers/README.md +++ b/rm_calibration_controllers/README.md @@ -2,7 +2,7 @@ ## Overview -Since the zero point of some motors will change after power off, rm_calibration_controller will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stop moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed before it detects a gpio which differs from the origin one.When the state of gpio changes,it will rotate at a small angle,then restore origin gpio,finally move at a small speed until the state of gpio changes again. +Since the zero point of some motors will change after power off, rm_calibration_controllers will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stops moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed until gpio changes to be different from initial gpio.Then it will retreat at a small angle and restore initial gpio.Finally it moves at a small speed until the state of gpio changes again. **Keywords:** calibration, ROS, position. From 2d1f7a257b928fb3a6d71c686e7a14469e8ac93d Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 8 Jul 2023 22:34:51 +0800 Subject: [PATCH 118/130] modify_readme.md --- rm_calibration_controllers/README.md | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/rm_calibration_controllers/README.md b/rm_calibration_controllers/README.md index 9255d4f3..b8af4dd1 100644 --- a/rm_calibration_controllers/README.md +++ b/rm_calibration_controllers/README.md @@ -2,7 +2,7 @@ ## Overview -Since the zero point of some motors will change after power off, rm_calibration_controllers will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stops moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed until gpio changes to be different from initial gpio.Then it will retreat at a small angle and restore initial gpio.Finally it moves at a small speed until the state of gpio changes again. +Since the zero point of some motors will change after power off, rm_calibration_controllers will move after it is started until motors reach the position we wanted,and motors position will be reset to zero,which can come true by two ways.One is that mechanical_calibration_controller stops moving after it reaches the mechanical limit.Another one is that gpio_calibration_controller moves at a fast speed until gpio changes to be different from initial gpio.Then it will retreat at a small angle and restore initial gpio.Finally it moves at a slow speed until the state of gpio changes to be different from initial gpio again. **Keywords:** calibration, ROS, position. @@ -68,9 +68,17 @@ Or better, use `rosdep`: #### gpio_calibration_controller -* **`gpio_state_handle_`** (double) +* **`backward_angle`** (double) - The state of gpio. + The angle of retreat when gpio changes to be different form initial gpio for the first time. + +* **`slow_forward_velocity`** (double) + + The velocity of second forward movement for reaching a more accurate calibration-position. + +* **`pos_threshold`** (double) + + The threshold for the difference between the command position and the current position. ### Complete description From 40dedf1440f81cc27f768ba8e7ecca94df9db970 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 11 Jul 2023 11:10:36 +0800 Subject: [PATCH 119/130] Modify service name. --- rm_shooter_controllers/src/standard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index ea12019b..125d9128 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -62,7 +62,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_qd_threshold_ = getParam(controller_nh, "push_qd_threshold", 0.); - extra_friction_wheel_speed_srv_ = controller_nh.advertiseService("/extra_friction_wheel_speed", &Controller::extraFrictionWheelSpeedCB, this); + extra_friction_wheel_speed_srv_ = controller_nh.advertiseService("extra_friction_wheel_speed", &Controller::extraFrictionWheelSpeedCB, this); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure From e1365d19dcd90e362e8b31a5b5e9b1cf0d26b8f3 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 11 Jul 2023 11:24:31 +0800 Subject: [PATCH 120/130] Modify for format. --- .../include/rm_shooter_controllers/standard.h | 3 ++- rm_shooter_controllers/src/standard.cpp | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index f985260b..5832f00d 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -82,7 +82,8 @@ class Controller : public controller_interface::MultiInterfaceController("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure @@ -249,7 +250,7 @@ void Controller::normalize() } bool Controller::extraFrictionWheelSpeedCB(rm_msgs::ExtraFrictionWheelSpeed::Request& req, - rm_msgs::ExtraFrictionWheelSpeed::Response& res) + rm_msgs::ExtraFrictionWheelSpeed::Response& res) { extra_friction_wheel_speed_ = req.extra_speed; res.is_success = true; From da46c74eeccc225b0b3878b78b0a5a711ba2957c Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Thu, 20 Jul 2023 21:52:17 +0800 Subject: [PATCH 121/130] Add function that tracking center of the enemy when enemy's angular velocity is too large. --- .../rm_gimbal_controllers/bullet_solver.h | 2 + rm_gimbal_controllers/src/bullet_solver.cpp | 74 ++++++++++++++----- 2 files changed, 59 insertions(+), 17 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 218436d6..3b1572fd 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -86,10 +86,12 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; + double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; int selected_armor_; + bool track_target_; geometry_msgs::Point target_pos_{}; visualization_msgs::Marker marker_desire_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c915e1a5..cbb5c8ae 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -53,6 +53,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.) }; + max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); marker_desire_.header.frame_id = "odom"; @@ -114,16 +115,30 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - if (((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) || ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4)) + track_target_ = std::abs(v_yaw) < max_track_target_vel_; + double switch_armor_angle = + track_target_ ? + acos(r / target_rho) - M_PI / 6 + (-acos(r / target_rho) + M_PI_4) * std::abs(v_yaw) / max_track_target_vel_ : + M_PI / 12; + if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { - selected_armor_ = (yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4 ? -1 : 1; + selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; } int count{}; double error = 999; - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + if (track_target_) + { + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + } + else + { + target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); + target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); + } target_pos_.z = z; while (error >= 0.001) { @@ -136,10 +151,23 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - target_pos_.x = - pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + if (track_target_) + { + target_pos_.x = + pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = + pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos.x + vel.x * fly_time; + target_pos_after_fly_time[1] = pos.y + vel.y * fly_time; + target_pos_.x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_.y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } target_pos_.z = z + vel.z * fly_time; double target_yaw = std::atan2(target_pos_.y, target_pos_.x); @@ -165,12 +193,21 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge r = r2; z = pos.z + dz; } - armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_pos.z = z; - armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_vel.z = vel.z; + if (track_target_) + { + armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.z = z; + armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.z = vel.z; + } + else + { + armor_pos = pos; + armor_pos.z = z; + armor_vel = vel; + } } void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) @@ -238,15 +275,18 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; + double delay = track_target_ ? 0. : config_.delay; double last_fly_time{}, target_rho{}; int count{}; geometry_msgs::Point target_pos{}; while (std::abs(fly_time - last_fly_time) > 0.01) { last_fly_time = fly_time; - target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos.z = z + vel.z * fly_time; + target_pos.x = pos.x + vel.x * (fly_time + delay) - + r * cos(yaw + v_yaw * (fly_time + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos.y = pos.y + vel.y * (fly_time + delay) - + r * sin(yaw + v_yaw * (fly_time + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos.z = z + vel.z * (fly_time + delay); target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2)); fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; count++; From 3dced19230a8f832ff50d8140b2accf4fbf0bbbb Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Fri, 28 Jul 2023 02:46:56 +0800 Subject: [PATCH 122/130] Rewrite function that get target's error. --- .../rm_gimbal_controllers/bullet_solver.h | 1 + rm_gimbal_controllers/src/bullet_solver.cpp | 67 +++++++++---------- 2 files changed, 32 insertions(+), 36 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 3b1572fd..5ed6d081 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -94,6 +94,7 @@ class BulletSolver bool track_target_; geometry_msgs::Point target_pos_{}; + double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; }; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index cbb5c8ae..98054f5b 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -145,30 +145,30 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - double fly_time = + fly_time_ = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - - config_.g * fly_time / resistance_coff_; + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; if (track_target_) { target_pos_.x = - pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); target_pos_.y = - pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); + pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); } else { double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time; + target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; + target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; target_pos_.x = target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); target_pos_.y = target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); } - target_pos_.z = z + vel.z * fly_time; + target_pos_.z = z + vel.z * fly_time_; double target_yaw = std::atan2(target_pos_.y, target_pos_.x); double error_theta = target_yaw - output_yaw_; @@ -263,8 +263,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed) { - config_ = *config_rt_buffer_.readFromRT(); - double resistance_coff = getResistanceCoefficient(bullet_speed); + double delay = track_target_ ? 0 : config_.delay; double r = r1; double z = pos.z; if (selected_armor_ != 0) @@ -272,34 +271,30 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; } - double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff / - (bullet_speed * std::cos(pitch_real)))) / - resistance_coff; - double delay = track_target_ ? 0. : config_.delay; - double last_fly_time{}, target_rho{}; - int count{}; - geometry_msgs::Point target_pos{}; - while (std::abs(fly_time - last_fly_time) > 0.01) + double error; + if (track_target_) { - last_fly_time = fly_time; - target_pos.x = pos.x + vel.x * (fly_time + delay) - - r * cos(yaw + v_yaw * (fly_time + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos.y = pos.y + vel.y * (fly_time + delay) - - r * sin(yaw + v_yaw * (fly_time + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos.z = z + vel.z * (fly_time + delay); - target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2)); - fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; - count++; - if (count >= 20 || std::isnan(fly_time)) - return 999; + double bullet_rho = + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + double bullet_x = bullet_rho * std::cos(yaw_real); + double bullet_y = bullet_rho * std::sin(yaw_real); + double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; + error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + + std::pow(target_pos_.z - bullet_z, 2)); + } + else + { + geometry_msgs::Point target_pos_after_fly_time_and_delay{}; + target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) - + r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) - + r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); + error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) + + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2)); } - double real_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff)) * - (1 - std::exp(-resistance_coff * fly_time)) / resistance_coff - - config_.g * fly_time / resistance_coff; - double target_yaw = std::atan2(target_pos.y, target_pos.x); - double error = std::sqrt(std::pow(target_rho * (std::cos(target_yaw) - std::cos(yaw_real)), 2) + - std::pow(target_rho * (std::sin(target_yaw) - std::sin(yaw_real)), 2) + - std::pow(target_pos.z - real_z, 2)); return error; } From 0c3429cbbe7239fd525b6907c3b37959d51679c8 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 28 Jul 2023 06:30:04 +0800 Subject: [PATCH 123/130] Add qd_des to ShootCmd and use it to set the desired speed of the friction wheel. --- rm_shooter_controllers/cfg/Shooter.cfg | 7 +-- .../include/rm_shooter_controllers/standard.h | 7 +-- rm_shooter_controllers/src/standard.cpp | 57 +++---------------- 3 files changed, 11 insertions(+), 60 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 787e6de3..b38dd3e6 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -13,11 +13,6 @@ gen.add("anti_block_angle", double_t, 0, "Trigger anti block angle", 0.35, 0.0, 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("qd_10", double_t, 0, "Friction wheel rotation speed when bullet speed is 10m/s", 300, 0.0, 9999) -gen.add("qd_15", double_t, 0, "Friction wheel rotation speed when bullet speed is 15m/s", 300, 0.0, 9999) -gen.add("qd_16", double_t, 0, "Friction wheel rotation speed when bullet speed is 16m/s", 300, 0.0, 9999) -gen.add("qd_18", double_t, 0, "Friction wheel rotation speed when bullet speed is 18m/s", 300, 0.0, 9999) -gen.add("qd_30", double_t, 0, "Friction wheel rotation speed when bullet speed is 30m/s", 300, 0.0, 9999) -gen.add("lf_extra_rotat_speed", double_t, 0, "Lefrt friction extra rotation speed", -200, 0.0, 200) +gen.add("extra_rotat_speed", double_t, 0, "Lefrt friction extra rotation speed", -200, 0.0, 200) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 5832f00d..88e58423 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,7 +48,6 @@ #include #include #include -#include #include @@ -58,7 +57,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double qd_10, qd_15, qd_16, qd_18, qd_30, lf_extra_rotat_speed; + double extra_rotat_speed; }; class Controller : public controller_interface::MultiInterfaceController> shoot_state_pub_; ros::Subscriber cmd_subscriber_; - ros::ServiceServer extra_friction_wheel_speed_srv_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 8fd66652..cf7f56a4 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -52,18 +52,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .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), - .qd_10 = getParam(controller_nh, "qd_10", 0.), - .qd_15 = getParam(controller_nh, "qd_15", 0.), - .qd_16 = getParam(controller_nh, "qd_16", 0.), - .qd_18 = getParam(controller_nh, "qd_18", 0.), - .qd_30 = getParam(controller_nh, "qd_30", 0.), - .lf_extra_rotat_speed = getParam(controller_nh, "lf_extra_rotat_speed", 0.) }; + .extra_rotat_speed = getParam(controller_nh, "lf_extra_rotat_speed", 0.) }; config_rt_buffer.initRT(config_); push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_qd_threshold_ = getParam(controller_nh, "push_qd_threshold", 0.); - extra_friction_wheel_speed_srv_ = - controller_nh.advertiseService("extra_friction_wheel_speed", &Controller::extraFrictionWheelSpeedCB, this); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure @@ -164,11 +157,10 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } - if ((cmd_.speed == cmd_.SPEED_ZERO_FOR_TEST || - (ctrl_friction_l_.joint_.getVelocity() >= push_qd_threshold_ * ctrl_friction_l_.command_ && - ctrl_friction_l_.joint_.getVelocity() > M_PI && - ctrl_friction_r_.joint_.getVelocity() <= push_qd_threshold_ * ctrl_friction_r_.command_ && - ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && + if ((cmd_.qd_des == 0. || (ctrl_friction_l_.joint_.getVelocity() >= push_qd_threshold_ * ctrl_friction_l_.command_ && + ctrl_friction_l_.joint_.getVelocity() > M_PI && + ctrl_friction_r_.joint_.getVelocity() <= push_qd_threshold_ * ctrl_friction_r_.command_ && + ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < @@ -226,21 +218,8 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { - double qd_des; - if (cmd_.speed == cmd_.SPEED_10M_PER_SECOND) - qd_des = config_.qd_10; - else if (cmd_.speed == cmd_.SPEED_15M_PER_SECOND) - qd_des = config_.qd_15; - else if (cmd_.speed == cmd_.SPEED_16M_PER_SECOND) - qd_des = config_.qd_16; - else if (cmd_.speed == cmd_.SPEED_18M_PER_SECOND) - qd_des = config_.qd_18; - else if (cmd_.speed == cmd_.SPEED_30M_PER_SECOND) - qd_des = config_.qd_30; - else - qd_des = 0.; - ctrl_friction_l_.setCommand(qd_des + config_.lf_extra_rotat_speed + extra_friction_wheel_speed_); - ctrl_friction_r_.setCommand(-qd_des - extra_friction_wheel_speed_); + ctrl_friction_l_.setCommand(cmd_.qd_des + config_.extra_rotat_speed); + ctrl_friction_r_.setCommand(-cmd_.qd_des - config_.extra_rotat_speed); } void Controller::normalize() @@ -249,14 +228,6 @@ void Controller::normalize() ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); } -bool Controller::extraFrictionWheelSpeedCB(rm_msgs::ExtraFrictionWheelSpeed::Request& req, - rm_msgs::ExtraFrictionWheelSpeed::Response& res) -{ - extra_friction_wheel_speed_ = req.extra_speed; - res.is_success = true; - return true; -} - void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); @@ -271,12 +242,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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.qd_10 = init_config.qd_10; - config.qd_15 = init_config.qd_15; - config.qd_16 = init_config.qd_16; - config.qd_18 = init_config.qd_18; - config.qd_30 = init_config.qd_30; - config.lf_extra_rotat_speed = init_config.lf_extra_rotat_speed; + config.extra_rotat_speed = init_config.extra_rotat_speed; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -287,12 +253,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .qd_10 = config.qd_10, - .qd_15 = config.qd_15, - .qd_16 = config.qd_16, - .qd_18 = config.qd_18, - .qd_30 = config.qd_30, - .lf_extra_rotat_speed = config.lf_extra_rotat_speed }; + .extra_rotat_speed = config.extra_rotat_speed }; config_rt_buffer.writeFromNonRT(config_non_rt); } From 9845c4baeaf9652025941a49fcaad5fc2864e655 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 31 Jul 2023 11:16:50 +0800 Subject: [PATCH 124/130] Modify variable name. --- rm_shooter_controllers/cfg/Shooter.cfg | 2 +- .../include/rm_shooter_controllers/standard.h | 4 ++-- rm_shooter_controllers/src/standard.cpp | 21 ++++++++++--------- .../test/shooter_config_template.yaml | 4 +--- 4 files changed, 15 insertions(+), 16 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index b38dd3e6..4e2a2719 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -13,6 +13,6 @@ gen.add("anti_block_angle", double_t, 0, "Trigger anti block angle", 0.35, 0.0, 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_rotat_speed", double_t, 0, "Lefrt friction extra rotation speed", -200, 0.0, 200) +gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 88e58423..0c9b1328 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -57,7 +57,7 @@ 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_rotat_speed; + double extra_wheel_speed; }; class Controller : public controller_interface::MultiInterfaceController("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); @@ -157,10 +157,11 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } - if ((cmd_.qd_des == 0. || (ctrl_friction_l_.joint_.getVelocity() >= push_qd_threshold_ * ctrl_friction_l_.command_ && - ctrl_friction_l_.joint_.getVelocity() > M_PI && - ctrl_friction_r_.joint_.getVelocity() <= push_qd_threshold_ * ctrl_friction_r_.command_ && - ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && + if ((cmd_.wheel_speed == 0. || + (ctrl_friction_l_.joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_.command_ && + ctrl_friction_l_.joint_.getVelocity() > M_PI && + ctrl_friction_r_.joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_.command_ && + ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < @@ -218,8 +219,8 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { - ctrl_friction_l_.setCommand(cmd_.qd_des + config_.extra_rotat_speed); - ctrl_friction_r_.setCommand(-cmd_.qd_des - config_.extra_rotat_speed); + ctrl_friction_l_.setCommand(cmd_.wheel_speed + config_.extra_wheel_speed); + ctrl_friction_r_.setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed); } void Controller::normalize() @@ -242,7 +243,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 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_rotat_speed = init_config.extra_rotat_speed; + config.extra_wheel_speed = init_config.extra_wheel_speed; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -253,7 +254,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .extra_rotat_speed = config.extra_rotat_speed }; + .extra_wheel_speed = config.extra_wheel_speed }; config_rt_buffer.writeFromNonRT(config_non_rt); } diff --git a/rm_shooter_controllers/test/shooter_config_template.yaml b/rm_shooter_controllers/test/shooter_config_template.yaml index b51d78db..b737aa83 100644 --- a/rm_shooter_controllers/test/shooter_config_template.yaml +++ b/rm_shooter_controllers/test/shooter_config_template.yaml @@ -12,12 +12,10 @@ controllers: 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 + 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 - qd_15: 458.0 - qd_30: 714.0 From f2ec2abe0ace68b5238f3b58de70d0352a7f4ba0 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Wed, 2 Aug 2023 23:55:36 +0800 Subject: [PATCH 125/130] Increase switch armor angle. --- rm_gimbal_controllers/src/bullet_solver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 98054f5b..e1e886ef 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -118,7 +118,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d track_target_ = std::abs(v_yaw) < max_track_target_vel_; double switch_armor_angle = track_target_ ? - acos(r / target_rho) - M_PI / 6 + (-acos(r / target_rho) + M_PI_4) * std::abs(v_yaw) / max_track_target_vel_ : + acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : M_PI / 12; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) From 41621055dcca5fb72476ba0200a67410973706b6 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 19:32:51 +0800 Subject: [PATCH 126/130] Avoid nan vel. --- rm_chassis_controllers/src/chassis_base.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index e6f0d197..2fcf50a6 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -281,10 +281,15 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p // integral vel to pos and angle tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); - odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); - odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); - double length = + double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if(length < 2000) + { + // avoid nan vel + odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); + odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); + odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); + } + length = std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2)); if (length > 0.001) { // avoid nan quat From d9c6fe59371a6a690052eab0be9bd3dbc8e5e43b Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 20:01:38 +0800 Subject: [PATCH 127/130] Add max_odom_vel and delete useless params. --- .../include/rm_chassis_controllers/chassis_base.h | 3 ++- rm_chassis_controllers/src/chassis_base.cpp | 5 ++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 0f87e7d0..f12f499b 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -146,8 +146,9 @@ class ChassisBase : public controller_interface::MultiInterfaceController hardware_interface::EffortJointInterface* effort_joint_interface_{}; std::vector joint_handles_{}; - double wheel_base_{}, wheel_track_{}, wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, + double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, power_offset_{}; + double max_odom_vel_; bool enable_odom_tf_ = false; bool topic_update_ = false; bool publish_odom_tf_ = false; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 2fcf50a6..6ab68985 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -61,9 +61,8 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan return false; } wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); - wheel_track_ = getParam(controller_nh, "wheel_track", 0.410); - wheel_base_ = getParam(controller_nh, "wheel_base", 0.320); twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); + max_odom_vel_ = getParam(controller_nh,"max_odom_vel",0); enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); @@ -282,7 +281,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if(length < 2000) + if(length < max_odom_vel_) { // avoid nan vel odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); From df4b9028c29f83c59f050c7615751088b02f6c42 Mon Sep 17 00:00:00 2001 From: yezi <1536117624@qq.com> Date: Sun, 6 Aug 2023 20:05:47 +0800 Subject: [PATCH 128/130] Run pre-commit. --- .../rm_chassis_controllers/chassis_base.h | 4 ++-- rm_chassis_controllers/src/chassis_base.cpp | 7 +++--- rm_gimbal_controllers/src/bullet_solver.cpp | 23 +++++++++++-------- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index f12f499b..d88be651 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -146,8 +146,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController hardware_interface::EffortJointInterface* effort_joint_interface_{}; std::vector joint_handles_{}; - double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, - velocity_coeff_{}, power_offset_{}; + double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, + power_offset_{}; double max_odom_vel_; bool enable_odom_tf_ = false; bool topic_update_ = false; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 6ab68985..bba37a5f 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -62,7 +62,7 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); - max_odom_vel_ = getParam(controller_nh,"max_odom_vel",0); + max_odom_vel_ = getParam(controller_nh, "max_odom_vel", 0); enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); @@ -280,8 +280,9 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p // integral vel to pos and angle tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - double length = std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if(length < max_odom_vel_) + double length = + std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if (length < max_odom_vel_) { // avoid nan vel odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e1e886ef..94188c8a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -116,10 +116,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = - track_target_ ? - acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : - M_PI / 12; + double switch_armor_angle = track_target_ ? + acos(r / target_rho) - M_PI / 12 + + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : + M_PI / 12; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -287,13 +287,16 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; - target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.x = + pos.x + vel.x * (fly_time_ + delay) - + r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.y = + pos.y + vel.y * (fly_time_ + delay) - + r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2)); + error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } From 18e937e1a98d4c5a27103ff82ff88268da854762 Mon Sep 17 00:00:00 2001 From: cc0h <116955603+cc0h@users.noreply.github.com> Date: Sat, 7 Oct 2023 14:03:30 +0800 Subject: [PATCH 129/130] Update deb_package.yml Work with the newly deployed server. --- .github/workflows/deb_package.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/deb_package.yml b/.github/workflows/deb_package.yml index fe6e48b8..fedd1648 100644 --- a/.github/workflows/deb_package.yml +++ b/.github/workflows/deb_package.yml @@ -31,8 +31,9 @@ jobs: host: ${{ secrets.SOURCE_SSH_HOST }} username: ${{ secrets.SOURCE_SSH_NAME }} key: ${{ secrets.SOURCE_SSH_KEYGEN }} + port: ${{ secrets.SOURCE_SSH_PORT }} source: "*.deb" - target: "/usr/local/web/rm-source/ubuntu/incoming/" + target: "/home/dynamicx/package_hub/wwwroot/ppa/incoming" - name: Deploy deb package to source if: ${{ github.repository == 'rm-controls/rm_controllers' }} uses: appleboy/ssh-action@master @@ -40,7 +41,7 @@ jobs: host: ${{ secrets.SOURCE_SSH_HOST }} username: ${{ secrets.SOURCE_SSH_NAME }} key: ${{ secrets.SOURCE_SSH_KEYGEN }} + port: ${{ secrets.SOURCE_SSH_PORT }} script: | - cd /usr/local/web/rm-source/ubuntu/incoming/ - reprepro -s -b /usr/local/web/rm-source/ubuntu/ --waitforlock 1000 includedeb focal /usr/local/web/rm-source/ubuntu/incoming/*.deb && \ - rm -f *.deb + cd /home/dynamicx/package_hub/wwwroot/ppa/ + bash deploy.sh From 700ab07be19c608f51178208f141ba294b8a8b6f Mon Sep 17 00:00:00 2001 From: Kook Date: Sat, 30 Dec 2023 21:17:38 +0800 Subject: [PATCH 130/130] Modifying the method to compensate for friction. --- .../include/rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/gimbal_base.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index e09d968f..1c919927 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -175,7 +175,7 @@ class Controller : public controller_interface::MultiInterfaceController velocity_dead_zone_) + if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; - else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_dead_zone_) + else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; + else + resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + yaw_k_v_ * yaw_vel_des + resistance_compensation); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des);