[ros2 control] exposed status light interface, allowed for firmware to parse both drivetrain and status light commands

urc_bringup/launch/bringup.launch.py

@@ -42,7 +42,8 @@ def generate_launch_description():
         xacro_file, mappings={"use_simulation": "false"}
     )
     robot_desc = robot_description_config.toxml()
-    gps_config = os.path.join(get_package_share_directory("urc_bringup"), "config", "nmea_serial_driver.yaml")
+    gps_config = os.path.join(get_package_share_directory("urc_bringup"),
+                              "config", "nmea_serial_driver.yaml")
 
     control_node = Node(
         package="controller_manager",
@@ -68,29 +69,35 @@ def generate_launch_description():
         arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"],
     )
 
-    load_arm_controller = Node(
+    load_drivetrain_controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["-p", controller_config_file_dir, "arm_controller"],
+        arguments=["rover_drivetrain_controller"],
     )
 
-    load_gripper_controller_left = Node(
+    load_status_light_controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["-p", controller_config_file_dir, "gripper_controller_left"],
+        arguments=["-p", controller_config_file_dir, "status_light_controller"],
     )
 
-    load_gripper_controller_right = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["-p", controller_config_file_dir, "gripper_controller_right"],
-    )
+    # load_arm_controller = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["-p", controller_config_file_dir, "arm_controller"],
+    # )
 
-    load_drivetrain_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["rover_drivetrain_controller"],
-    )
+    # load_gripper_controller_left = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["-p", controller_config_file_dir, "gripper_controller_left"],
+    # )
+
+    # load_gripper_controller_right = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["-p", controller_config_file_dir, "gripper_controller_right"],
+    # )
 
     teleop_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -145,9 +152,10 @@ def generate_launch_description():
             load_robot_state_publisher,
             load_joint_state_broadcaster,
             load_drivetrain_controller,
-            load_arm_controller,
-            load_gripper_controller_left,
-            load_gripper_controller_right,
+            load_status_light_controller,
+            # load_arm_controller,
+            # load_gripper_controller_left,
+            # load_gripper_controller_right,
             teleop_launch,
             launch_gps,
             launch_imu,

urc_bringup/launch/bringup_simulation.launch.py

@@ -25,12 +25,12 @@ def generate_launch_description():
     controller_config_file_dir = os.path.join(
         pkg_urc_bringup, "config", "ros2_control_walli.yaml"
     )
-    world_path = os.path.join(pkg_urc_gazebo, "urdf/worlds/urc_world.world")
+    # world_path = os.path.join(pkg_urc_gazebo, "urdf/worlds/urc_world.world")
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
     xacro_file = os.path.join(
         get_package_share_directory("urc_hw_description"),
-        "urdf/walli_sim.xacro"
+        "urdf/walli.xacro"
     )
     assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file)
     robot_description_config = process_file(

urc_controllers/CMakeLists.txt

@@ -53,7 +53,7 @@ ament_target_dependencies(
 )
 
 target_compile_definitions(urc_controllers PRIVATE "urc_hw_BUILDING_LIBRARY")
-pluginlib_export_plugin_description_file(controller_interface test_hardware_controller.xml)
+pluginlib_export_plugin_description_file(controller_interface urc_controllers.xml)
 
 install(
   DIRECTORY include/

urc_controllers/test_hardware_controller.xml → urc_controllers/urc_controllers.xml

@@ -1,28 +1,17 @@
 <library path="urc_controllers">
     <class name="urc_controllers/IMUBroadcaster"
         type="urc_controllers::IMUBroadcaster"
-        base_class_type="controller_interface::ControllerInterface"
-    >
-        <description>
-            Imu broadcaster.
-        </description>
+        base_class_type="controller_interface::ControllerInterface">
+        <description>Imu broadcaster.</description>
     </class>
-
     <class name="urc_controllers/BMSBroadcaster"
         type="urc_controllers::BMSBroadcaster"
-        base_class_type="controller_interface::ControllerInterface"
-    >
-        <description>
-            BMS broadcaster.
-        </description>
+        base_class_type="controller_interface::ControllerInterface">
+        <description>BMS broadcaster.</description>
     </class>
-
     <class name="urc_controllers/StatusLightController"
         type="urc_controllers::StatusLightController"
-        base_class_type="controller_interface::ControllerInterface"
-    >
-        <description>
-            Status light controller.
-        </description>
+        base_class_type="controller_interface::ControllerInterface">
+        <description>Status light controller.</description>
     </class>
 </library>

urc_hw/include/urc_hw/hardware_interfaces/rover_drivetrain.hpp

@@ -74,7 +74,7 @@ class RoverDrivetrain : public hardware_interface::SystemInterface
   std::string udp_port;
 
   // nanopb
-  uint8_t buffer[DriveEncodersMessage_size];
+  uint8_t buffer[TeensyMessage_size];
   size_t message_length;
 };
 

urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp

@@ -54,8 +54,12 @@ class StatusLight : public hardware_interface::SystemInterface
   std::string udp_port;
 
   // nanopb
-  uint8_t buffer[StatusLightCommand_size];
+  uint8_t buffer[TeensyMessage_size];
   size_t message_length;
+
+  // private info for lights
+  uint8_t currentLight = 0;
+  uint8_t lightModes[3]; // current pattern for each of 3 lights
 };
 
 }  // namespace urc_hardware::hardware_interfaces

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("test"),
+  //   "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);
 
   return hardware_interface::return_type::OK;
 }
@@ -127,16 +127,30 @@ hardware_interface::return_type RoverDrivetrain::write(
     return hardware_interface::return_type::OK;
   }
 
-  DriveEncodersMessage message = DriveEncodersMessage_init_zero;
+  TeensyMessage message = TeensyMessage_init_zero;
+  // DrivetrainRequest drivetrainRequest = DrivetrainRequest_init_zero;
   pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
 
-  message.leftSpeed = velocity_rps_commands[0] * ENCODER_CPR * -1;
-  message.has_leftSpeed = true;
-  message.rightSpeed = velocity_rps_commands[1] * ENCODER_CPR * -1;
-  message.has_rightSpeed = true;
-  message.timestamp = time.nanoseconds();
+  message.m1Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+  message.m2Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+  message.m3Setpoint = velocity_rps_commands[0] * ENCODER_CPR * -1;
+
+  message.m4Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+  message.m5Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+  message.m6Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
+
+
+  // Fill Required Fields
+  message.redEnabled = 0;
+  message.blueEnabled = 0;
+  message.greenEnabled = 0;
+  message.redBlink = 0;
+  message.blueBlink = 0;
+  message.greenBlink = 0;
+
+  message.messageID = 0;
 
-  bool status = pb_encode(&stream, DriveEncodersMessage_fields, &message);
+  bool status = pb_encode(&stream, TeensyMessage_fields, &message);
   message_length = stream.bytes_written;
 
   if (!status) {

urc_hw/src/hardware_interfaces/status_light.cpp

@@ -117,15 +117,33 @@ hardware_interface::return_type StatusLight::read(const rclcpp::Time &, const rc
 
 hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const rclcpp::Duration &)
 {
-  StatusLightCommand message = StatusLightCommand_init_zero;
+
+  TeensyMessage message = TeensyMessage_init_zero;
   pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
 
-  message.color = signals[0];
-  message.has_color = true;
-  message.display = signals[1];
-  message.has_display = true;
+  currentLight = signals[0];
+  if (currentLight >= 0 && currentLight < 3 && signals[1] >= 0 && signals[1] < 3) {
+    lightModes[currentLight] = signals[1];
+  }
+
+  message.redEnabled = (lightModes[0] != 0);
+  message.redBlink = (lightModes[0] == 2);
+  message.blueEnabled = (lightModes[1] != 0);
+  message.blueBlink = (lightModes[1] == 2);
+  message.greenEnabled = (lightModes[2] != 0);
+  message.greenBlink = (lightModes[2] == 2);
+
+  // Fill Required Fields
+  message.m1Setpoint = 0;
+  message.m2Setpoint = 0;
+  message.m3Setpoint = 0;
+  message.m4Setpoint = 0;
+  message.m5Setpoint = 0;
+  message.m6Setpoint = 0;
+
+  message.messageID = 1;
 
-  bool status = pb_encode(&stream, StatusLightCommand_fields, &message);
+  bool status = pb_encode(&stream, TeensyMessage_fields, &message);
   message_length = stream.bytes_written;
 
   if (!status) {

urc_hw_description/urdf/ros2_control.xacro

@@ -10,27 +10,22 @@
         <hardware>
           <plugin>gazebo_ros2_control/GazeboSystem</plugin>
         </hardware>
-
         <joint name="leftbogiejoint">
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="leftrockerjoint">
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="rightbogiejoint">
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="rightrockerjoint">
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="left_front_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -39,7 +34,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="right_front_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -48,7 +42,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="left_center_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -57,7 +50,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="right_center_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -66,7 +58,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="left_rear_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -75,7 +66,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="right_rear_wheel_joint">
           <command_interface name="velocity">
             <param name="min">-0.05</param>
@@ -84,7 +74,6 @@
           <state_interface name="velocity" />
           <state_interface name="position" />
         </joint>
-
         <joint name="shoulderjoint">
           <command_interface name="position" />
           <command_interface name="velocity" />
@@ -149,9 +138,6 @@
           <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>
@@ -160,19 +146,6 @@
     </xacro:if>
 
     <xacro:if value="${not use_simulation}">
-      <ros2_control name="walli_others" 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="rover_drivetrain" type="system">
         <hardware>
           <plugin>urc_hw/RoverDrivetrain</plugin>
@@ -197,9 +170,8 @@
       <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" />
@@ -252,13 +224,4 @@
       </ros2_control>
     </xacro:if>
   </xacro:macro>
-
-  <!-- Gazebo Configuration -->
-
-
 </robot>
-
-
-
-
-

urc_hw_description/urdf/walli.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot name="walli" xmlns:xacro="http://ros.org/wiki/xacro">
-   <xacro:arg name="use_simulation" default="false" />
+   <xacro:arg name="use_simulation" default="true" />
 
    <xacro:include filename="$(find urc_hw_description)/urdf/walli_prop.xacro" />
    <xacro:include filename="$(find urc_hw_description)/urdf/walli_inertials.xacro" />
@@ -16,4 +16,4 @@
    <xacro:ros2_control use_simulation="$(arg use_simulation)"
       config_file_path="$(find urc_bringup)/config/controller_config.yaml"
       arm_initial_positions_file="$(find urc_hw_description)/config/initial_positions.yaml" />
-</robot>
+</robot>