diff --git a/.gitmodules b/.gitmodules index c57094fd..024568e3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,3 +9,7 @@ [submodule "src/third-party/zed-ros2-wrapper"] path = src/third-party/zed-ros2-wrapper url = https://github.com/stereolabs/zed-ros2-wrapper.git +[submodule "src/third-party/micro_ros_setup"] + path = src/third-party/micro_ros_setup + url = https://github.com/micro-ROS/micro_ros_setup.git + branch = humble diff --git a/rosdep-keys.txt b/rosdep-keys.txt index f32c849e..c79763d3 100644 --- a/rosdep-keys.txt +++ b/rosdep-keys.txt @@ -77,4 +77,11 @@ boost joint_trajectory_controller diff_drive_controller joint_state_publisher_gui -libjsoncpp \ No newline at end of file +libjsoncpp +flex +bison +libncurses-dev +usbutils +tinyxml2 +clang-tidy +python3-vcstool \ No newline at end of file diff --git a/src/HW-Devices/ros_phoenix_humble/launch/science.launch.py b/src/HW-Devices/ros_phoenix_humble/launch/science.launch.py new file mode 100644 index 00000000..80665828 --- /dev/null +++ b/src/HW-Devices/ros_phoenix_humble/launch/science.launch.py @@ -0,0 +1,31 @@ +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description with multiple components.""" + container = ComposableNodeContainer( + name="PhoenixContainer", + namespace="", + package="ros_phoenix", + executable="phoenix_container", + parameters=[{"interface": "can0"}], + composable_node_descriptions=[ + ComposableNode( + package="ros_phoenix", + plugin="ros_phoenix::TalonSRX", + name="drill", + parameters=[{"id": 15}], + ), + ComposableNode( + package="ros_phoenix", + plugin="ros_phoenix::TalonSRX", + name="elevator", + parameters=[{"id": 14}], + ), + ], + output="screen", + ) + + return launch.LaunchDescription([container]) diff --git a/src/HW-Devices/ros_roboclaw/ros_roboclaw/drill_roboclaw_node.py b/src/HW-Devices/ros_roboclaw/ros_roboclaw/drill_roboclaw_node.py new file mode 100644 index 00000000..5c791838 --- /dev/null +++ b/src/HW-Devices/ros_roboclaw/ros_roboclaw/drill_roboclaw_node.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +import rclpy +from .roboclaw_library import Roboclaw +from rclpy.node import Node +from std_msgs.msg import Float32 + + +class RoboClaw(Node): + def __init__(self): + super().__init__("drill_roboclaw_node") + self.get_logger().info("Drill RoboClaw node has been started.") + self.declare_parameter("port", "/dev/ttyACM1") + self.declare_parameter("baud_rate", 115200) + port = self.get_parameter("port").get_parameter_value().string_value + baud_rate = self.get_parameter("baud_rate").get_parameter_value().integer_value + self.controller = Roboclaw(port, baud_rate) + self.address = 0x80 # Default address for the RoboClaw + if not self.controller.Open(): + self.get_logger().error(f"Failed to connect to RoboClaw on {port}.") + raise RuntimeError("Could not open serial connection to RoboClaw.") + self.get_logger().info(f"Connected to RoboClaw on {port} at {baud_rate} baud.") + self.cmd_sub = self.create_subscription( + Float32, "drill/set", self.cmd_callback, 10 + ) + (rc, version) = self.controller.ReadVersion(self.address) + if rc: + self.get_logger().info(f"Version: {version}") + else: + self.get_logger().error("Failed to read version from RoboClaw.") + self.telemetry_timer = self.create_timer(10.0, self.telemetry_callback) + self.watchdog_flag = False + self.log_flag = True + self.watchdog_timer = self.create_timer(0.5, self.watchdog_callback) + + def telemetry_callback(self): + (okE, err) = self.controller.ReadError(self.address) + if okE: + if err: + self.get_logger().error(f"Error code: {err}") + else: + self.get_logger().error("Failed to read error code from RoboClaw.") + + def watchdog_callback(self): + if self.watchdog_flag: + self.watchdog_flag = False + self.log_flag = True + return + if self.log_flag: + self.get_logger().warn( + "Watchdog timeout: no command received in the last 0.5 seconds. Stopping motor." + ) + self.log_flag = False + if not self.controller.DutyM1(self.address, 0): + self.get_logger().error("Failed to send watchdog command to RoboClaw.") + + def cmd_callback(self, msg): + duty_cycle = msg.data + if duty_cycle < -1.0 or duty_cycle > 1.0: + self.get_logger().error( + f"Received invalid duty cycle {duty_cycle}. Must be between -1.0 and 1.0." + ) + return + duty_cycle_int16 = int( + duty_cycle * 32767 + ) # Scale from -1.0 to 1.0 range to -32767 to 32767 + if not self.controller.DutyM1(self.address, duty_cycle_int16): + self.get_logger().error( + f"Failed to set duty cycle {duty_cycle_int16} on RoboClaw." + ) + self.watchdog_flag = True + + +def main(args=None): + """ + Initializes the ROS 2 python library, starts this node, and enters the + ROS 2 spin loop to process incoming messages and trigger callbacks. + """ + rclpy.init(args=args) + node = RoboClaw() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/HW-Devices/ros_roboclaw/ros_roboclaw/elevator_roboclaw_node.py b/src/HW-Devices/ros_roboclaw/ros_roboclaw/elevator_roboclaw_node.py new file mode 100644 index 00000000..56fdd558 --- /dev/null +++ b/src/HW-Devices/ros_roboclaw/ros_roboclaw/elevator_roboclaw_node.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +import rclpy +from .roboclaw_library import Roboclaw +from rclpy.node import Node +from std_msgs.msg import Float32 + + +class RoboClaw(Node): + def __init__(self): + super().__init__("elevator_roboclaw_node") + self.get_logger().info("Elevator RoboClaw node has been started.") + self.declare_parameter("port", "/dev/ttyACM0") + self.declare_parameter("baud_rate", 115200) + port = self.get_parameter("port").get_parameter_value().string_value + baud_rate = self.get_parameter("baud_rate").get_parameter_value().integer_value + self.controller = Roboclaw(port, baud_rate) + self.address = 0x80 # Default address for the RoboClaw + if not self.controller.Open(): + self.get_logger().error(f"Failed to connect to RoboClaw on {port}.") + raise RuntimeError("Could not open serial connection to RoboClaw.") + self.get_logger().info(f"Connected to RoboClaw on {port} at {baud_rate} baud.") + self.cmd_sub = self.create_subscription( + Float32, "elevator/set", self.cmd_callback, 10 + ) + (rc, version) = self.controller.ReadVersion(self.address) + if rc: + self.get_logger().info(f"Version: {version}") + else: + self.get_logger().error("Failed to read version from RoboClaw.") + self.telemetry_timer = self.create_timer(10.0, self.telemetry_callback) + self.watchdog_flag = False + self.log_flag = True + self.watchdog_timer = self.create_timer(0.5, self.watchdog_callback) + + def telemetry_callback(self): + (okE, err) = self.controller.ReadError(self.address) + if okE: + if err: + self.get_logger().error(f"Error code: {err}") + else: + self.get_logger().error("Failed to read error code from RoboClaw.") + + def watchdog_callback(self): + if self.watchdog_flag: + self.watchdog_flag = False + self.log_flag = True + return + if self.log_flag: + self.get_logger().warn( + "Watchdog timeout: no command received in the last 0.5 seconds. Stopping motor." + ) + self.log_flag = False + if not self.controller.DutyM1(self.address, 0): + self.get_logger().error("Failed to send watchdog command to RoboClaw.") + + def cmd_callback(self, msg): + duty_cycle = msg.data + if duty_cycle < -1.0 or duty_cycle > 1.0: + self.get_logger().error( + f"Received invalid duty cycle {duty_cycle}. Must be between -1.0 and 1.0." + ) + return + duty_cycle_int16 = int( + duty_cycle * 32767 + ) # Scale from -1.0 to 1.0 range to -32767 to 32767 + if not self.controller.DutyM1(self.address, duty_cycle_int16): + self.get_logger().error( + f"Failed to set duty cycle {duty_cycle_int16} on RoboClaw." + ) + self.watchdog_flag = True + + +def main(args=None): + """ + Initializes the ROS 2 python library, starts this node, and enters the + ROS 2 spin loop to process incoming messages and trigger callbacks. + """ + rclpy.init(args=args) + node = RoboClaw() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/HW-Devices/ros_roboclaw/setup.py b/src/HW-Devices/ros_roboclaw/setup.py index 327d672a..450c686c 100644 --- a/src/HW-Devices/ros_roboclaw/setup.py +++ b/src/HW-Devices/ros_roboclaw/setup.py @@ -18,6 +18,10 @@ license="Apache-2.0", tests_require=["pytest"], entry_points={ - "console_scripts": ["roboclaw_node = ros_roboclaw.roboclaw_node:main"], + "console_scripts": [ + "roboclaw_node = ros_roboclaw.roboclaw_node:main", + "drill_roboclaw_node = ros_roboclaw.drill_roboclaw_node:main", + "elevator_roboclaw_node = ros_roboclaw.elevator_roboclaw_node:main", + ], }, ) diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index c31c639c..00bd7154 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ros_phoenix REQUIRED) find_package(interfaces REQUIRED) find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) set(CMAKE_CXX_STANDARD 14) @@ -28,6 +29,10 @@ add_executable(arm src/arm.cpp ) +add_executable(science + src/science.cpp +) + ament_target_dependencies(drive rclcpp geometry_msgs @@ -48,9 +53,18 @@ ament_target_dependencies(arm std_srvs ) +ament_target_dependencies(science + rclcpp + joy + ros_phoenix + interfaces + std_msgs +) + install(TARGETS drive arm + science DESTINATION lib/${PROJECT_NAME}) # Install header files diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index c987df68..9eda3e15 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -22,4 +22,58 @@ arm_teleop_node: max_wrist_speed: 1.0 max_act1_speed: 1.0 max_act2_speed: 1.0 - max_elbow_yaw_speed: 1.0 \ No newline at end of file + max_elbow_yaw_speed: 1.0 +science_teleop_node: + ros__parameters: + drill_button: 0 + drill_elevation_axis: 1 + servo_control_axis: 5 + servo1: + name: "pump1" + button: 12 + max_value: 65535 + default_value: 0 + min_value: 0 + servo2: + name: "pump2" + button: 13 + max_value: 65535 + default_value: 0 + min_value: 0 + servo3: + name: "pump3" + button: 14 + max_value: 65535 + default_value: 0 + min_value: 0 + servo4: + name: "pump4" + button: 15 + max_value: 65535 + default_value: 0 + min_value: 0 + servo5: + name: "sample_servo" + button: 16 + max_value: 6881 + default_value: 2949 + min_value: 2949 + servo6: + name: "ion_exchange_servo" + button: 17 + max_value: 65535 + default_value: 0 + min_value: 0 + servo7: + name: "servo7" + button: 2 + max_value: 65535 + default_value: 0 + min_value: 0 + servo8: + name: "servo8" + button: 1 + max_value: 65535 + default_value: 0 + min_value: 0 + \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/drive.hpp b/src/Teleop-Control/joystick_control/include/drive.hpp index 2835aed0..0aa08dce 100644 --- a/src/Teleop-Control/joystick_control/include/drive.hpp +++ b/src/Teleop-Control/joystick_control/include/drive.hpp @@ -11,8 +11,8 @@ class drive : public rclcpp::Node { void drive_control(std::shared_ptr joystickMsg); private: - rclcpp::Publisher::SharedPtr twist_pub; - rclcpp::Subscription::SharedPtr joy_sub; + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Subscription::SharedPtr joy_sub_; void declare_parameters(); void load_parameters(); diff --git a/src/Teleop-Control/joystick_control/include/science.hpp b/src/Teleop-Control/joystick_control/include/science.hpp new file mode 100644 index 00000000..a039079c --- /dev/null +++ b/src/Teleop-Control/joystick_control/include/science.hpp @@ -0,0 +1,41 @@ +#ifndef SCIENCE_HPP +#define SCIENCE_HPP + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joy.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/u_int32.hpp" + +class science : public rclcpp::Node { +public: + science(); + void science_control(std::shared_ptr joystickMsg); + +private: + rclcpp::Publisher::SharedPtr drill_pub_; + rclcpp::Publisher::SharedPtr elevator_pub_; + rclcpp::Subscription::SharedPtr joy_sub_; + + std::vector servo_values_; + std::vector::SharedPtr> servo_pubs_; + + void update_servo_value(int servo_index, + const sensor_msgs::msg::Joy &joystickMsg); + void declare_parameters(); + void load_parameters(); + + // Parameters + int kDrillButton; + int kDrillElevationAxis; + int kServoControlAxis; + + std::vector kServoNames; + std::vector kServoButtons; + std::vector kServoMaxValues; + std::vector kServoMinValues; + std::vector kServoDefaultValues; + + bool initialized_; +}; + +#endif // SCIENCE_HPP \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/science_controller.launch.py b/src/Teleop-Control/joystick_control/launch/science_controller.launch.py new file mode 100644 index 00000000..57a5da62 --- /dev/null +++ b/src/Teleop-Control/joystick_control/launch/science_controller.launch.py @@ -0,0 +1,77 @@ +import launch +import launch_ros.actions +from launch_ros.actions import Node +from launch import LaunchDescription +import os +from ament_index_python.packages import get_package_share_directory + + +def get_joy_id(dev_path, default_id): + """Checks for a symlink and returns the integer ID, or a default.""" + if os.path.exists(dev_path): + # Resolves /dev/joy_a -> /dev/input/jsX + real_path = os.path.realpath(dev_path) + # Pulls the digits out of the path string + try: + print(f"Found symlink {dev_path} -> {real_path}") + return int("".join(filter(str.isdigit, real_path))) + except ValueError: + print( + f"Could not parse joystick ID from {real_path}, using default {default_id}" + ) + return default_id + else: + print(f"Symlink {dev_path} does not exist, using default ID {default_id}") + return default_id + + +def generate_launch_description(): + pkg_joystick_control = get_package_share_directory("joystick_control") + parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml") + # Detect IDs dynamically + # We use 0 and 1 as defaults if the symlinks aren't found + id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 1) + id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 0) + + return LaunchDescription( + [ + Node( + package="joy", + executable="joy_node", + name="joy_node_a", + parameters=[ + { + "device_id": id_a, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/drive/joy")], + ), + Node( + package="joy", + executable="joy_node", + name="joy_node_b", + parameters=[ + { + "device_id": id_b, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/science/joy")], + ), + Node( + package="joystick_control", + executable="science", + name="science_teleop_node", + parameters=[parameters_file], + remappings=[("/joy", "/science/joy")], + ), + Node( + package="joystick_control", + executable="drive", + name="drive_teleop_node", + parameters=[parameters_file], + remappings=[("/joy", "/drive/joy")], + ), + ] + ) diff --git a/src/Teleop-Control/joystick_control/src/drive.cpp b/src/Teleop-Control/joystick_control/src/drive.cpp index 73f3c86a..17dcaae8 100644 --- a/src/Teleop-Control/joystick_control/src/drive.cpp +++ b/src/Teleop-Control/joystick_control/src/drive.cpp @@ -3,8 +3,8 @@ drive::drive() : Node("drive_node"), initialized_(false) { declare_parameters(); load_parameters(); - twist_pub = this->create_publisher("cmd_vel", 10); - joy_sub = this->create_subscription( + twist_pub_ = this->create_publisher("cmd_vel", 10); + joy_sub_ = this->create_subscription( "/joy", 10, std::bind(&drive::drive_control, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "Drive controller started"); @@ -24,7 +24,7 @@ void drive::drive_control(std::shared_ptr joystickMsg) { twist.linear.y = joystickMsg->axes[kStrafeAxis] * kMaxLinear; twist.angular.z = joystickMsg->axes[kYawAxis] * kMaxAngular; - twist_pub->publish(twist); + twist_pub_->publish(twist); }; void drive::declare_parameters() { diff --git a/src/Teleop-Control/joystick_control/src/science.cpp b/src/Teleop-Control/joystick_control/src/science.cpp new file mode 100644 index 00000000..9e5f03ab --- /dev/null +++ b/src/Teleop-Control/joystick_control/src/science.cpp @@ -0,0 +1,243 @@ +#include "science.hpp" + +science::science() : Node("science_node") { + declare_parameters(); + load_parameters(); + drill_pub_ = this->create_publisher("drill/set", 10); + elevator_pub_ = + this->create_publisher("elevator/set", 10); + joy_sub_ = this->create_subscription( + "/joy", 10, + std::bind(&science::science_control, this, std::placeholders::_1)); + + for (int i = 0; i < kServoNames.size(); i++) { + servo_values_.push_back(kServoDefaultValues[i]); + servo_pubs_.push_back( + this->create_publisher(kServoNames[i], 10)); + } + + RCLCPP_INFO(this->get_logger(), "Science controller started"); +}; + +void science::science_control( + std::shared_ptr joystickMsg) { + auto drill_msg_ = std_msgs::msg::Float32(); + auto elevator_msg_ = std_msgs::msg::Float32(); + if (joystickMsg->buttons[kDrillButton]) { + drill_msg_.data = 1.0; + } else { + drill_msg_.data = 0.0; + } + elevator_msg_.data = -(joystickMsg->axes[kDrillElevationAxis]); + + for (int i = 0; i < kServoNames.size(); i++) { + update_servo_value(i, *joystickMsg); + } + + drill_pub_->publish(drill_msg_); + elevator_pub_->publish(elevator_msg_); +}; + +void science::update_servo_value(int servo_index, + const sensor_msgs::msg::Joy &joystickMsg) { + if (servo_index < 0 || servo_index >= kServoButtons.size()) { + RCLCPP_ERROR(this->get_logger(), "Invalid servo index: %d", servo_index); + return; + } + + int button = kServoButtons[servo_index]; + int max_value = kServoMaxValues[servo_index]; + int min_value = kServoMinValues[servo_index]; + + if (servo_values_[servo_index] <= max_value && + servo_values_[servo_index] >= min_value) { + if (joystickMsg.buttons[button]) { + if (joystickMsg.axes[kServoControlAxis] == 1.0) { + if (servo_values_[servo_index] + (max_value - min_value) / 10 > + max_value) { + servo_values_[servo_index] = max_value; + } else { + servo_values_[servo_index] += (max_value - min_value) / 10; + } + servo_pubs_[servo_index]->publish( + std_msgs::msg::UInt32().set__data(servo_values_[servo_index])); + } else if (joystickMsg.axes[kServoControlAxis] == -1.0) { + if (servo_values_[servo_index] - (max_value - min_value) / 10 < + min_value) { + servo_values_[servo_index] = min_value; + } else { + servo_values_[servo_index] -= (max_value - min_value) / 10; + } + servo_pubs_[servo_index]->publish( + std_msgs::msg::UInt32().set__data(servo_values_[servo_index])); + } + } + } else { + RCLCPP_ERROR(this->get_logger(), "Error: %s value out of bounds: %d", + kServoNames[servo_index].c_str(), servo_values_[servo_index]); + servo_values_[servo_index] = min_value; + servo_pubs_[servo_index]->publish( + std_msgs::msg::UInt32().set__data(servo_values_[servo_index])); + } +} + +void science::declare_parameters() { + this->declare_parameter("drill_button", 1); + this->declare_parameter("drill_elevation_axis", 2); + this->declare_parameter("servo_control_axis", 5); + + this->declare_parameter("servo1.button", 12); + this->declare_parameter("servo1.max_value", 65535); + this->declare_parameter("servo1.min_value", 0); + this->declare_parameter("servo1.default_value", 0); + this->declare_parameter("servo1.name", "servo1"); + + this->declare_parameter("servo2.button", 13); + this->declare_parameter("servo2.max_value", 65535); + this->declare_parameter("servo2.min_value", 0); + this->declare_parameter("servo2.default_value", 0); + this->declare_parameter("servo2.name", "servo2"); + + this->declare_parameter("servo3.button", 14); + this->declare_parameter("servo3.max_value", 65535); + this->declare_parameter("servo3.min_value", 0); + this->declare_parameter("servo3.default_value", 0); + this->declare_parameter("servo3.name", "servo3"); + + this->declare_parameter("servo4.button", 15); + this->declare_parameter("servo4.max_value", 65535); + this->declare_parameter("servo4.min_value", 0); + this->declare_parameter("servo4.default_value", 0); + this->declare_parameter("servo4.name", "servo4"); + + this->declare_parameter("servo5.button", 16); + this->declare_parameter("servo5.max_value", 65535); + this->declare_parameter("servo5.min_value", 0); + this->declare_parameter("servo5.default_value", 0); + this->declare_parameter("servo5.name", "servo5"); + + this->declare_parameter("servo6.button", 17); + this->declare_parameter("servo6.max_value", 65535); + this->declare_parameter("servo6.min_value", 0); + this->declare_parameter("servo6.default_value", 0); + this->declare_parameter("servo6.name", "servo6"); + + this->declare_parameter("servo7.button", 2); + this->declare_parameter("servo7.max_value", 65535); + this->declare_parameter("servo7.min_value", 0); + this->declare_parameter("servo7.default_value", 0); + this->declare_parameter("servo7.name", "servo7"); + + this->declare_parameter("servo8.button", 1); + this->declare_parameter("servo8.max_value", 65535); + this->declare_parameter("servo8.min_value", 0); + this->declare_parameter("servo8.default_value", 0); + this->declare_parameter("servo8.name", "servo8"); +} + +void science::load_parameters() { + std::string servo_name; + int servo_button; + int servo_max_value; + int servo_min_value; + int servo_default_value; + + this->get_parameter("drill_button", kDrillButton); + this->get_parameter("drill_elevation_axis", kDrillElevationAxis); + this->get_parameter("servo_control_axis", kServoControlAxis); + this->get_parameter("servo1.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo1.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo1.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo1.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo1.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo2.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo2.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo2.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo2.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo2.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo3.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo3.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo3.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo3.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo3.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo4.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo4.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo4.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo4.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo4.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo5.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo5.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo5.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo5.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo5.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo6.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo6.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo6.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo6.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo6.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo7.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo7.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo7.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo7.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo7.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); + + this->get_parameter("servo8.name", servo_name); + kServoNames.push_back(servo_name); + this->get_parameter("servo8.button", servo_button); + kServoButtons.push_back(servo_button); + this->get_parameter("servo8.max_value", servo_max_value); + kServoMaxValues.push_back(servo_max_value); + this->get_parameter("servo8.min_value", servo_min_value); + kServoMinValues.push_back(servo_min_value); + this->get_parameter("servo8.default_value", servo_default_value); + kServoDefaultValues.push_back(servo_default_value); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/third-party/micro_ros_setup b/src/third-party/micro_ros_setup new file mode 160000 index 00000000..f8697a32 --- /dev/null +++ b/src/third-party/micro_ros_setup @@ -0,0 +1 @@ +Subproject commit f8697a3262b0c42caf29052de1adff1561412827