Low-level controller

smb/launch/smb.launch

@@ -34,11 +34,6 @@
   <arg name="use_lidar_odometry"
        default="false"
        doc="Use lidar odometry instead of the tracking camera odometry if set to true."/>
-
-  <!-- enable ekf -->
-  <arg name="enable_ekf" 
-       default="false"
-       doc="Launch ekf node. Used for the wheel odometry."/>
 
   <!-- launch rviz -->
   <arg name="launch_rviz"
@@ -112,7 +107,6 @@
 
 
   <include file="$(find smb_control)/launch/smb_control.launch">
-    <arg name="enable_ekf" value="$(arg enable_ekf)" />
     <arg name="description_name" value="$(arg description_name)"/>
     <arg name="control_namespace" value="$(arg control_namespace)"/>
     <arg name="mpc"               value="$(arg mpc)"/>

smb_control/CMakeLists.txt

@@ -2,25 +2,45 @@ cmake_minimum_required(VERSION 3.0.2)
 project(smb_control)
 
 find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
   controller_manager
   joint_state_controller
   robot_state_publisher
   roslaunch
+  roscpp
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+)
+
+generate_dynamic_reconfigure_options(
+  config/PIDConfig.cfg
 )
 
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES smb_control
-#  CATKIN_DEPENDS controller_manager joint_state_controller robot_state_publisher
-#  DEPENDS system_lib
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS dynamic_reconfigure controller_manager joint_state_controller robot_state_publisher roscpp std_msgs geometry_msgs sensor_msgs dynamic_reconfigure
 )
 
 include_directories(
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
+
+add_executable(pid_controller_node src/PIDControllerNode.cpp)
+add_dependencies(pid_controller_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})  # Ensure the node is built after generating dynamic reconfigure headers
+
+target_link_libraries(pid_controller_node
+   ${EXTERNAL_LIBS}
+   ${catkin_LIBRARIES}
+)
+
 roslaunch_add_file_check(launch)
 
-install(DIRECTORY config launch
+install(DIRECTORY config launch include
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
 )
+

smb_control/config/PIDConfig.cfg

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+# Parameter generator handle
+gen = ParameterGenerator()
+
+# Add double parameters for angular velocity control
+# Name | Type | Level | Description | Default | Min | Max
+gen.add("kp_angular", double_t, 0, "Proportional gain for angular velocity", 3.0, 0, 10)
+gen.add("ki_angular", double_t, 0, "Integral gain for angular velocity", 1.0, 0, 5)
+gen.add("kd_angular", double_t, 0, "Derivative gain for angular velocity", 0.0, 0, 1.0)
+
+# Add double parameters for linear velocity control
+gen.add("kp_linear", double_t, 0, "Proportional gain for linear velocity", 1.5, 0, 10)
+gen.add("ki_linear", double_t, 0, "Integral gain for linear velocity", 1.0, 0, 5)
+gen.add("kd_linear", double_t, 0, "Derivative gain for linear velocity", 0.0, 0, 1.0)
+
+# Add boolean parameters for EKF sensor inputs
+# Name | Type | Level | Description | Default
+gen.add("use_imu", bool_t, 0, "Use IMU data", True)
+gen.add("use_differential_drive_estimation", bool_t, 0, "Use Differential Drive Estimation data", True)
+gen.add("use_vio", bool_t, 0, "Use VIO data", False)
+
+# Exit parameter generator
+# Package Name | Node Name (used to generate documentation only) | cfg File Name
+exit(gen.generate("smb_control", "PIDControllerNode", "PID"))

smb_control/include/smb_control/PIDControllerNode.hpp

@@ -0,0 +1,114 @@
+#ifndef PIDCONTROLLERNODE_HPP
+#define PIDCONTROLLERNODE_HPP
+
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <smb_control/PIDConfig.h>
+
+class PIDController {
+public:
+    PIDController(double kp = 0.0, double ki = 0.0, double kd = 0.0, double max_integral = 2.0)
+        : kp_(kp), ki_(ki), kd_(kd), integral_(0.0), prev_error_(0.0), max_integral_(max_integral) {}
+
+    void setGains(double kp, double ki, double kd) {
+        kp_ = kp;
+        ki_ = ki;
+        kd_ = kd;
+    }
+
+    void setMaxIntegral(double max_integral) {
+        max_integral_ = max_integral;
+    }
+
+    double update(double error, double dt) {
+        if (dt <= 0.0001) return 0.0; // Prevent division by zero
+
+        integral_ += error * dt;
+
+        // Clamp the integral value to the maximum limit
+        if (integral_ > max_integral_) integral_ = max_integral_;
+        if (integral_ < -max_integral_) integral_ = -max_integral_;
+
+        double derivative = (error - prev_error_) / dt;
+        prev_error_ = error;
+
+        return kp_ * error + ki_ * integral_ + kd_ * derivative;
+    }
+
+    void reset() {
+        integral_ = 0.0;
+        prev_error_ = 0.0;
+    }
+
+    void resetIfZero(double cmd) {
+        if (std::abs(cmd) < 0.05) { // Check if the command is approximately zero
+            reset();
+        }
+    }
+
+private:
+    double kp_, ki_, kd_;
+    double integral_, prev_error_;
+    double max_integral_;
+};
+
+class PIDControllerNode
+{
+public:
+    // Constructor
+    PIDControllerNode();
+
+    // Destructor
+    ~PIDControllerNode();
+
+    // Main loop function
+    void run();
+
+    void configCallback(smb_control::PIDConfig &config, uint32_t level);
+
+    void ekfConfigCallback(smb_control::PIDConfig &config, uint32_t level);
+
+private:
+    ros::NodeHandle nh_;
+
+    // Publisher
+    ros::Publisher cmd_vel_pub_;
+
+    // Subscriber
+    ros::Subscriber twist_sub_;
+    ros::Subscriber odom_sub_;
+
+    bool pure_feed_forward_{true};
+
+    // Subscriber callback
+    void twistCallback(const geometry_msgs::Twist::ConstPtr& msg);
+    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
+
+    // PID
+    PIDController angular_velocity_pid_;
+    PIDController linear_velocity_pid_;
+    bool use_diff_drive_estimation_{false};
+    bool use_vio_{false};
+    bool use_imu_{false};
+
+    std::shared_ptr<dynamic_reconfigure::Server<smb_control::PIDConfig>> config_server_;
+
+    geometry_msgs::Twist cmd_vel_requested_;
+    geometry_msgs::Twist cmd_vel_;
+
+    ros::Time last_twist_time_;
+    ros::Time last_odom_time_;
+    double dt_{0.01};
+    double odom_timeout_{0.0};
+    double twist_timeout_{0.0};
+
+    void checkTimeout();
+    void executeFeedforwardControl();
+};
+
+#endif // PIDCONTROLLERNODE_HPP

smb_control/launch/smb_control.launch

@@ -3,7 +3,6 @@
   <arg name="robot_namespace"            default=""/>
   <arg name="description_name"           default="smb_description"/>
   <arg name="control_namespace"          default="control"/>
-  <arg name="enable_ekf"                 default="false"/>
   <arg name="lidar"                      default="true"/>
   <arg name="tracking_camera"            default="false"/>
   <arg name="joint_states_topic"         default="joint_states"/>
@@ -51,16 +50,17 @@
 
   <node pkg="twist_mux" type="twist_mux" name="twist_mux" ns="control">
     <rosparam command="load" file="$(find smb_control)/config/twist_mux.yaml" />
-    <remap from="cmd_vel_out" to="/control/smb_diff_drive/cmd_vel"/>
+    <remap from="cmd_vel_out" to="/control/twist_mux/output"/>
   </node>
 
+  <!-- PID Controller Node -->
+  <node name="pid_controller_node" pkg="smb_control" type="pid_controller_node" output="screen"/>
+
 
   <!-- Start EKF for localization -->
-  <group if="$(arg enable_ekf)" >
-    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" ns="$(arg control_namespace)">
-      <rosparam command="load" file="$(find smb_control)/config/localization.yaml" />
-    </node>
-  </group>
+
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" output="screen">
+  </node>
 
   <include file="$(find smb_control)/launch/teleop.launch" if="$(arg joystick)">
     <arg name="joystick_name_space" value="$(arg control_namespace)/joy_teleop"/>
@@ -73,4 +73,6 @@
   <!-- Launch keyboard twist node -->
   <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="keyboard_teleop" ns="control/keyboard_teleop" if="$(arg keyboard_teleop)"/>
 
+  <node pkg="smb_control" type="ekf_restarter.py" name="ekf_restarter" output="screen"/>
+
 </launch>

smb_control/launch/start_ekf_reconfigure.launch

@@ -0,0 +1,7 @@
+
+
+  <launch>
+  <!-- Launch the EKF node -->
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" output="screen"/>
+
+</launch>

smb_control/package.xml

@@ -18,6 +18,8 @@
   <build_export_depend>joint_state_controller</build_export_depend>
   <build_export_depend>robot_state_publisher</build_export_depend>
 
+  <depend>dynamic_reconfigure</depend>
+
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>joint_state_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
@@ -35,6 +37,11 @@
   <exec_depend>teleop_twist_keyboard</exec_depend>
   <exec_depend>twist_mux</exec_depend>
 
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+
 
   <export>
   </export>