Refactoring, removing unnecessary code for ros2 control

RoboJackets · May 28, 2024 · 78d2b10 · 78d2b10
1 parent f376bf0
commit 78d2b10
Show file tree

Hide file tree

Showing 5 changed files with 72 additions and 76 deletions.
diff --git a/urc_bringup/config/controller_config.yaml b/urc_bringup/config/controller_config.yaml
@@ -2,42 +2,34 @@ controller_manager:
   ros__parameters:
     update_rate: 200
 
-    imu_broadcaster:
-      type: urc_controllers/IMUBroadcaster
-
-    status_light_controller:
-      type: urc_controllers/StatusLightController
-
     rover_drivetrain_controller:
       type: diff_drive_controller/DiffDriveController
 
-    bms_broadcaster:
-      type: urc_controllers/BMSBroadcaster
+    status_light_controller:
+      type: urc_controllers/StatusLightController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    arm_controller:
-      type: velocity_controllers/JointGroupVelocityController
+    # imu_broadcaster:
+    #   type: urc_controllers/IMUBroadcaster
+
+    # bms_broadcaster:
+    #   type: urc_controllers/BMSBroadcaster
+
+    # arm_controller:
+    #   type: velocity_controllers/JointGroupVelocityController
 
-    gripper_controller_left:
-      type: position_controllers/GripperActionController
+    # gripper_controller_left:
+    #   type: position_controllers/GripperActionController
 
-    gripper_controller_right:
-      type: position_controllers/GripperActionController
+    # gripper_controller_right:
+    #   type: position_controllers/GripperActionController
 
 joint_state_broadcaster:
   ros__parameters:
     type: joint_state_broadcaster/JointStateBroadcaster
 
-imu_broadcaster:
-  ros__parameters:
-    imu_name: imu_sensor
-
-bms_broadcaster:
-  ros__parameters:
-    bms_name: battery_management
-    cell_num: 6
 
 status_light_controller:
   ros__parameters:
@@ -91,34 +83,6 @@ rover_drivetrain_controller:
     angular.z.max_jerk: 0.0
     angular.z.min_jerk: 0.0
 
-arm_controller:
-  ros__parameters:
-    joints:
-      - shoulderjoint
-      - bicepjoint
-      - elbowjoint
-      - wristjoint
-      - clawjoint
-
-gripper_controller_left:
-  ros__parameters:
-    action_monitor_rate: 20.0
-    allow_stalling: false
-    goal_tolerance: 0.01
-    max_effort: 0.50
-    stall_timeout: 1.0
-    stall_velocity_threshold: 0.001
-    joint: leftgripperjoint
-
-gripper_controller_right:
-  ros__parameters:
-    action_monitor_rate: 20.0
-    allow_stalling: false
-    goal_tolerance: 0.01
-    max_effort: 0.50
-    stall_timeout: 1.0
-    stall_velocity_threshold: 0.001
-    joint: rightgripperjoint
 
 joy_drive:
   ros__parameters:
@@ -129,3 +93,41 @@ joy_drive:
     target_axes: [1, 3]
     invert_linear_velocity: false
     invert_angular_velocity: false
+
+# imu_broadcaster:
+#   ros__parameters:
+#     imu_name: imu_sensor
+
+# bms_broadcaster:
+#   ros__parameters:
+#     bms_name: battery_management
+#     cell_num: 6
+
+# arm_controller:
+#   ros__parameters:
+#     joints:
+#       - shoulderjoint
+#       - bicepjoint
+#       - elbowjoint
+#       - wristjoint
+#       - clawjoint
+
+# gripper_controller_left:
+#   ros__parameters:
+#     action_monitor_rate: 20.0
+#     allow_stalling: false
+#     goal_tolerance: 0.01
+#     max_effort: 0.50
+#     stall_timeout: 1.0
+#     stall_velocity_threshold: 0.001
+#     joint: leftgripperjoint
+
+# gripper_controller_right:
+#   ros__parameters:
+#     action_monitor_rate: 20.0
+#     allow_stalling: false
+#     goal_tolerance: 0.01
+#     max_effort: 0.50
+#     stall_timeout: 1.0
+#     stall_velocity_threshold: 0.001
+#     joint: rightgripperjoint
diff --git a/urc_controllers/src/status_light_controller.cpp b/urc_controllers/src/status_light_controller.cpp
@@ -27,6 +27,9 @@ StatusLightController::StatusLightController()
 
 controller_interface::CallbackReturn StatusLightController::on_init()
 {
+  // RCLCPP_ERROR(
+  //     get_node()->get_logger(),
+  //     "STATUS STATUS STATUS STATUS STATUS STATUS STATUS STATUS STATUS STATUS STATUS STATUS");
   return controller_interface::CallbackReturn::SUCCESS;
 }
 

diff --git a/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp b/urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
@@ -112,9 +112,9 @@ hardware_interface::return_type RoverDrivetrain::read(
   const rclcpp::Time &,
   const rclcpp::Duration &)
 {
-  RCLCPP_INFO(
-    rclcpp::get_logger(
-      "test"), "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
+  // RCLCPP_INFO(
+  //   rclcpp::get_logger(
+  //     "Drivetrain Command:"), "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
 
   return hardware_interface::return_type::OK;
 }

diff --git a/urc_hw/src/hardware_interfaces/status_light.cpp b/urc_hw/src/hardware_interfaces/status_light.cpp
@@ -125,6 +125,10 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r
   message.display = signals[1];
   message.has_display = true;
 
+  // RCLCPP_INFO(
+  //   rclcpp::get_logger(
+  //     "Status Light Command:"), "%.5f, %.5f", signals[0], signals[1]);
+
   bool status = pb_encode(&stream, StatusLightCommand_fields, &message);
   message_length = stream.bytes_written;
 

diff --git a/urc_hw_description/urdf/ros2_control.xacro b/urc_hw_description/urdf/ros2_control.xacro
@@ -149,30 +149,28 @@
           <state_interface name="velocity" />
         </joint>
       </ros2_control>
-    </xacro:if>
-
-    <xacro:if value="${use_simulation}">
       <gazebo>
         <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
           <parameters>${config_file_path}</parameters>
         </plugin>
       </gazebo>
     </xacro:if>
 
+
     <xacro:if value="${not use_simulation}">
-      <ros2_control name="walli_others" type="system">
+      <ros2_control name="status_light" type="system">
         <hardware>
           <plugin>urc_hw/StatusLight</plugin>
           <param name="serial_port">/dev/ttyACM0</param>
-          <param name="udp_address">143.215.63.147</param>
-          <param name="udp_port">5000</param>
+          <param name="udp_address">192.168.1.113</param>
+          <param name="udp_port">8443</param>
         </hardware>
-
         <gpio name="status_light">
           <command_interface name="color" />
           <command_interface name="display" />
         </gpio>
       </ros2_control>
+
       <ros2_control name="rover_drivetrain" type="system">
         <hardware>
           <plugin>urc_hw/RoverDrivetrain</plugin>
@@ -194,19 +192,8 @@
           <state_interface name="velocity.back" />
         </joint>
       </ros2_control>
-      <ros2_control name="status_light" type="system">
-        <hardware>
-          <plugin>urc_hw/StatusLight</plugin>
-          <param name="serial_port">/dev/ttyACM0</param>
-          <param name="udp_address">143.215.63.147</param>
-          <param name="udp_port">5000</param>
-        </hardware>
-        <gpio name="status_light">
-          <command_interface name="color" />
-          <command_interface name="display" />
-        </gpio>
-      </ros2_control>
-      <ros2_control name="battery_management" type="sensor">
+
+      <!-- <ros2_control name="battery_management" type="sensor">
         <hardware>
           <plugin>urc_hw/BatteryManagement</plugin>
           <param name="udp_address">127.0.0.1</param>
@@ -228,8 +215,8 @@
           <state_interface name="cell7_voltage" />
           <state_interface name="cell8_voltage" />
         </sensor>
-      </ros2_control>
-      <ros2_control name="imu" type="sensor">
+      </ros2_control> -->
+      <!-- <ros2_control name="imu" type="sensor">
         <hardware>
           <plugin>urc_hw/IMU</plugin>
           <param name="udp_address">127.0.0.1</param>
@@ -249,7 +236,7 @@
           <state_interface name="angular_velocity.y" />
           <state_interface name="angular_velocity.z" />
         </sensor>
-      </ros2_control>
+      </ros2_control> -->
     </xacro:if>
   </xacro:macro>