Hardware Interface & ROS2 Control (#149)

* feat: implementation in progress * feat: finish urc_hw, adding urc_description * feat: simple controller and robot description * feat: working example of hardware interface (no eth) * fix: urc control bringup error partially resolved * fix building * Create common_issues.md Signed-off-by: David Calderon <[email protected]> * fix: error during configuration * fix: crappy minimal working version * fix: minor updates * feat: async udp, working example * fix: add parameter choice for udp address and port * Added udp tester * Changed packet size to 1024 instead of 4096 bytes * feat: refactor & start writing imu * feat: add status light and imu * fix: imu and status light bug, add readme * fix: include bug * feat: nanopb and implementation in status light * Added small package.xml modification to allow everything to build * Added rover hardware interface information from other branch * refactor: seperate to imu & status light * refactor: reduce info logging & change cmd name * fix: update udp_test, working nanopb * fix: usable nanopb for status light, test imu * feat: battery management * Started code for rover hardware interface * Finished rough version of drivetrain hardware interface * Corrected some issues with rover drivetrai * Added science module hardware interface * feat: bms broadcaster * Ported urdf to hardware description * other changes * feat: diff drive controller * fix: minor updates * fix: udp test update (verified working version) * uncrustify 1 * uncrustify 2 * uncrustify 3 * redid python file formatting * auto format * manual linting * more manual linting * linting mk4 * cmake linting * added ros2 control xacro * added ros2 control xacro --------- Signed-off-by: David Calderon <[email protected]> Co-authored-by: Harris Nesteruk <[email protected]> Co-authored-by: David Calderon <[email protected]>
RoboJackets · Nov 17, 2023 · b56bafa · b56bafa
1 parent 6c650d2
commit b56bafa
Show file tree

Hide file tree

Showing 60 changed files with 6,140 additions and 1 deletion.
diff --git a/documents/helpers/common_issues.md b/documents/helpers/common_issues.md
@@ -0,0 +1,5 @@
+# Common Troubleshooting Issues
+This guide is to help members quickly resolve issues with working with the codebase that have been encountered in the past.
+
+* Could not find a package configuration file provided by "ament_cmake_core" or any other ament files...
+For this, the package that is failing may not have listed another package as a dependency in the package.xml. Try adding that dependency and rebuilding so that CMake knows to build the other package first.
diff --git a/tools/README.md b/tools/README.md
@@ -0,0 +1,3 @@
+# URC Dev Tools
+
+This is the folder for all the small tools used for testing purposes.
diff --git a/tools/udp/udp_test.py b/tools/udp/udp_test.py
@@ -0,0 +1,23 @@
+import socket
+import tools.udp.urc_pb2 as urc_pb2
+
+# Define the server's IP address and port
+server_ip = "127.0.0.1"  # Replace with your server's IP address
+server_port = 5000  # Replace with the desired port number
+
+# Create a UDP socket
+server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# Bind the socket to the server's address and port
+server_socket.bind((server_ip, server_port))
+
+print(f"UDP server is listening on {server_ip}:{server_port}")
+
+while True:
+    # Receive data from a client
+    data, client_address = server_socket.recvfrom(128)
+
+    msg = urc_pb2.DriveEncodersMessage()
+    msg.ParseFromString(data)
+
+    print(msg.leftSpeed, "  ", msg.rightSpeed)
diff --git a/tools/udp/urc_pb2.py b/tools/udp/urc_pb2.py
diff --git a/urc_arm_moveit_config/CMakeLists.txt b/urc_arm_moveit_config/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required(VERSION 3.22)
 project(urc_arm_moveit_config)
 
+
 find_package(ament_cmake REQUIRED)
 
 ament_package()

diff --git a/urc_control/CMakeLists.txt b/urc_control/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.5)
+project(urc_control)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Disable boost/shared_ptr in pluginlib while building on rpi4
+add_compile_definitions(PLUGINLIB__DISABLE_BOOST_FUNCTIONS)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(controller_manager REQUIRED)
+
+add_executable(control_node
+  src/control_node.cpp
+)
+ament_target_dependencies(control_node
+  rclcpp
+  controller_manager
+  controller_manager_msgs
+)
+
+install(TARGETS
+  control_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY
+    launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/urc_control/README.md b/urc_control/README.md
@@ -0,0 +1,18 @@
+# URC Control
+
+This is the central bring-up package for the whole ros2_control system used in URC. The related packages are:
+
+- **urc_hw**: contains hardware resource managers, hardware interfaces, and utilities.
+- **urc_hw_description**: provides .urdf and .xacro files that describes the hardware on the rover.
+- **urc_controllers**: contains all the controllers for communicating with the hardware interfaces.
+- **urc_control**: contains a node to start the control manager, initialize all the hardware interfaces, and finally start the publishers & subscribers for receiving and sending control signals.
+
+## Usage
+
+Simply type in command line:
+
+```
+ros2 launch urc_control bringup.launch.py
+```
+
+Everything will go live.
diff --git a/urc_control/launch/bringup.launch.py b/urc_control/launch/bringup.launch.py
@@ -0,0 +1,56 @@
+import os
+import xacro
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+# from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Get the launch configuration
+    # use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    # Get the robot description
+    urdf_path = os.path.join(get_package_share_directory(
+        'urc_hw_description'), 'urdf', 'wallii.urdf')
+    urdf_doc = xacro.parse(open(urdf_path, 'r'))
+    xacro.process_doc(urdf_doc)
+    robot_description = urdf_doc.toxml()
+
+    robot_controller_config = os.path.join(get_package_share_directory(
+        'urc_hw_description'),
+        'config', 'test_hardware_controller_config.yaml')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_sim_time',
+                             default_value='false',
+                             description='Use simulation (Gazebo) clock'
+                             + 'if true'
+                              ),
+
+        # IncludeLaunchDescription(
+        #     PythonLaunchDescriptionSource(
+        #         [ThisLaunchFileDir(), '/mecanumbot_state_publisher.py']
+        #     ),
+        #     launch_arguments={
+        #         'use_sim_time': use_sim_time
+        #     }.items()
+        # ),
+        # IncludeLaunchDescription(
+        #     PythonLaunchDescriptionSource(
+        #         [ThisLaunchFileDir(), '/mecanumbot_teleop.py']
+        #     )
+        # ),
+
+        Node(
+            package='urc_control',
+            executable='control_node',
+            output='screen',
+            parameters=[
+                {'robot_description': robot_description},
+                robot_controller_config
+            ]
+        )
+    ])
diff --git a/urc_control/launch/udp_send.py b/urc_control/launch/udp_send.py
@@ -0,0 +1,20 @@
+
+# Import socket module
+import socket
+
+# Create a UDP socket
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# Define the server address and port
+server_address = ('192.168.1.168', 8443)
+
+# Send a message to the server
+message = b'Hello, this is a UDP message'
+sock.sendto(message, server_address)
+
+# Receive a response from the server
+data, address = sock.recvfrom(4096)
+print(f'Received {data} from {address}')
+
+# Close the socket
+sock.close()
diff --git a/urc_control/package.xml b/urc_control/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>urc_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">keseterg</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>controller_manager</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/urc_control/src/control_node.cpp b/urc_control/src/control_node.cpp
@@ -0,0 +1,98 @@
+#include <rclcpp/executors.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <controller_manager/controller_manager.hpp>
+#include "controller_interface/controller_interface.hpp"
+#include <realtime_tools/thread_priority.hpp>
+
+int const kSchedPriority = 50;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  // Setup the controller manager node
+  std::string controller_manager_node_name = "controller_manager";
+  std::shared_ptr<rclcpp::Executor> executor =
+    std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+
+  auto controller_manager_node =
+    std::make_shared<controller_manager::ControllerManager>(executor, controller_manager_node_name);
+
+  std::thread cm_thread([controller_manager_node]() {
+      if (realtime_tools::has_realtime_kernel()) {
+        if (!realtime_tools::configure_sched_fifo(kSchedPriority)) {
+          RCLCPP_WARN(
+            controller_manager_node->get_logger(),
+            "Could not enable FIFO RT scheduling policy");
+        }
+      } else {
+        RCLCPP_INFO(
+          controller_manager_node->get_logger(),
+          "RT kernel is recommended for better performance");
+      }
+
+      // for calculating sleep time
+      auto const period =
+      std::chrono::nanoseconds(1'000'000'000 / controller_manager_node->get_update_rate());
+      auto const cm_now = std::chrono::nanoseconds(controller_manager_node->now().nanoseconds());
+      std::chrono::time_point<std::chrono::system_clock,
+      std::chrono::nanoseconds> next_iteration_time{cm_now};
+
+      // for calculating the measured period of the loop
+      rclcpp::Time previous_time = controller_manager_node->now();
+
+      while (rclcpp::ok()) {
+        // calculate measured period
+        auto const current_time = controller_manager_node->now();
+        auto const measured_period = current_time - previous_time;
+        previous_time = current_time;
+
+        controller_manager_node->read(controller_manager_node->now(), measured_period);
+        controller_manager_node->update(controller_manager_node->now(), measured_period);
+        controller_manager_node->write(controller_manager_node->now(), measured_period);
+
+        next_iteration_time += period;
+        std::this_thread::sleep_until(next_iteration_time);
+      }
+    });
+
+  // Load the controllers
+  std::vector<std::string> start_controllers;
+  std::vector<std::string> stop_controllers;
+  RCLCPP_INFO(
+    controller_manager_node->get_logger(), "Update rate is %d Hz",
+    controller_manager_node->get_update_rate());
+  RCLCPP_INFO(controller_manager_node->get_logger(), "Loading controllers...");
+  controller_manager_node->load_controller("imu_broadcaster", "urc_controllers/IMUBroadcaster");
+  controller_manager_node->load_controller("bms_broadcaster", "urc_controllers/BMSBroadcaster");
+  controller_manager_node->load_controller(
+    "status_light_controller",
+    "urc_controllers/StatusLightController");
+  controller_manager_node->load_controller(
+    "rover_drivetrain_controller",
+    "diff_drive_controller/DiffDriveController");
+  controller_manager_node->configure_controller("imu_broadcaster");
+  controller_manager_node->configure_controller("rover_drivetrain_controller");
+  controller_manager_node->configure_controller("bms_broadcaster");
+  controller_manager_node->configure_controller("status_light_controller");
+
+  start_controllers.push_back("imu_broadcaster");
+  start_controllers.push_back("bms_broadcaster");
+  start_controllers.push_back("status_light_controller");
+  start_controllers.push_back("rover_drivetrain_controller");
+  controller_manager_node->switch_controller(
+    start_controllers, stop_controllers, 1,
+    controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT);
+
+  RCLCPP_INFO(controller_manager_node->get_logger(), "Controllers are loaded and turned on.");
+
+  // // // Run the node(s)
+  executor->add_node(controller_manager_node);
+  executor->spin();
+
+  // Exit
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/urc_controllers/CMakeLists.txt b/urc_controllers/CMakeLists.txt
@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 3.8)
+project(urc_controllers)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(urc_hw REQUIRED)
+find_package(urc_util REQUIRED)
+find_package(urc_msgs REQUIRED)
+find_package(urc_nanopb REQUIRED)
+
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/test_hardware_controller.cpp
+  src/imu_broadcaster.cpp
+  src/status_light_controller.cpp
+  src/bms_broadcaster.cpp
+  include/urc_controllers/test_hardware_controller.hpp
+  include/urc_controllers/imu_broadcaster.hpp
+  include/urc_controllers/status_light_controller.hpp
+  include/urc_controllers/bms_broadcaster.hpp
+)
+
+target_compile_features(urc_controllers PUBLIC c_std_99 cxx_std_17)
+target_include_directories(urc_controllers PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  controller_interface
+  hardware_interface
+  realtime_tools
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  urc_hw
+  urc_util
+  urc_nanopb
+  urc_msgs
+)
+
+target_compile_definitions(urc_controllers PRIVATE "urc_hw_BUILDING_LIBRARY")
+pluginlib_export_plugin_description_file(controller_interface test_hardware_controller.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+    TARGETS urc_controllers
+    EXPORT export_${PROJECT_NAME}
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  urc_hw
+  urc_util
+)
+
+ament_export_targets(
+    export_${PROJECT_NAME}
+)
+
+ament_package()
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		# URC Dev Tools

		This is the folder for all the small tools used for testing purposes.