Added position interface for joint state publisher

firmware/base/src/config.h

@@ -1,5 +1,9 @@
 #pragma once
 
+// STEP_PIN 26
+// DIR_PIN 27
+
+
 //############## CONFIG STEPPERS ################
 #define DIR_PIN_A   26
 #define DIR_PIN_B   16
@@ -10,7 +14,7 @@
 #define SM_B 1
 
 #define PULSE_PER_REV (1 / 3200.0)
-#define SPEED_SET_TIMEOUT 1000
+#define SPEED_SET_TIMEOUT 1000000
 //###############################################
 
 //================ CONFIG ENCODERS =====================
@@ -20,7 +24,7 @@
 #define ENCODER_LEFT_PIN_B  7
 #define ENCODER_CPR         3840
 
-#define WHEEL_RADIUS     0.0279
-#define WHEEL_SEPARATION 0.310735
+#define WHEEL_RADIUS     0.0300
+#define WHEEL_SEPARATION 0.264
 #define TIMER_CYCLE_US   1000
-//======================================================
+//======================================================

firmware/base/src/main.c

@@ -213,8 +213,6 @@ void stepper_fifo(char c, uint8_t itf) {
         double vr = btod(stepper_instr + 10);
 
         set_speeds(vl, vr);
-        last_speed_change = SPEED_SET_TIMEOUT;
-
         // tud_cdc_n_write(itf, (uint8_t const *) stepper_instr, 18);
         // tud_cdc_n_write_flush(itf);
     }
@@ -265,10 +263,6 @@ int main()
     
     
     while (true) {
-        if(last_speed_change--) {
-            set_speeds(0,  0);
-            last_speed_change = SPEED_SET_TIMEOUT;
-        }
         tud_task();
         sleep_ms(1);
     }

firmware/base/src/stepper.c

@@ -34,7 +34,7 @@ void start_pulse(PIO pio, uint sm, uint offset, uint pin, uint freq) {
 
 void update_sm(PIO pio, uint sm, const uint pin ,double v) {
     double u_v = fabs(v);
-    if(u_v > 0.0005)
+    if(u_v > 0.00005)
         pio->txf[sm] = (int)((double)clock_get_hz(clk_sys) * PULSE_PER_REV / u_v) / 2 - 5;
     else
         pio->txf[sm] = 0;

mg_bringup/launch/base.launch.py

@@ -46,7 +46,7 @@ def generate_launch_description():
             condition=UnlessCondition(LaunchConfiguration('local_test')),
             parameters=[{
                 'odom': "odom",
-                'serial_path': "/dev/ttyACM0",
+                'serial_path': "/dev/ttyACM1",
             }],
 
             emulate_tty=True,

mg_bringup/launch/launch.py

@@ -33,7 +33,7 @@ def generate_launch_description():
             condition=UnlessCondition(LaunchConfiguration('local_test')),
             parameters=[{
                 'odom': "odom",
-                'serial_path': "/dev/ttyACM0",
+                'serial_path': "/dev/ttyACM1",
             }],
 
             emulate_tty=True,

mg_control/assets/toid_general_params.yaml

@@ -18,5 +18,5 @@ diffdrive_controller:
 
     open_loop: true 
 
-    wheel_separation: 0.258
-    wheel_radius: 0.0375
+    wheel_separation: 0.192
+    wheel_radius: 0.032

mg_control/launch/toid.launch.py

@@ -48,7 +48,7 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
-        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_sim:=', use_mock])}]
+        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
     )
 
     controller_manager = Node(
@@ -108,4 +108,4 @@ def generate_launch_description():
         joint_state_broadcaster,
         diffbot_base_controller,
         rviz_node
-    ])
+    ])

mg_control/src/mg_control.cpp

@@ -11,7 +11,7 @@ CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::Hardwar
     if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
         serial_port_name = info_.hardware_parameters["device_path"];
     } else {
-        serial_port_name = "/dev/ttyACM1";
+        serial_port_name = "/dev/ttyACM0";
     }
 
     left_wheel_pos_state  = 0;
@@ -31,6 +31,8 @@ CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::Stat
                      "Failed to open serial port (Is the stepper driver plugged in)");
         return CallbackReturn::ERROR;
     }
+    RCLCPP_INFO(rclcpp::get_logger("MgStepperInterface"),
+                     "Configured stepper motor interface");
     return CallbackReturn::SUCCESS;
 }
 
@@ -45,8 +47,8 @@ std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_s
 
     std::vector<hardware_interface::StateInterface> state_interfaces;
     state_interfaces.reserve(2);
-    state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
-    state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
+    state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
+    state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
 
     return state_interfaces;
 }
@@ -55,8 +57,8 @@ std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export
 
     std::vector<hardware_interface::CommandInterface> command_interfaces;
     command_interfaces.reserve(2);
-    command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
-    command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
+    command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
+    command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
 
     return command_interfaces;
 }

mg_odometry/src/mg_odom_publisher.cpp

@@ -23,7 +23,7 @@
 
 #define TIMEOUT 10u
 
-constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM0";
+constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
 
 using SendDoubleSrv = mg_msgs::srv::SendDouble;
 using ZeroSrv       = std_srvs::srv::Empty;

scripts/build_base.sh

@@ -0,0 +1,10 @@
+source /opt/ros/jazzy/setup.bash
+
+colcon build --packages-select \
+    mg_bringup \
+    mg_navigation \
+    mg_msgs \
+    mg_control \
+    mg_odometry \
+    mg_obstacles \
+    toid_bot_description

scripts/docker_build.sh

@@ -1,3 +1,4 @@
+#!/bin/bash
 usage() {
 	cat <<EOF
 Usage:

scripts/run_bt.sh

@@ -5,6 +5,8 @@ ros_distro="jazzy"
 script_dir=$(dirname "$(readlink -f "${bash_source[0]}")")
 pids=()
 
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
 source "/opt/ros/${ROS_DISTRO}/setup.bash"
 source "./install/setup.bash"
 

scripts/test_wheel.sh

@@ -0,0 +1,78 @@
+#!/bin/bash
+
+TARGET="$1"
+
+if [[ "$TARGET" == "-h" || "$TARGET" == "--help" ]]; then
+    exit 0
+fi
+
+if [[ $# -ne 1 ]]; then
+    echo "Error: Expected at least 1 arg"
+    exit 1
+fi
+
+source install/setup.bash
+
+trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
+
+case $TARGET in
+    "forward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.4
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "backward") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: -0.8
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.0"
+        ;;
+    "left") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: 0.4"
+        ;;
+    "right") 
+        ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
+          header: auto
+          twist:
+            linear:
+              x: 0.0
+              y: 0.0
+              z: 0.0
+            angular:
+              x: 0.0
+              y: 0.0
+              z: -0.4"
+        ;;
+    "zero")
+        ros2 service call /zero std_srvs/srv/Empty
+        ;;
+    *)
+        echo "Target not defined"
+        exit 1
+        ;;
+esac

toid_bot_description/src/toid_bot_control.xacro

@@ -11,25 +11,23 @@
       </xacro:unless>
       <xacro:if value="${use_mock_hardware}">
         <hardware>
-          <plugin>mock_components/GenericSystem</plugin>
-          <param name="calculate_dynamics">true</param>
+          <plugin>mock_components/GenericSystem</plugin> 
+          <param name="calculate_dynamics">true</param> 
         </hardware>
       </xacro:if>
 
       <joint name="drivewhl_l_joint">
         <command_interface name="velocity"/>
         <state_interface name="position"/>
-        <state_interface name="velocity"/>
       </joint>
 
       <joint name="drivewhl_r_joint">
         <command_interface name="velocity"/>
         <state_interface name="position"/>
-        <state_interface name="velocity"/>
       </joint>
 
     </ros2_control>
 
   </xacro:macro>
 
-</robot>
+</robot>