fix hero xacro binding

Author: tz61

decision/hero_vehicle/src/dbus_interpreter.cpp

@@ -113,7 +113,7 @@ void DbusInterpreter::input_video_link_vt03(const operation_interface::msg::VT03
     wheel = (msg->wheel-1024)/684.0; apply_deadzone(wheel);
 
     if(msg->cns == 1) lsw = "MID";
-    if(msg->cns == 0) lsw = ""; //rough shit
+    if(msg->cns == 0) lsw = ""; // TODO rough shit
 }
 
 void DbusInterpreter::update()

decomposition/metav_description/urdf/hero/hero.xacro

@@ -49,12 +49,47 @@
 
         <ros2_control name="dm_motors_yaw" type="system">
             <hardware>
-                <plugin>meta_hardware/MetaRobotDjiMotorNetwork</plugin>
+                <plugin>meta_hardware/MetaRobotDmMotorNetwork</plugin>
                 <param name="can_network_name">can_1</param>
+                <param name="master_id">0x30</param>
+            </hardware>
+            <joint name="yaw_gimbal_joint">
+                <command_interface name="position" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+                <param name="motor_model">DaMiao</param>
+                <param name="motor_id">9</param>
+                <param name="mechanical_reduction">1.0</param>
+                <param name="max_pos">12.5</param>
+                <param name="max_vel">45.0</param>
+                <param name="max_effort">12.0</param>
+                <param name="offset">0.0</param>
+                <param name="control_mode">mit</param>
+                <param name="Kp">30.0</param>
+                <param name="Kd">0.5</param>
+            </joint>
+        </ros2_control>
+        <ros2_control name="mi_motors_pitch" type="system">
+            <hardware>
+                <plugin>meta_hardware/MetaRobotMiMotorNetwork</plugin>
+                <param name="can_network_name">can_2</param>
             </hardware>
-            <xacro:gimbal_transmission prefix="pitch" mechanical_reduction="20.0" motor_id="7" offset="0"/>
-            <xacro:gimbal_transmission prefix="yaw" mechanical_reduction="20.0" motor_id="8" offset="0"/>
-            
+            <!-- <xacro:gimbal_transmission prefix="pitch" mechanical_reduction="20.0" motor_id="1" offset="0"/>
+             -->
+            <joint name="pitch_gimbal_joint">
+                <command_interface name="position" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+                <param name="motor_model">CyberGear</param>
+                <param name="motor_id">1</param>
+                <param name="mechanical_reduction">1.0</param>
+                <param name="offset">0.7599</param> <!-- read from (int("7842",16)-65535/2)/(65535/2)*4*pi = -0.75990, remember to negate -->
+                <param name="control_mode">dynamic</param>
+                <param name="Kp">30.0</param>
+                <param name="Kd">1.0</param>
+            </joint>
         </ros2_control>
 
         <ros2_control name="dji_motors_gimbal" type="system">

decomposition/metav_description/urdf/playground/multiple_motors.xacro

@@ -74,7 +74,9 @@
                 <param name="motor_model">CyberGear</param>
                 <param name="motor_id">1</param>
                 <param name="mechanical_reduction">1.0</param>
-                <param name="offset">0.0</param>
+                <!-- cansend can_2 01000001#0000.0000.0000.000 -->
+                <!-- can_2  020001FD   [8]  77 2C 7F F5 7F FF 01 2 -->
+                <param name="offset">0.86652061961</param> <!-- -0.7338292567 (int("772C",16)-65535/2)/(65535/2)*4*pi -->
                 <param name="control_mode">dynamic</param>
                 <param name="Kp">30.0</param>
                 <param name="Kd">1.0</param>

decomposition/scara_moveit

@@ -20,6 +20,8 @@
   <depend>angles</depend>
 
   <depend>serial_driver</depend>
+  
+  <depend>forward_command_cntroller</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

execution/meta_hardware/package.xml

@@ -16,6 +16,11 @@ controller_manager:
 
     shoot_controller:
       type: meta_shoot_controller/ShootController
+    
+    forward_pitch_controller:
+        type: forward_command_controller/ForwardCommandController
+    forward_yaw_controller:
+        type: forward_command_controller/ForwardCommandController
 
 omni_chassis_controller:
   ros__parameters:
@@ -133,4 +138,14 @@ referee_serial:
     video_link_baud: 921600
     isDebug: false
     debugUnhandledShown: false
-    warningShown: false
+    warningShown: false
+forward_pitch_controller:
+  ros__parameters:
+    joints:
+      - pitch_gimbal_joint
+    interface_name: position
+forward_yaw_controller:
+  ros__parameters:
+    joints:
+      - yaw_gimbal_joint
+    interface_name: position

meta_bringup/config/hero.yaml

@@ -101,7 +101,9 @@ def generate_launch_description():
         load_controller('wheels_pid_controller'),
         # load_controller('gimbal_controller'),
         load_controller('omni_chassis_controller'),
-        load_controller('shoot_controller')
+        load_controller('shoot_controller'),
+        load_controller('forward_yaw_controller'),
+        load_controller('forward_pitch_controller')
     ]
     referee_system_node = Node(
         package='referee_serial',

meta_bringup/launch/hero.launch.py

@@ -19,7 +19,7 @@ def generate_launch_description():
         package="fdilink_ahrs",
         executable="ahrs_driver_node",
         parameters=[{'if_debug_': False,
-            'serial_port_':'/dev/fdilink_ahrs',
+            'serial_port_':'/dev/tty_IMU',
             'serial_baud_':921600,
             'imu_topic':'/imu',
             'imu_frame_id_':'gyro_link',