Merge pull request #83 from rm-controls/dev

Merge the new OmniController to master
rm-controls · Sep 3, 2022 · 67a67bf · 67a67bf
2 parents fe0bf2d + b0765ab
commit 67a67bf
Show file tree

Hide file tree

Showing 16 changed files with 89 additions and 307 deletions.
diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt
@@ -20,13 +20,9 @@ find_package(catkin REQUIRED
 
         nav_msgs
         pluginlib
-        hardware_interface
         controller_interface
-        forward_command_controller
         realtime_tools
-        control_toolbox
         effort_controllers
-        tf2
         tf2_geometry_msgs
         angles
         )
@@ -56,13 +52,9 @@ catkin_package(
 
         nav_msgs
         pluginlib
-        hardware_interface
         controller_interface
-        forward_command_controller
         realtime_tools
-        control_toolbox
         effort_controllers
-        tf2
         tf2_geometry_msgs
         angles
         LIBRARIES ${PROJECT_NAME}
@@ -82,7 +74,6 @@ include_directories(
 ## Declare cpp executables
 add_library(${PROJECT_NAME}
         src/chassis_base.cpp
-        src/mecanum.cpp
         src/omni.cpp
         src/sentry.cpp
         src/swerve.cpp

diff --git a/rm_chassis_controllers/config/localization.yaml b/rm_chassis_controllers/config/localization.yaml
diff --git a/rm_chassis_controllers/config/template_mecanum.yaml b/rm_chassis_controllers/config/template_mecanum.yaml
diff --git a/rm_chassis_controllers/config/template_omni.yaml b/rm_chassis_controllers/config/template_omni.yaml
@@ -14,17 +14,23 @@ controllers:
     twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]
 
     # OmniController
-    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 }
-    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 }
-    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 }
-    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 }
+    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.6, 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
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
@@ -114,7 +114,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
    */
   void gyro();
   virtual void moveJoint(const ros::Time& time, const ros::Duration& period) = 0;
-  virtual geometry_msgs::Twist forwardKinematics() = 0;
+  virtual geometry_msgs::Twist odometry() = 0;
   /** @brief Init frame on base_link. Integral vel to pos and angle.
    *
    * @param time The current time.

diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h b/rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h
@@ -1,10 +1,12 @@
 //
-// Created by yezi on 2021/12/3.
+// Created by qiayuan on 2022/7/29.
 //
 
 #pragma once
 
-#include <rm_chassis_controllers/chassis_base.h>
+#include <Eigen/Dense>
+
+#include "rm_chassis_controllers/chassis_base.h"
 
 namespace rm_chassis_controllers
 {
@@ -16,9 +18,10 @@ class OmniController : public ChassisBase<rm_control::RobotStateInterface, hardw
 
 private:
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
-  geometry_msgs::Twist forwardKinematics() override;
+  geometry_msgs::Twist odometry() override;
 
-  double chassis_radius_;
-  effort_controllers::JointVelocityController ctrl_lf_, ctrl_rf_, ctrl_lb_, ctrl_rb_;
+  std::vector<std::shared_ptr<effort_controllers::JointVelocityController>> joints_;
+  Eigen::MatrixXd chassis2joints_;
 };
+
 }  // namespace rm_chassis_controllers
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h b/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h
@@ -71,7 +71,7 @@ class SentryController : public ChassisBase<rm_control::RobotStateInterface, har
    *
    * @return Calculated vel_data included linear_x.
    */
-  geometry_msgs::Twist forwardKinematics() override;
+  geometry_msgs::Twist odometry() override;
 
   effort_controllers::JointVelocityController ctrl_wheel_;
   effort_controllers::JointPositionController ctrl_catapult_joint_;

diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h
@@ -60,7 +60,7 @@ class SwerveController : public ChassisBase<rm_control::RobotStateInterface, har
 
 private:
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
-  geometry_msgs::Twist forwardKinematics() override;
+  geometry_msgs::Twist odometry() override;
   std::vector<Module> modules_;
 };
 

diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml
@@ -20,19 +20,12 @@
 
     <depend>nav_msgs</depend>
     <depend>pluginlib</depend>
-    <depend>hardware_interface</depend>
     <depend>controller_interface</depend>
-    <depend>forward_command_controller</depend>
     <depend>realtime_tools</depend>
-    <depend>control_toolbox</depend>
     <depend>effort_controllers</depend>
-    <depend>tf2</depend>
     <depend>tf2_geometry_msgs</depend>
     <depend>angles</depend>
 
-    <exec_depend>imu_sensor_controller</exec_depend>
-    <exec_depend>robot_localization</exec_depend>
-
     <export>
         <controller_interface plugin="${prefix}/rm_chassis_controllers_plugins.xml"/>
     </export>

diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml
@@ -1,13 +1,5 @@
 <library path="lib/librm_chassis_controllers">
 
-    <class name="rm_chassis_controllers/MecanumController"
-           type="rm_chassis_controllers::MecanumController"
-           base_class_type="controller_interface::ControllerBase">
-        <description>
-            The StandardController is RoboMaster mecanum wheel Chassis controller. It expects a EffortJointInterface
-            type of hardware interface.
-        </description>
-    </class>
     <class name="rm_chassis_controllers/OmniController"
            type="rm_chassis_controllers::OmniController"
            base_class_type="controller_interface::ControllerBase">

diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp
@@ -274,7 +274,7 @@ void ChassisBase<T...>::raw()
 template <typename... T>
 void ChassisBase<T...>::updateOdom(const ros::Time& time, const ros::Duration& period)
 {
-  geometry_msgs::Twist vel_base = forwardKinematics();  // on base_link frame
+  geometry_msgs::Twist vel_base = odometry();  // on base_link frame
   if (enable_odom_tf_)
   {
     geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom;

diff --git a/rm_chassis_controllers/src/mecanum.cpp b/rm_chassis_controllers/src/mecanum.cpp