Updated robot parameters

mg_wheel_interface/assets/mg_base.urdf

@@ -13,7 +13,7 @@
     <link name="base-link">
         <visual>
             <geometry>
-                <box size="0.5 0.5 0.2" color="yellow"/>
+                <box size="0.24 0.32 0.12" color="yellow"/>
             </geometry>
             <origin xyz="0 0 0.12" rpy="0 0 0" />
             <material name="red" />
@@ -24,7 +24,7 @@
 
         <visual>
             <geometry>
-                <cylinder radius="0.1" length="0.05"/>
+                <cylinder radius="0.075" length="0.05"/>
             </geometry>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <material name="gray" />
@@ -34,7 +34,7 @@
     <link name="right-wheel">
         <visual>
             <geometry>
-                <cylinder radius="0.1" length="0.05"/>
+                <cylinder radius="0.075" length="0.05"/>
             </geometry>
             <origin xyz="0 0 0." rpy="0 0 0" />
             <material name="gray" />
@@ -43,21 +43,25 @@
     <joint name="right_motor" type="continuous">
         <parent link="base-link"/>
         <child link="right-wheel"/>
-        <origin xyz="0 -0.25 0.1" rpy="1.57 0 0" />
+        <origin xyz="0 -0.16 0.075" rpy="1.57 0 0" />
         <axis xyz="0 0 1"/>
     </joint>
 
     <joint name="left_motor" type="continuous">
         <parent link="base-link"/>
         <child link="left-wheel"/>
-        <origin xyz="0 0.25 0.1" rpy="1.57 0 0" />
+        <origin xyz="0 0.16 0.075" rpy="1.57 0 0" />
         <axis xyz="0 0 1"/>
     </joint>
 
 
     <ros2_control name="mg-base" type="system">
         <hardware>
+        <!--
             <plugin>mg_wheel_interface/MgOdriveInterface</plugin>
+        -->
+            <plugin>mock_components/GenericSystem</plugin> 
+            <param name="calculate_dynamics">true</param> 
         </hardware>
         <joint name="left_motor">
             <command_interface name="velocity"/>

mg_wheel_interface/assets/mg_base_controllers.yaml

@@ -18,5 +18,5 @@ diffdrive_controller:
 
     open_loop: true 
 
-    wheel_separation: 0.5
+    wheel_separation: 0.258
-    wheel_radius: 0.1
+    wheel_radius: 0.075

mg_wheel_interface/src/mg_wheel_interface.cpp

@@ -43,24 +43,6 @@ CallbackReturn mg_wheel_interface::MgOdriveInterface::on_configure(const rclcpp_
 CallbackReturn mg_wheel_interface::MgOdriveInterface::on_shutdown(const rclcpp_lifecycle::State &)
 {
     if(odrive_serial_interface.IsOpen()){
-        std::string cmd_left;
-        std::string cmd_right;
-        int hash;
-
-        cmd_left  = "v 1 " + std::to_string(0)  + " ";
-        cmd_right = "v 0 " + std::to_string(0) + " ";
-
-        hash = 0;
-        for(const auto &c: cmd_left) {
-            hash ^= c;
-        }
-        cmd_left += "*"+std::to_string(hash)+"\n";
-
-        hash = 0;
-        for(const auto &c: cmd_right) {
-            hash ^= c;
-        }
-        cmd_right += "*"+std::to_string(hash)+"\n";
         odrive_serial_interface.Close();
     }
     return CallbackReturn::SUCCESS;
@@ -96,13 +78,15 @@ std::vector<hardware_interface::CommandInterface> mg_wheel_interface::MgOdriveIn
 
 hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period)
 {
-    std::string resp_l, resp_r;
+    /*
-    odrive_serial_interface.Write("f 1\n");
+     *   std::string resp_l, resp_r;
-    odrive_serial_interface.ReadLine(resp_l,'\n',10);
+     *   odrive_serial_interface.Write("f 1\n");
-    odrive_serial_interface.Write("f 0\n");
+     *   odrive_serial_interface.ReadLine(resp_l,'\n',10);
-    odrive_serial_interface.ReadLine(resp_r,'\n',10);
+     *   odrive_serial_interface.Write("f 0\n");
-    left_wheel_pos_state =   std::stod(resp_l)*2*M_PI;
+     *   odrive_serial_interface.ReadLine(resp_r,'\n',10);
-    right_wheel_pos_state = -std::stod(resp_r)*2*M_PI;
+     *   left_wheel_pos_state =   std::stod(resp_l)*2*M_PI;
+     *   right_wheel_pos_state = -std::stod(resp_r)*2*M_PI;
+    */
     return hardware_interface::return_type::OK ;
 }
 
@@ -113,21 +97,8 @@ hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::write(con
     std::string cmd_right;
     int hash;
 
-    cmd_left  = "v 1 " + std::to_string(left_wheel_vel_cmd/2*M_PI)  + " ";
+    cmd_left  = std::to_string(left_wheel_vel_cmd/2*M_PI)  + " ";
-    cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/2*M_PI) + " ";
+    cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI);
-
-    hash = 0;
-    for(const auto &c: cmd_left) {
-        hash ^= c;
-    }
-    cmd_left += "*"+std::to_string(hash)+"\n";
-
-    hash = 0;
-    for(const auto &c: cmd_right) {
-        hash ^= c;
-    }
-    cmd_right += "*"+std::to_string(hash)+"\n";
-
 
     try {
         odrive_serial_interface.Write(cmd_left + cmd_right);