Implement rm_dshot_shooter_controllers and update configurations by HydrogenZp · Pull Request #4 · HydrogenZp/rm_controllers

HydrogenZp · 2026-03-27T10:04:38Z

No description provided.

…ate related configurations.

…d comment out unused time variable in setDshotVelocity function.

gemini-code-assist

Code Review

This pull request introduces the rm_dshot_shooter_controllers package, which implements a shooter controller for RoboMaster robots with support for STOP, READY, PUSH, and BLOCK states. The code review identified several critical issues, including potential memory leaks from raw pointer usage, a compilation error due to C++20 features in a C++14 project, and a possible segmentation fault from missing bounds checks. Additional feedback focuses on improving code maintainability by removing unused variables, refactoring complex state transition logic, and ensuring documentation and package names are consistent throughout the repository.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/include/rm_dshot_shooter_controllers/standard.h

+  std::vector<std::vector<velocity_controllers::JointVelocityController*>> ctrls_friction_;
+  std::vector<std::vector<std::shared_ptr<control_toolbox::Pid>>> friction_pid_controllers_;
+  effort_controllers::JointPositionController ctrl_trigger_;
+  LowPassFilter* lp_filter_;
+  int push_per_rotation_{}, count_{};
+  double push_wheel_speed_threshold_{};
+  double freq_threshold_{};
+  bool dynamic_reconfig_initialized_ = false;
+  bool state_changed_ = false;
+  bool enter_ready_ = false;
+  bool maybe_block_ = false;
+  int friction_block_count = 0;
+  bool last_friction_wheel_block = false, friction_wheel_block = false;
+  double friction_block_vel_{};
+  double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{};
+  bool has_shoot_ = false, has_shoot_last_ = false;
+  bool wheel_speed_drop_ = true;
+  double last_fricition_speed_{};
+  double last_friction_change_speed_{};
+
+  ros::Time last_shoot_time_, block_time_, last_block_time_;
+  enum
+  {
+    STOP,
+    READY,
+    PUSH,
+    BLOCK
+  };
+  int state_ = STOP;
+  Config config_{};
+  realtime_tools::RealtimeBuffer<Config> config_rt_buffer;
+  realtime_tools::RealtimeBuffer<rm_msgs::ShootCmd> cmd_rt_buffer_;
+  rm_msgs::ShootCmd cmd_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>> local_heat_state_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootState>> shoot_state_pub_;
+  std::vector<std::vector<std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::JointControllerState>>>>
+      friction_state_publishers_;
+  int loop_count_ = 0;
+  ros::Subscriber cmd_subscriber_;
+  dynamic_reconfigure::Server<rm_dshot_shooter_controllers::ShooterConfig>* d_srv_{};


The class Controller manages several resources with raw pointers (ctrls_friction_ on line 94, lp_filter_ on line 97, and d_srv_ on line 133) which are allocated with new in init() but are never deallocated. This will cause memory leaks. Please use smart pointers (std::unique_ptr) to manage the lifecycle of these objects automatically.

For example:

std::vector<std::vector<velocity_controllers::JointVelocityController*>> ctrls_friction_; should become std::vector<std::vector<std::unique_ptr<velocity_controllers::JointVelocityController>>> ctrls_friction_;

LowPassFilter* lp_filter_; should become std::unique_ptr<LowPassFilter> lp_filter_;

dynamic_reconfigure::Server<...>* d_srv_{}; should become std::unique_ptr<dynamic_reconfigure::Server<...>> d_srv_;

You will also need to update the allocation logic in standard.cpp to use std::make_unique or std::unique_ptr's constructor and std::move.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+  config_ = { .block_effort = getParam(controller_nh, "block_effort", 0.),
+              .block_speed = getParam(controller_nh, "block_speed", 0.),
+              .block_duration = getParam(controller_nh, "block_duration", 0.),
+              .block_overtime = getParam(controller_nh, "block_overtime", 0.),
+              .anti_block_angle = getParam(controller_nh, "anti_block_angle", 0.),
+              .anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.),
+              .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1),
+              .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1),
+              .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.),
+              .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.),
+              .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) };


Designated initializers (e.g., .member = value) are used here to initialize the config_ struct. This is a C++20 feature, but the project is configured to use C++14 in CMakeLists.txt. This will cause a compilation error on compilers that strictly follow the C++14 standard. Please use aggregate initialization, which is compatible with C++14.

Suggested change

config_ = { .block_effort = getParam(controller_nh, "block_effort", 0.),

.block_speed = getParam(controller_nh, "block_speed", 0.),

.block_duration = getParam(controller_nh, "block_duration", 0.),

.block_overtime = getParam(controller_nh, "block_overtime", 0.),

.anti_block_angle = getParam(controller_nh, "anti_block_angle", 0.),

.anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.),

.forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1),

.exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1),

.extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.),

.wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.),

.wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) };

config_ = { getParam(controller_nh, "block_effort", 0.),

getParam(controller_nh, "block_speed", 0.),

getParam(controller_nh, "block_duration", 0.),

getParam(controller_nh, "block_overtime", 0.),

getParam(controller_nh, "anti_block_angle", 0.),

getParam(controller_nh, "anti_block_threshold", 0.),

getParam(controller_nh, "forward_push_threshold", 0.1),

getParam(controller_nh, "exit_push_threshold", 0.1),

getParam(controller_nh, "extra_wheel_speed", 0.),

getParam(controller_nh, "wheel_speed_drop_threshold", 10.),

getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) };

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+
+void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period)
+{
+  lp_filter_->input(ctrls_friction_[0][0]->joint_.getVelocity());


The code directly accesses ctrls_friction_[0][0] without checking if the ctrls_friction_ vector and its nested vector are non-empty. If no friction wheels are configured in the YAML file, this will result in a segmentation fault. Please add checks to ensure these vectors are not empty before accessing their elements.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/CHANGELOG.rst

@@ -0,0 +1,142 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rm_shooter_controllers


The changelog title indicates this is for rm_shooter_controllers, but the package is rm_dshot_shooter_controllers. Please update the package name for consistency.

Suggested change

Changelog for package rm_shooter_controllers

Changelog for package rm_dshot_shooter_controllers

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/CMakeLists.txt

+        velocity_controllers
+        effort_controllers
+        dynamic_reconfigure
+        LIBRARIES ${PROJECT_NAME}


The LIBRARIES ${PROJECT_NAME} declaration is redundant because catkin_package automatically exports targets created with add_library. Removing this line will make the file cleaner and align with modern catkin practices.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/include/rm_dshot_shooter_controllers/standard.h

+  int push_per_rotation_{}, count_{};
+  double push_wheel_speed_threshold_{};
+  double freq_threshold_{};
+  bool dynamic_reconfig_initialized_ = false;
+  bool state_changed_ = false;
+  bool enter_ready_ = false;
+  bool maybe_block_ = false;
+  int friction_block_count = 0;
+  bool last_friction_wheel_block = false, friction_wheel_block = false;
+  double friction_block_vel_{};
+  double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{};
+  bool has_shoot_ = false, has_shoot_last_ = false;
+  bool wheel_speed_drop_ = true;
+  double last_fricition_speed_{};
+  double last_friction_change_speed_{};


There are several unused member variables in the Controller class. They should be removed to improve code clarity and reduce memory footprint.

The unused variables are:

count_ (line 98)

push_wheel_speed_threshold_ (line 99)

friction_block_count (line 105)

last_friction_wheel_block, friction_wheel_block (line 106)

friction_block_vel_ (line 107)

anti_friction_block_duty_cycle_, anti_friction_block_vel_ (line 108)

has_shoot_last_ (line 109)

wheel_speed_drop_ (line 110)

last_friction_change_speed_ (line 112)

Please also remove their initializations in standard.cpp.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+  controller_nh.getParam("friction", friction);
+  for (const auto& its : friction)
+  {
+    std::vector<double> wheel_speed_offset_temp;


The variable wheel_speed_offset_temp is declared but never used. It should be removed to improve code clarity.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+      if ((state_ != PUSH || cmd_.mode != READY) ||
+          (cmd_.mode == READY &&
+           (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
+                config_.exit_push_threshold ||
+            cmd_.hz >= freq_threshold_)))


The conditional logic for state transitions is very complex and difficult to understand at a glance. To improve readability and maintainability, consider breaking this complex condition into smaller, well-named boolean variables.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+  {
+    for (size_t j = 0; j < ctrls_friction_[i].size(); j++)
+    {
+      if (ctrls_friction_[i][j]->joint_.getVelocity() <= M_PI)


The magic number M_PI is used as a velocity threshold to check if the friction wheels are ready. This makes the code harder to understand. It would be better to define this as a named constant or a configurable parameter (e.g., min_wheel_speed_for_push) to improve clarity.

gemini-code-assist · 2026-03-27T10:06:49Z

rm_dshot_shooter_controllers/src/standard.cpp

+      if (j == 0)
+      {
+        target_speed = cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_back;
+      }
+      else if (j == 1)
+      {
+        target_speed = cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_front;
+      }


The logic for setting target_speed relies on the index j being 0 or 1. This is brittle and makes assumptions about the robot's hardware configuration (e.g., exactly two friction wheels per side, with specific roles for front and back). If more than two wheels are configured, target_speed will remain 0.0 for j > 1, which is a bug. Consider making this mapping from joint to speed offset configurable to make the controller more robust and generic.

ZiDXie added 4 commits November 1, 2025 16:04

Add initial implementation of rm_dshot_shooter_controllers package.

4ed8cca

Rename rm_shooter_controllers to rm_dshot_shooter_controllers and upd…

1c887c9

…ate related configurations.

Increase LowPassFilter cutoff frequency for dshot velocity filters an…

82e878d

…d comment out unused time variable in setDshotVelocity function.

Remove wheel ooffset.

2855af6

HydrogenZp merged commit 0d767a9 into HydrogenZp:dshot Mar 27, 2026
0 of 5 checks passed

gemini-code-assist bot reviewed Mar 27, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement rm_dshot_shooter_controllers and update configurations#4

Implement rm_dshot_shooter_controllers and update configurations#4
HydrogenZp merged 4 commits intoHydrogenZp:dshotfrom
ZiDXie:feature/dshot

HydrogenZp commented Mar 27, 2026

Uh oh!

Uh oh!

gemini-code-assist bot left a comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

gemini-code-assist bot Mar 27, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

		@@ -0,0 +1,142 @@
		^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
		Changelog for package rm_shooter_controllers

	Changelog for package rm_shooter_controllers
	Changelog for package rm_dshot_shooter_controllers

Conversation

HydrogenZp commented Mar 27, 2026

Uh oh!

Uh oh!

gemini-code-assist bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

gemini-code-assist bot Mar 27, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants