Fully functional sentry (based on ros2_control

decision/dbus_vehicle/src/dbus_interpreter.cpp

@@ -58,16 +58,19 @@ void DbusInterpreter::update()
     {
         shoot_->fric_state = false;
         shoot_->feed_state = false;
+        shoot_->feed_speed = 0;
     }
     else if (rsw == "MID")
     {
         shoot_->fric_state = true;
         shoot_->feed_state = false;
+        shoot_->feed_speed = 0;
     }
     else if (rsw == "DOWN")
     {
         shoot_->fric_state = true;
         shoot_->feed_state = true;
+        shoot_->feed_speed = -4.0;
     }
 }
 

decision/dbus_vehicle/src/dbus_vehicle.cpp

@@ -16,6 +16,7 @@ class DbusVehicle : public rclcpp::Node
         double aim_sens = this->declare_parameter("control.stick_sens", 1.57);
         double deadzone = this->declare_parameter("control.deadzone", 0.05);
         std::string aim_topic = this->declare_parameter("aim_topic", "aim");
+        std::string shoot_topic = this->declare_parameter("shoot_topic", "shoot");
         RCLCPP_INFO(this->get_logger(), "max_vel: %f, max_omega: %f, aim_sens: %f, deadzone: %f",
             max_vel, max_omega, aim_sens, deadzone);
         enable_ros2_control_ = this->declare_parameter("enable_ros2_control", false);
@@ -29,7 +30,7 @@ class DbusVehicle : public rclcpp::Node
         } else {
             move_pub_ = this->create_publisher<behavior_interface::msg::Move>("move", 10);
         }
-        shoot_pub_ = this->create_publisher<behavior_interface::msg::Shoot>("shoot", 10);
+        shoot_pub_ = this->create_publisher<behavior_interface::msg::Shoot>(shoot_topic, 10);
         aim_pub_ = this->create_publisher<behavior_interface::msg::Aim>(aim_topic, 10);
         dbus_sub_ = this->create_subscription<operation_interface::msg::DbusControl>(
             "dbus_control", 10,

decomposition/meta_chassis_controller/CMakeLists.txt

@@ -5,71 +5,34 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow)
 endif()
 
-# find dependencies
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  angles
-  control_toolbox
-  control_msgs
-  controller_interface
-  hardware_interface
-  pluginlib
-  rclcpp
-  rclcpp_lifecycle
-  realtime_tools
-  std_srvs
-  Eigen3
-)
-
 find_package(ament_cmake REQUIRED)
-find_package(generate_parameter_library REQUIRED)
-foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  find_package(${Dependency} REQUIRED)
-endforeach()
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-# Add omni_chassis_controller library related compile commands
 generate_parameter_library(omni_chassis_controller_parameters
   src/omni_chassis_controller.yaml
-  include/meta_chassis_controller/validate_omni_chassis_controller_parameters.hpp
 )
-add_library(
-  meta_chassis_controller
+generate_parameter_library(agv_chassis_controller_parameters
+  src/agv_chassis_controller.yaml
+)
+ament_auto_add_library(
+  ${PROJECT_NAME}
   SHARED
   src/omni_chassis_controller.cpp
   src/omni_wheel_kinematics.cpp
 )
-target_include_directories(meta_chassis_controller PUBLIC
+target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
-target_link_libraries(meta_chassis_controller omni_chassis_controller_parameters)
-ament_target_dependencies(meta_chassis_controller ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(meta_chassis_controller
+  omni_chassis_controller_parameters
+  agv_chassis_controller_parameters)
 
 pluginlib_export_plugin_description_file(
   controller_interface meta_chassis_controller.xml)
 
-install(
-  TARGETS
-  meta_chassis_controller
-  RUNTIME DESTINATION bin
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-)
-
-install(
-  DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
-)
-
 if(BUILD_TESTING)
-endif()
 
-ament_export_include_directories(
-  include
-)
-ament_export_dependencies(
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
-)
-ament_export_libraries(
-  omni_chassis_controller
-)
+endif()
 
-ament_package()
+ament_auto_package()

decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp

@@ -5,10 +5,10 @@
 #include <string>
 #include <vector>
 
+#include "behavior_interface/msg/chassis.hpp"
 #include "control_toolbox/pid_ros.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "meta_chassis_controller/omni_wheel_kinematics.hpp"
-#include "meta_chassis_controller/visibility_control.h"
 #include "omni_chassis_controller_parameters.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
@@ -28,40 +28,29 @@ enum class control_mode_type : std::uint8_t {
     CHASSIS_FOLLOW_GIMBAL = 2,
 };
 
-class OmniChassisController
-    : public controller_interface::ChainableControllerInterface {
+class OmniChassisController : public controller_interface::ChainableControllerInterface {
   public:
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     OmniChassisController();
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::CallbackReturn on_init() override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::InterfaceConfiguration
     command_interface_configuration() const override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::InterfaceConfiguration
     state_interface_configuration() const override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::CallbackReturn
     on_configure(const rclcpp_lifecycle::State &previous_state) override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::CallbackReturn
     on_activate(const rclcpp_lifecycle::State &previous_state) override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::CallbackReturn
     on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
-    controller_interface::return_type
-    update_reference_from_subscribers() override;
+    controller_interface::return_type update_reference_from_subscribers() override;
 
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
     controller_interface::return_type
     update_and_write_commands(const rclcpp::Time &time,
                               const rclcpp::Duration &period) override;
@@ -70,18 +59,19 @@ class OmniChassisController
     using ControllerReferenceMsgUnstamped = geometry_msgs::msg::Twist;
     using ControllerStateMsg = control_msgs::msg::JointControllerState;
 
-  protected:
+  private:
     std::shared_ptr<omni_chassis_controller::ParamListener> param_listener_;
     omni_chassis_controller::Params params_;
 
     std::shared_ptr<control_toolbox::PidROS> follow_pid_;
 
-    // Command subscribers and Controller State publisher
     rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0);
-    rclcpp::Subscription<ControllerReferenceMsgUnstamped>::SharedPtr
-        ref_subscriber_ = nullptr;
-    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>>
-        input_ref_;
+    rclcpp::Subscription<ControllerReferenceMsgUnstamped>::SharedPtr twist_sub_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> ref_buf_;
+
+    rclcpp::Subscription<behavior_interface::msg::Chassis>::SharedPtr chassis_sub_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<behavior_interface::msg::Chassis>>
+        chassis_buf_;
 
     using ControllerStatePublisher =
         realtime_tools::RealtimePublisher<ControllerStateMsg>;
@@ -98,11 +88,15 @@ class OmniChassisController
 
     bool on_set_chained_mode(bool chained_mode) override;
 
-  private:
-    // callback for topic interface
-    METAV_CHASSIS_CONTROLLER__VISIBILITY_LOCAL
-    void reference_callback(
-        const std::shared_ptr<ControllerReferenceMsgUnstamped> msg);
+    // Callbacks
+    void reference_callback(ControllerReferenceMsgUnstamped::UniquePtr msg);
+
+    void chassis_follow_mode(Eigen::Vector3d &twist, const rclcpp::Time &time,
+                             const rclcpp::Duration &period);
+    void gimbal_mode(Eigen::Vector3d &twist);
+    void gyro_mode(Eigen::Vector3d &twist);
+
+    void transform_twist_to_gimbal(Eigen::Vector3d &twist, const double &yaw_gimbal_joint_pos) const;
 };
 
 } // namespace meta_chassis_controller

decomposition/meta_chassis_controller/package.xml

@@ -13,6 +13,8 @@
 
   <depend>angles</depend>
 
+  <depend>behavior_interface</depend>
+
   <depend>control_toolbox</depend>
 
   <depend>control_msgs</depend>

decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml

@@ -0,0 +1,48 @@
+agv_chassis_controller:
+  agv_wheel_joints:
+    {
+      type: string_array,
+      default_value: [],
+      description: "Specifies joints of the AGV wheels.",
+      read_only: true,
+      validation: { unique<>: null, not_empty<>: null },
+    }
+  agv_wheel_center_x:
+    {
+      type: double_array,
+      default_value: [],
+      description: "Specifies the x coordinate of the AGV wheel center.",
+      read_only: true,
+      validation: { not_empty<>: null },
+    }
+  agv_wheel_center_y:
+    {
+      type: double_array,
+      default_value: [],
+      description: "Specifies the y coordinate of the AGV wheel center.",
+      read_only: true,
+      validation: { not_empty<>: null },
+    }
+  agv_wheel_radius:
+    { type: double, default_value: 0.0, description: "Specifies radius of the AGV wheels." }
+  control_mode:
+    {
+      type: int,
+      default_value: 0,
+      description: "Specifies control mode of the AGV wheel controller. 0: CHASSIS, 1: GIMBAL, 2: CHASSIS_FOLLOW_GIMBAL.",
+    }
+  yaw_gimbal_joint:
+    {
+      type: string,
+      default_value: "",
+      description: "Specifies the joint of the yaw gimbal.",
+      validation: { not_empty<>: null },
+    }
+  follow_pid_target:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Specifies target of the follow PID controller. Setting 0.0 meaning the car is always moving towards the front. Usually 0.7853981634(45 degree) gives the maximum speed.",
+    }
+  reference_timeout:
+    { type: double, default_value: 0.1, description: "Specifies timeout for the reference." }