From 1a6647bf858c1cbf3cf9f5850b2726374b81c43b Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 22 Jan 2026 05:39:35 +0000 Subject: [PATCH 01/23] Implement teleop modeless control with arm and drive classes --- build.sh | 2 +- src/teleop_modeless/CMakeLists.txt | 26 ++++++++++++++++++++++++++ src/teleop_modeless/include/arm.hpp | 17 +++++++++++++++++ src/teleop_modeless/include/drive.hpp | 21 +++++++++++++++++++++ src/teleop_modeless/package.xml | 18 ++++++++++++++++++ src/teleop_modeless/src/arm.cpp | 12 ++++++++++++ src/teleop_modeless/src/drive.cpp | 15 +++++++++++++++ 7 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 src/teleop_modeless/CMakeLists.txt create mode 100644 src/teleop_modeless/include/arm.hpp create mode 100644 src/teleop_modeless/include/drive.hpp create mode 100644 src/teleop_modeless/package.xml create mode 100644 src/teleop_modeless/src/arm.cpp create mode 100644 src/teleop_modeless/src/drive.cpp diff --git a/build.sh b/build.sh index 6a62a8bc..a0f1aefd 100755 --- a/build.sh +++ b/build.sh @@ -10,4 +10,4 @@ find ./src -path ./src/third-party -prune -o \ rosdep install --from-paths src -i -r -y -colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ No newline at end of file +colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers 1 \ No newline at end of file diff --git a/src/teleop_modeless/CMakeLists.txt b/src/teleop_modeless/CMakeLists.txt new file mode 100644 index 00000000..6759a367 --- /dev/null +++ b/src/teleop_modeless/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(teleop_modeless) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/teleop_modeless/include/arm.hpp b/src/teleop_modeless/include/arm.hpp new file mode 100644 index 00000000..229dda43 --- /dev/null +++ b/src/teleop_modeless/include/arm.hpp @@ -0,0 +1,17 @@ +#ifndef ARM_HPP +#define ARM_HPP + +#include "geometry_msgs/msg/twist.hpp" +#include "interfaces/srv/move_servo.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joy.hpp" + +class arm : public rclcpp::Node { +public: + arm(); + void arm_control(std::shared_ptr joystickMsg); + +private: + rclcpp::Subscription::SharedPtr joy_sub; + rclcpp::Publisher::SharedPtr joint_pub; +}; \ No newline at end of file diff --git a/src/teleop_modeless/include/drive.hpp b/src/teleop_modeless/include/drive.hpp new file mode 100644 index 00000000..35a21a6b --- /dev/null +++ b/src/teleop_modeless/include/drive.hpp @@ -0,0 +1,21 @@ +#ifndef DRIVE_HPP +#define DRIVE_HPP + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "sensor_msgs/msg/joy.hpp" + +class drive : public rclcpp::Node { +public: + drive(); + void drive_control(std::shared_ptr joystickMsg); +private: + rclcpp::Publisher::SharedPtr twist_pub; + rclcpp::Subscription::SharedPtr joy_sub; + int kForwardAxis = 1; // Example axis index for forward/backward + int kYawAxis = 0; // Example axis index for left/right + double kMaxLinear = 2.0; // Max linear speed + double kMaxAngular = 2.0; // Max angular speed +}; + +#endif // DRIVE_HPP \ No newline at end of file diff --git a/src/teleop_modeless/package.xml b/src/teleop_modeless/package.xml new file mode 100644 index 00000000..3b97f8b8 --- /dev/null +++ b/src/teleop_modeless/package.xml @@ -0,0 +1,18 @@ + + + + teleop_modeless + 0.0.0 + TODO: Package description + vscode + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/teleop_modeless/src/arm.cpp b/src/teleop_modeless/src/arm.cpp new file mode 100644 index 00000000..4c7abd1d --- /dev/null +++ b/src/teleop_modeless/src/arm.cpp @@ -0,0 +1,12 @@ +#include "arm.hpp" + +arm::arm() : Node("arm_node") { + joy_sub = this->create_subscription( + "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); + joint_pub = this->create_publisher( + "/servo_node/delta_joint_cmds", 10); + + joint_msg_ = control_msgs::msg::JointJog(); + joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", + "Joint_4", "Joint_5", "Joint_6"}; +} \ No newline at end of file diff --git a/src/teleop_modeless/src/drive.cpp b/src/teleop_modeless/src/drive.cpp new file mode 100644 index 00000000..9297da1a --- /dev/null +++ b/src/teleop_modeless/src/drive.cpp @@ -0,0 +1,15 @@ +#include "drive.hpp" + +drive::drive() : Node("drive_node") { + twist_pub = this->create_publisher("cmd_vel", 10); + joy_sub = this->create_subscription( + "controller_b/joy", 10, + std::bind(&drive::drive_control, this, std::placeholders::_1)); +}; + +void drive::drive_control(std::shared_ptr joystickMsg) { + auto twist = geometry_msgs::msg::Twist(); + twist.linear.x = joystickMsg->axes[kForwardAxis] * kMaxLinear; + twist.angular.z = joystickMsg->axes[kYawAxis] * kMaxAngular; + twist_pub->publish(twist); +}; From 949778ba0c28755fff5028b9bdbfe62f3a179578 Mon Sep 17 00:00:00 2001 From: Seysha Date: Fri, 23 Jan 2026 02:37:29 +0000 Subject: [PATCH 02/23] Add arm control functionality and configuration parameters for teleoperation --- src/teleop_modeless/config/pxn.yaml | 55 ++++++++++++++++++ src/teleop_modeless/include/ArmHelpers.hpp | 10 ++++ src/teleop_modeless/include/arm.hpp | 22 ++++++++ src/teleop_modeless/include/drive.hpp | 23 ++++---- src/teleop_modeless/src/ArmHelpers.cpp | 20 +++++++ src/teleop_modeless/src/arm.cpp | 65 +++++++++++++++++++++- 6 files changed, 182 insertions(+), 13 deletions(-) create mode 100644 src/teleop_modeless/config/pxn.yaml create mode 100644 src/teleop_modeless/include/ArmHelpers.hpp create mode 100644 src/teleop_modeless/src/ArmHelpers.cpp diff --git a/src/teleop_modeless/config/pxn.yaml b/src/teleop_modeless/config/pxn.yaml new file mode 100644 index 00000000..3353bc5f --- /dev/null +++ b/src/teleop_modeless/config/pxn.yaml @@ -0,0 +1,55 @@ + +throttle: + axis: 2 + min: -1.0 + max: 1.0 +forward_axis: 1 +yaw_axis: 0 +max_linear: 2.0 +max_angular: 1.0 +max_increment: 0.1 +min_speed: 0.001 +default_pan: 3.14159 +default_tilt: 0.785398 +base_axis: 0 +wrist_roll: 4 +wrist_yaw_positive: 3 +wrist_yaw_negative: 5 +act1_axis: 1 +act2_axis: 5 +elbow_yaw: 3 +claw: 1 +simple_forward: 2 +simple_backward: 4 +servo_name: "arm" + + # arm_ik_mode: + # x_axis: 1 + # y_axis: 0 + # up_button: 5 + # down_button: 3 + # rotate_around_y: 5 + # rotate_around_x: 4 + # rotate_around_z: 3 + # open_claw: 1 + # close_claw: 0 + # base_frame: 4 + # eef_frame: 2 + # servo_name: "arm" + + # science_mode: + # platform_axis: 1 + # drill_button: 0 + # microscope_axis: 5 + # soil_collection_button: 5 + # cancel_collection_button: 3 + # soil_test_button: 2 + # lock_sample_button: 4 + # microscope_light_button: 4 + # microscope_servo_name: "microscope" + # collection_servo_name: "collection" + # collection_open: 2.967060 + # collection_dump: 1.396263 + # collection_test: 0.0 + # collection_lock: 3.141593 + # throttle_axis: 2 diff --git a/src/teleop_modeless/include/ArmHelpers.hpp b/src/teleop_modeless/include/ArmHelpers.hpp new file mode 100644 index 00000000..770ee3f9 --- /dev/null +++ b/src/teleop_modeless/include/ArmHelpers.hpp @@ -0,0 +1,10 @@ +#ifndef JOYSTICK_CONTROL__ARM_HELPERS_HPP_ +#define JOYSTICK_CONTROL__ARM_HELPERS_HPP_ + +#include "rclcpp/rclcpp.hpp" + +namespace ArmHelpers { +bool start_moveit_servo(rclcpp::Node *node, int attempts = 3); +} + +#endif \ No newline at end of file diff --git a/src/teleop_modeless/include/arm.hpp b/src/teleop_modeless/include/arm.hpp index 229dda43..7617c9e1 100644 --- a/src/teleop_modeless/include/arm.hpp +++ b/src/teleop_modeless/include/arm.hpp @@ -14,4 +14,26 @@ class arm : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr joy_sub; rclcpp::Publisher::SharedPtr joint_pub; + + int artificial_wrist_axis(int, int); + + declareParameters(); + loadParameters(); + + double const baseSpeed = 1.0; + double const wristRollSpeed = 1.0; + double const wristSpeed = 1.0; + double const act1Speed = 1.0; + double const act2Speed = 1.0; + double const elbowYawSpeed = 1.0; + + int kThrottleAxis; + int kBaseAxis; + int kWristRoll; + int kWristYaw; + int kAct1Axis; + int kAct2Axis; + int kElbowYaw; + int kclaw; + }; \ No newline at end of file diff --git a/src/teleop_modeless/include/drive.hpp b/src/teleop_modeless/include/drive.hpp index 35a21a6b..34dfd9d0 100644 --- a/src/teleop_modeless/include/drive.hpp +++ b/src/teleop_modeless/include/drive.hpp @@ -1,21 +1,22 @@ -#ifndef DRIVE_HPP +#ifndef DRIVE_HPP #define DRIVE_HPP -#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joy.hpp" class drive : public rclcpp::Node { public: - drive(); - void drive_control(std::shared_ptr joystickMsg); + drive(); + void drive_control(std::shared_ptr joystickMsg); + private: - rclcpp::Publisher::SharedPtr twist_pub; - rclcpp::Subscription::SharedPtr joy_sub; - int kForwardAxis = 1; // Example axis index for forward/backward - int kYawAxis = 0; // Example axis index for left/right - double kMaxLinear = 2.0; // Max linear speed - double kMaxAngular = 2.0; // Max angular speed + rclcpp::Publisher::SharedPtr twist_pub; + rclcpp::Subscription::SharedPtr joy_sub; + int kForwardAxis = 1; // Example axis index for forward/backward + int kYawAxis = 0; // Example axis index for left/right + double kMaxLinear = 2.0; // Max linear speed + double kMaxAngular = 2.0; // Max angular speed }; -#endif // DRIVE_HPP \ No newline at end of file +#endif // DRIVE_HPP \ No newline at end of file diff --git a/src/teleop_modeless/src/ArmHelpers.cpp b/src/teleop_modeless/src/ArmHelpers.cpp new file mode 100644 index 00000000..a0da874a --- /dev/null +++ b/src/teleop_modeless/src/ArmHelpers.cpp @@ -0,0 +1,20 @@ +#include "ArmHelpers.hpp" + +#include "std_srvs/srv/trigger.hpp" + +bool ArmHelpers::start_moveit_servo(rclcpp::Node *node, int attempts) { + auto start_moveit_client = + node->create_client("/servo_node/start_servo"); + int i = 0; + while (!start_moveit_client->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_WARN(node->get_logger(), "Service not available, Waiting..."); + if (++i >= attempts) { + RCLCPP_ERROR(node->get_logger(), + "Service not available after %d attempts. Giving up", i); + return false; + } + } + auto start_request = std::make_shared(); + start_moveit_client->async_send_request(start_request); + return true; +} diff --git a/src/teleop_modeless/src/arm.cpp b/src/teleop_modeless/src/arm.cpp index 4c7abd1d..361995f9 100644 --- a/src/teleop_modeless/src/arm.cpp +++ b/src/teleop_modeless/src/arm.cpp @@ -2,11 +2,72 @@ arm::arm() : Node("arm_node") { joy_sub = this->create_subscription( - "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); + "/controller_b/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub = this->create_publisher( "/servo_node/delta_joint_cmds", 10); - + if (!ArmHelpers::start_moveit_servo(this)) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to start MoveIt servo service, Manual mode will not work"); + } joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; + + servo_pub_ = node_->create_publisher( + "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); +} + +arm::arm_control(std::shared_ptr joystickMsg) { + auto joint_msg = control_msgs::msg::JointJog(); + joint_msg_.header = joystickMsg->header; + + //sample control logic + + joint_msg.velocities = {joystickMsg->axes[kBaseAxis] * baseSpeed * axes[ kThrottleAxis], + joystickMsg->axes[kAct1Axis] * act1Speed * axes[ kThrottleAxis], + joystickMsg->axes[kAct2Axis] * act2Speed * axes[ kThrottleAxis], + joystickMsg->axes[kWristRoll] * wristRollSpeed * axes[ kThrottleAxis], + joystickMsg->axes[kElbowYaw] * elbowYawSpeed * axes[ kThrottleAxis], + joystickMsg->artificial_wrist_axis(axes[kWristYaw_positive], axes[kWristYaw_negative]) * wristSpeed * axes[ kThrottleAxis]}; + + joint_pub->publish(joint_msg); + + if (joystickMsg->buttons[kclaw] == 1) { + auto servo_msg = std_msgs::msg::Float32(); + servo_msg.data = 1.0; // Example position + servo_pub_->publish(servo_msg); + } else { + auto servo_msg = std_msgs::msg::Float32(); + servo_msg.data = 0.0; // Example position + servo_pub_->publish(servo_msg); + } +} + +arm::artificial_wrist_axis(int axis1, int axis2) { + // Creates a cool artificial axis + return axis1 - axis2; +} + +declareParameters() { + this->declare_parameter("throttle.axis", 2); + this->declare_parameter("base_axis", 0); + this->declare_parameter("wrist_roll", 4); + this->declare_parameter("wrist_yaw_positive", 3); + this->declare_parameter("wrist_yaw_negative", 5); + this->declare_parameter("act1_axis", 1); + this->declare_parameter("act2_axis", 5); + this->declare_parameter("elbow_yaw", 6); + this->declare_parameter("claw", 1); +} +loadParameters() { + this->get_parameter("throttle.axis", kThrottleAxis); + this->get_parameter("base_axis", kBaseAxis); + this->get_parameter("wrist_roll", kWristRoll); + this->get_parameter("wrist_yaw_positive", kWristYaw_positive); + this->get_parameter("wrist_yaw_negative", kWristYaw_negative); + this->get_parameter("act1_axis", kAct1Axis); + this->get_parameter("act2_axis", kAct2Axis); + this->get_parameter("elbow_yaw", kElbowYaw); + this->get_parameter("claw", kclaw); } \ No newline at end of file From 85ca540e43baa0c4a5a7c432d27c4b3bba25f455 Mon Sep 17 00:00:00 2001 From: Seysha Date: Sat, 24 Jan 2026 21:51:02 +0000 Subject: [PATCH 03/23] Implement teleoperation functionality with drive and arm control nodes, including parameter handling and launch configuration --- .gitignore | 3 +- .../include/joystick_control/ArmHelpers.hpp | 1 - .../urdf/chassis_urdf.ros2_control.xacro | 6 +- src/teleop_modeless/CMakeLists.txt | 62 ++++++++++++++- src/teleop_modeless/config/pxn.yaml | 49 ++++++------ src/teleop_modeless/include/ArmHelpers.hpp | 4 +- src/teleop_modeless/include/arm.hpp | 32 ++++---- src/teleop_modeless/include/drive.hpp | 14 +++- .../launch/controller.launch.py | 77 +++++++++++++++++++ src/teleop_modeless/src/arm.cpp | 66 ++++++++++------ src/teleop_modeless/src/drive.cpp | 34 +++++++- 11 files changed, 276 insertions(+), 72 deletions(-) create mode 100644 src/teleop_modeless/launch/controller.launch.py diff --git a/.gitignore b/.gitignore index b965222e..3f74b0e8 100644 --- a/.gitignore +++ b/.gitignore @@ -15,4 +15,5 @@ camera_setup/images/* # macOS Directory Information .DS_Store -upgrade_pkg.tar.* \ No newline at end of file +upgrade_pkg.tar.* +src/kindr \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp index 770ee3f9..2c7e1cdb 100644 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp +++ b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp @@ -2,7 +2,6 @@ #define JOYSTICK_CONTROL__ARM_HELPERS_HPP_ #include "rclcpp/rclcpp.hpp" - namespace ArmHelpers { bool start_moveit_servo(rclcpp::Node *node, int attempts = 3); } diff --git a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro index 24b41bd3..3af6078b 100644 --- a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro @@ -5,8 +5,10 @@ - odrive_ros2_control_plugin/ODriveHardwareInterface - can0 + + mock_components/GenericSystem + true + 5 diff --git a/src/teleop_modeless/CMakeLists.txt b/src/teleop_modeless/CMakeLists.txt index 6759a367..50f6c8d2 100644 --- a/src/teleop_modeless/CMakeLists.txt +++ b/src/teleop_modeless/CMakeLists.txt @@ -7,10 +7,70 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(joy REQUIRED) +find_package(ros_phoenix REQUIRED) +find_package(interfaces REQUIRED) +find_package(control_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +set(CMAKE_CXX_STANDARD 14) + +include_directories(include) + + +add_executable(drive + src/drive.cpp +) + +add_executable(arm + src/arm.cpp + src/ArmHelpers.cpp +) + +ament_target_dependencies(drive + rclcpp + geometry_msgs + joy + ros_phoenix + interfaces + control_msgs + std_srvs +) + +ament_target_dependencies(arm + rclcpp + geometry_msgs + joy + ros_phoenix + interfaces + control_msgs + std_srvs +) + +install(TARGETS + drive + arm + DESTINATION lib/${PROJECT_NAME}) + +# Install header files +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +#install config files +# install(DIRECTORY config/ +# DESTINATION share/${PROJECT_NAME} +# ) + +# Install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/src/teleop_modeless/config/pxn.yaml b/src/teleop_modeless/config/pxn.yaml index 3353bc5f..301faecb 100644 --- a/src/teleop_modeless/config/pxn.yaml +++ b/src/teleop_modeless/config/pxn.yaml @@ -1,27 +1,28 @@ - -throttle: - axis: 2 - min: -1.0 - max: 1.0 -forward_axis: 1 -yaw_axis: 0 -max_linear: 2.0 -max_angular: 1.0 -max_increment: 0.1 -min_speed: 0.001 -default_pan: 3.14159 -default_tilt: 0.785398 -base_axis: 0 -wrist_roll: 4 -wrist_yaw_positive: 3 -wrist_yaw_negative: 5 -act1_axis: 1 -act2_axis: 5 -elbow_yaw: 3 -claw: 1 -simple_forward: 2 -simple_backward: 4 -servo_name: "arm" +drive_teleop_node: + ros__parameters: + #TODO change these for an actual PXN + forward_axis: 1 + strafe_axis: 0 + yaw_axis: 2 + max_linear: 2.0 + max_angular: 1.0 +arm_teleop_node: + ros__parameters: + throttle: + axis: 2 + min: -1.0 + max: 1.0 + base_axis: 0 + wrist_roll: 4 + wrist_yaw_positive: 3 + wrist_yaw_negative: 5 + act1_axis: 1 + act2_axis: 5 + elbow_yaw: 3 + claw: 1 + simple_forward: 2 + simple_backward: 4 + servo_name: "arm" # arm_ik_mode: # x_axis: 1 diff --git a/src/teleop_modeless/include/ArmHelpers.hpp b/src/teleop_modeless/include/ArmHelpers.hpp index 770ee3f9..ff010463 100644 --- a/src/teleop_modeless/include/ArmHelpers.hpp +++ b/src/teleop_modeless/include/ArmHelpers.hpp @@ -1,5 +1,5 @@ -#ifndef JOYSTICK_CONTROL__ARM_HELPERS_HPP_ -#define JOYSTICK_CONTROL__ARM_HELPERS_HPP_ +#ifndef teleop_modeless__ARM_HELPERS_HPP_ +#define teleop_modeless__ARM_HELPERS_HPP_ #include "rclcpp/rclcpp.hpp" diff --git a/src/teleop_modeless/include/arm.hpp b/src/teleop_modeless/include/arm.hpp index 7617c9e1..76e1db6b 100644 --- a/src/teleop_modeless/include/arm.hpp +++ b/src/teleop_modeless/include/arm.hpp @@ -1,10 +1,12 @@ #ifndef ARM_HPP #define ARM_HPP +#include "control_msgs/msg/joint_jog.hpp" #include "geometry_msgs/msg/twist.hpp" #include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joy.hpp" +#include "std_msgs/msg/float32.hpp" class arm : public rclcpp::Node { public: @@ -14,26 +16,30 @@ class arm : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr joy_sub; rclcpp::Publisher::SharedPtr joint_pub; + rclcpp::Publisher::SharedPtr servo_pub; + control_msgs::msg::JointJog joint_msg; - int artificial_wrist_axis(int, int); + void declareParameters(); + void loadParameters(); - declareParameters(); - loadParameters(); - - double const baseSpeed = 1.0; - double const wristRollSpeed = 1.0; - double const wristSpeed = 1.0; - double const act1Speed = 1.0; - double const act2Speed = 1.0; - double const elbowYawSpeed = 1.0; + // TODO change these constants based on test + double const maxBaseSpeed = 1.0; + double const maxWristRollSpeed = 1.0; + double const maxWristSpeed = 1.0; + double const maxAct1Speed = 1.0; + double const maxAct2Speed = 1.0; + double const maxElbowYawSpeed = 1.0; int kThrottleAxis; int kBaseAxis; int kWristRoll; - int kWristYaw; + int kWristYaw_positive; + int kWristYaw_negative; int kAct1Axis; int kAct2Axis; int kElbowYaw; int kclaw; - -}; \ No newline at end of file + std::string servoName; +}; + +#endif \ No newline at end of file diff --git a/src/teleop_modeless/include/drive.hpp b/src/teleop_modeless/include/drive.hpp index 34dfd9d0..99b0c21b 100644 --- a/src/teleop_modeless/include/drive.hpp +++ b/src/teleop_modeless/include/drive.hpp @@ -13,10 +13,16 @@ class drive : public rclcpp::Node { private: rclcpp::Publisher::SharedPtr twist_pub; rclcpp::Subscription::SharedPtr joy_sub; - int kForwardAxis = 1; // Example axis index for forward/backward - int kYawAxis = 0; // Example axis index for left/right - double kMaxLinear = 2.0; // Max linear speed - double kMaxAngular = 2.0; // Max angular speed + + void declare_parameters(); + void load_parameters(); + + // Parameters + double kMaxLinear; + double kMaxAngular; + int kForwardAxis; + int kYawAxis; + int kStrafeAxis; }; #endif // DRIVE_HPP \ No newline at end of file diff --git a/src/teleop_modeless/launch/controller.launch.py b/src/teleop_modeless/launch/controller.launch.py new file mode 100644 index 00000000..73b694df --- /dev/null +++ b/src/teleop_modeless/launch/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_teleop_modeless = get_package_share_directory("teleop_modeless") + parameters_file = os.path.join(pkg_teleop_modeless, "pxn.yaml") + # Detect IDs dynamically + # We use 0 and 1 as defaults if the symlinks aren't found + id_a = get_joy_id("/dev/joy_a", 1) + id_b = get_joy_id("/dev/joy_b", 0) + + return LaunchDescription( + [ + # Controller A (Top/Left) + Node( + package="joy", + executable="joy_node", + name="joy_node_a", + parameters=[ + { + "device_id": id_a, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/controller_a/joy")], + ), + # Controller B (Bottom/Right) + Node( + package="joy", + executable="joy_node", + name="joy_node_b", + parameters=[ + { + "device_id": id_b, + "deadzone": 0.05, + } + ], + remappings=[("/joy", "/controller_b/joy")], + ), + Node( + package="teleop_modeless", + executable="arm", + name="arm_teleop_node", + parameters=[parameters_file], + ), + Node( + package="teleop_modeless", + executable="drive", + name="drive_teleop_node", + parameters=[parameters_file], + ), + ] + ) diff --git a/src/teleop_modeless/src/arm.cpp b/src/teleop_modeless/src/arm.cpp index 361995f9..afc2f23a 100644 --- a/src/teleop_modeless/src/arm.cpp +++ b/src/teleop_modeless/src/arm.cpp @@ -1,8 +1,14 @@ #include "arm.hpp" +#include "ArmHelpers.hpp" arm::arm() : Node("arm_node") { + + declareParameters(); + loadParameters(); + joy_sub = this->create_subscription( - "/controller_b/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); + "/controller_b/joy", 10, + std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub = this->create_publisher( "/servo_node/delta_joint_cmds", 10); if (!ArmHelpers::start_moveit_servo(this)) { @@ -10,46 +16,49 @@ arm::arm() : Node("arm_node") { this->get_logger(), "Failed to start MoveIt servo service, Manual mode will not work"); } - joint_msg_ = control_msgs::msg::JointJog(); - joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", - "Joint_4", "Joint_5", "Joint_6"}; + joint_msg = control_msgs::msg::JointJog(); + joint_msg.joint_names = {"Joint_1", "Joint_2", "Joint_3", + "Joint_4", "Joint_5", "Joint_6"}; - servo_pub_ = node_->create_publisher( + servo_pub = this->create_publisher( "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); + + RCLCPP_INFO(this->get_logger(), "Arm controller started"); } -arm::arm_control(std::shared_ptr joystickMsg) { +void arm::arm_control(std::shared_ptr joystickMsg) { auto joint_msg = control_msgs::msg::JointJog(); - joint_msg_.header = joystickMsg->header; - //sample control logic + auto axes = joystickMsg->axes; + auto buttons = joystickMsg->buttons; + + joint_msg.header = joystickMsg->header; + + // sample control logic - joint_msg.velocities = {joystickMsg->axes[kBaseAxis] * baseSpeed * axes[ kThrottleAxis], - joystickMsg->axes[kAct1Axis] * act1Speed * axes[ kThrottleAxis], - joystickMsg->axes[kAct2Axis] * act2Speed * axes[ kThrottleAxis], - joystickMsg->axes[kWristRoll] * wristRollSpeed * axes[ kThrottleAxis], - joystickMsg->axes[kElbowYaw] * elbowYawSpeed * axes[ kThrottleAxis], - joystickMsg->artificial_wrist_axis(axes[kWristYaw_positive], axes[kWristYaw_negative]) * wristSpeed * axes[ kThrottleAxis]}; + joint_msg.velocities = {axes[kBaseAxis] * maxBaseSpeed, + axes[kAct1Axis] * maxAct1Speed, + axes[kAct2Axis] * maxAct2Speed, + axes[kWristRoll] * maxWristRollSpeed, + axes[kElbowYaw] * maxElbowYawSpeed, + buttons[kWristYaw_positive] - + buttons[kWristYaw_negative] * maxWristSpeed * + axes[kThrottleAxis]}; joint_pub->publish(joint_msg); if (joystickMsg->buttons[kclaw] == 1) { auto servo_msg = std_msgs::msg::Float32(); servo_msg.data = 1.0; // Example position - servo_pub_->publish(servo_msg); + servo_pub->publish(servo_msg); } else { auto servo_msg = std_msgs::msg::Float32(); servo_msg.data = 0.0; // Example position - servo_pub_->publish(servo_msg); + servo_pub->publish(servo_msg); } } -arm::artificial_wrist_axis(int axis1, int axis2) { - // Creates a cool artificial axis - return axis1 - axis2; -} - -declareParameters() { +void arm::declareParameters() { this->declare_parameter("throttle.axis", 2); this->declare_parameter("base_axis", 0); this->declare_parameter("wrist_roll", 4); @@ -59,8 +68,9 @@ declareParameters() { this->declare_parameter("act2_axis", 5); this->declare_parameter("elbow_yaw", 6); this->declare_parameter("claw", 1); + this->declare_parameter("servo_name", "arm"); } -loadParameters() { +void arm::loadParameters() { this->get_parameter("throttle.axis", kThrottleAxis); this->get_parameter("base_axis", kBaseAxis); this->get_parameter("wrist_roll", kWristRoll); @@ -70,4 +80,14 @@ loadParameters() { this->get_parameter("act2_axis", kAct2Axis); this->get_parameter("elbow_yaw", kElbowYaw); this->get_parameter("claw", kclaw); + this->get_parameter("servo_name", servoName); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + // Replace 'arm' with your actual class name if different + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/src/teleop_modeless/src/drive.cpp b/src/teleop_modeless/src/drive.cpp index 9297da1a..41e00531 100644 --- a/src/teleop_modeless/src/drive.cpp +++ b/src/teleop_modeless/src/drive.cpp @@ -1,15 +1,47 @@ #include "drive.hpp" drive::drive() : Node("drive_node") { + declare_parameters(); + load_parameters(); twist_pub = this->create_publisher("cmd_vel", 10); joy_sub = this->create_subscription( - "controller_b/joy", 10, + "/controller_a/joy", 10, std::bind(&drive::drive_control, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Drive controller started"); }; void drive::drive_control(std::shared_ptr joystickMsg) { auto twist = geometry_msgs::msg::Twist(); twist.linear.x = joystickMsg->axes[kForwardAxis] * kMaxLinear; + twist.linear.y = joystickMsg->axes[kStrafeAxis] * kMaxLinear; twist.angular.z = joystickMsg->axes[kYawAxis] * kMaxAngular; + twist_pub->publish(twist); }; + +void drive::declare_parameters() { + this->declare_parameter("max_linear", 1.0); + this->declare_parameter("max_angular", 1.0); + this->declare_parameter("forward_axis", 1); + this->declare_parameter("yaw_axis", 2); + this->declare_parameter("strafe_axis", 3); +} +void drive::load_parameters() { + this->get_parameter("max_linear", kMaxLinear); + this->get_parameter("max_angular", kMaxAngular); + this->get_parameter("forward_axis", kForwardAxis); + this->get_parameter("yaw_axis", kYawAxis); + this->get_parameter("strafe_axis", kStrafeAxis); + + RCLCPP_INFO(this->get_logger(), "Loaded Max Linear: %f", kMaxLinear); + RCLCPP_INFO(this->get_logger(), "Loaded Max Angular: %f", kMaxAngular); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + // Replace 'arm' with your actual class name if different + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 4b46bf9f4b65505963f7b573d860b4d2e2f01fc8 Mon Sep 17 00:00:00 2001 From: Seysha Date: Sat, 24 Jan 2026 23:09:50 +0000 Subject: [PATCH 04/23] Reverted unwanted differences from main --- .gitignore | 3 +-- build.sh | 2 +- .../include/joystick_control/ArmHelpers.hpp | 1 + src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro | 6 ++---- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index 3f74b0e8..b965222e 100644 --- a/.gitignore +++ b/.gitignore @@ -15,5 +15,4 @@ camera_setup/images/* # macOS Directory Information .DS_Store -upgrade_pkg.tar.* -src/kindr \ No newline at end of file +upgrade_pkg.tar.* \ No newline at end of file diff --git a/build.sh b/build.sh index a0f1aefd..6a62a8bc 100755 --- a/build.sh +++ b/build.sh @@ -10,4 +10,4 @@ find ./src -path ./src/third-party -prune -o \ rosdep install --from-paths src -i -r -y -colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers 1 \ No newline at end of file +colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp index 2c7e1cdb..770ee3f9 100644 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp +++ b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp @@ -2,6 +2,7 @@ #define JOYSTICK_CONTROL__ARM_HELPERS_HPP_ #include "rclcpp/rclcpp.hpp" + namespace ArmHelpers { bool start_moveit_servo(rclcpp::Node *node, int attempts = 3); } diff --git a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro index 3af6078b..24b41bd3 100644 --- a/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/chassis_urdf.ros2_control.xacro @@ -5,10 +5,8 @@ - - mock_components/GenericSystem - true - + odrive_ros2_control_plugin/ODriveHardwareInterface + can0 5 From 41a24f3654e668c32ef78610b384a1dc30b15b92 Mon Sep 17 00:00:00 2001 From: tomaswilliston Date: Sun, 25 Jan 2026 16:39:06 -0500 Subject: [PATCH 05/23] Removed some comments and renamed some topics --- src/teleop_modeless/CMakeLists.txt | 21 ++------- src/teleop_modeless/config/pxn.yaml | 37 +++------------ src/teleop_modeless/include/arm.hpp | 24 +++++----- .../launch/controller.launch.py | 10 ++--- src/teleop_modeless/src/arm.cpp | 45 +++++++++++-------- src/teleop_modeless/src/drive.cpp | 1 - 6 files changed, 51 insertions(+), 87 deletions(-) diff --git a/src/teleop_modeless/CMakeLists.txt b/src/teleop_modeless/CMakeLists.txt index 50f6c8d2..3436796d 100644 --- a/src/teleop_modeless/CMakeLists.txt +++ b/src/teleop_modeless/CMakeLists.txt @@ -59,28 +59,13 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) #install config files -# install(DIRECTORY config/ -# DESTINATION share/${PROJECT_NAME} -# ) +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME} +) # Install launch files install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/src/teleop_modeless/config/pxn.yaml b/src/teleop_modeless/config/pxn.yaml index 301faecb..536e4d49 100644 --- a/src/teleop_modeless/config/pxn.yaml +++ b/src/teleop_modeless/config/pxn.yaml @@ -23,34 +23,9 @@ arm_teleop_node: simple_forward: 2 simple_backward: 4 servo_name: "arm" - - # arm_ik_mode: - # x_axis: 1 - # y_axis: 0 - # up_button: 5 - # down_button: 3 - # rotate_around_y: 5 - # rotate_around_x: 4 - # rotate_around_z: 3 - # open_claw: 1 - # close_claw: 0 - # base_frame: 4 - # eef_frame: 2 - # servo_name: "arm" - - # science_mode: - # platform_axis: 1 - # drill_button: 0 - # microscope_axis: 5 - # soil_collection_button: 5 - # cancel_collection_button: 3 - # soil_test_button: 2 - # lock_sample_button: 4 - # microscope_light_button: 4 - # microscope_servo_name: "microscope" - # collection_servo_name: "collection" - # collection_open: 2.967060 - # collection_dump: 1.396263 - # collection_test: 0.0 - # collection_lock: 3.141593 - # throttle_axis: 2 + max_base_speed: 1.0 + max_wrist_roll_speed: 1.0 + 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 diff --git a/src/teleop_modeless/include/arm.hpp b/src/teleop_modeless/include/arm.hpp index 76e1db6b..88c4e77e 100644 --- a/src/teleop_modeless/include/arm.hpp +++ b/src/teleop_modeless/include/arm.hpp @@ -14,22 +14,20 @@ class arm : public rclcpp::Node { void arm_control(std::shared_ptr joystickMsg); private: - rclcpp::Subscription::SharedPtr joy_sub; - rclcpp::Publisher::SharedPtr joint_pub; - rclcpp::Publisher::SharedPtr servo_pub; - control_msgs::msg::JointJog joint_msg; + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::SharedPtr servo_pub__; + control_msgs::msg::JointJog joint_msg_; void declareParameters(); void loadParameters(); - // TODO change these constants based on test - double const maxBaseSpeed = 1.0; - double const maxWristRollSpeed = 1.0; - double const maxWristSpeed = 1.0; - double const maxAct1Speed = 1.0; - double const maxAct2Speed = 1.0; - double const maxElbowYawSpeed = 1.0; - + double kMaxBaseSpeed; + double kMaxWristRollSpeed; + double kMaxWristSpeed; + double kMaxAct1Speed; + double kMaxAct2Speed; + double kMaxElbowYawSpeed; int kThrottleAxis; int kBaseAxis; int kWristRoll; @@ -38,7 +36,7 @@ class arm : public rclcpp::Node { int kAct1Axis; int kAct2Axis; int kElbowYaw; - int kclaw; + int kClaw; std::string servoName; }; diff --git a/src/teleop_modeless/launch/controller.launch.py b/src/teleop_modeless/launch/controller.launch.py index 73b694df..db2e00ed 100644 --- a/src/teleop_modeless/launch/controller.launch.py +++ b/src/teleop_modeless/launch/controller.launch.py @@ -30,12 +30,11 @@ def generate_launch_description(): parameters_file = os.path.join(pkg_teleop_modeless, "pxn.yaml") # Detect IDs dynamically # We use 0 and 1 as defaults if the symlinks aren't found - id_a = get_joy_id("/dev/joy_a", 1) - id_b = get_joy_id("/dev/joy_b", 0) + 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( [ - # Controller A (Top/Left) Node( package="joy", executable="joy_node", @@ -46,9 +45,8 @@ def generate_launch_description(): "deadzone": 0.05, } ], - remappings=[("/joy", "/controller_a/joy")], + remappings=[("/joy", "/drive/joy")], ), - # Controller B (Bottom/Right) Node( package="joy", executable="joy_node", @@ -59,7 +57,7 @@ def generate_launch_description(): "deadzone": 0.05, } ], - remappings=[("/joy", "/controller_b/joy")], + remappings=[("/joy", "/arm/joy")], ), Node( package="teleop_modeless", diff --git a/src/teleop_modeless/src/arm.cpp b/src/teleop_modeless/src/arm.cpp index afc2f23a..65317964 100644 --- a/src/teleop_modeless/src/arm.cpp +++ b/src/teleop_modeless/src/arm.cpp @@ -6,37 +6,35 @@ arm::arm() : Node("arm_node") { declareParameters(); loadParameters(); - joy_sub = this->create_subscription( + joy_sub_ = this->create_subscription( "/controller_b/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); - joint_pub = this->create_publisher( + joint_pub_ = this->create_publisher( "/servo_node/delta_joint_cmds", 10); if (!ArmHelpers::start_moveit_servo(this)) { RCLCPP_ERROR( this->get_logger(), "Failed to start MoveIt servo service, Manual mode will not work"); } - joint_msg = control_msgs::msg::JointJog(); - joint_msg.joint_names = {"Joint_1", "Joint_2", "Joint_3", + joint_msg_ = control_msgs::msg::JointJog(); + joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; - servo_pub = this->create_publisher( + servo_pub_ = this->create_publisher( "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); RCLCPP_INFO(this->get_logger(), "Arm controller started"); } void arm::arm_control(std::shared_ptr joystickMsg) { - auto joint_msg = control_msgs::msg::JointJog(); + auto joint_msg_ = control_msgs::msg::JointJog(); auto axes = joystickMsg->axes; auto buttons = joystickMsg->buttons; - joint_msg.header = joystickMsg->header; + joint_msg_.header = joystickMsg->header; - // sample control logic - - joint_msg.velocities = {axes[kBaseAxis] * maxBaseSpeed, + joint_msg_.velocities = {axes[kBaseAxis] * maxBaseSpeed, axes[kAct1Axis] * maxAct1Speed, axes[kAct2Axis] * maxAct2Speed, axes[kWristRoll] * maxWristRollSpeed, @@ -45,16 +43,16 @@ void arm::arm_control(std::shared_ptr joystickMsg) { buttons[kWristYaw_negative] * maxWristSpeed * axes[kThrottleAxis]}; - joint_pub->publish(joint_msg); + joint_pub_->publish(joint_msg_); - if (joystickMsg->buttons[kclaw] == 1) { + if (joystickMsg->buttons[kClaw] == 1) { auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = 1.0; // Example position - servo_pub->publish(servo_msg); + servo_msg.data = 1.0; + servo_pub_->publish(servo_msg); } else { auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = 0.0; // Example position - servo_pub->publish(servo_msg); + servo_msg.data = 0.0; + servo_pub_->publish(servo_msg); } } @@ -69,6 +67,12 @@ void arm::declareParameters() { this->declare_parameter("elbow_yaw", 6); this->declare_parameter("claw", 1); this->declare_parameter("servo_name", "arm"); + this->declare_parameter("max_base_speed", 1.0); + this->declare_parameter("max_wrist_roll_speed", 1.0); + this->declare_parameter("max_wrist_speed", 1.0); + this->declare_parameter("max_act1_speed", 1.0); + this->declare_parameter("max_act2_speed", 1.0); + this->declare_parameter("max_elbow_yaw_speed", 1.0); } void arm::loadParameters() { this->get_parameter("throttle.axis", kThrottleAxis); @@ -79,13 +83,18 @@ void arm::loadParameters() { this->get_parameter("act1_axis", kAct1Axis); this->get_parameter("act2_axis", kAct2Axis); this->get_parameter("elbow_yaw", kElbowYaw); - this->get_parameter("claw", kclaw); + this->get_parameter("claw", kClaw); this->get_parameter("servo_name", servoName); + this->get_parameter("max_base_speed", kMaxBaseSpeed); + this->get_parameter("max_wrist_roll_speed", kMaxWristRollSpeed); + this->get_parameter("max_wrist_speed", kMaxWristSpeed); + this->get_parameter("max_act1_speed", kMaxAct1Speed); + this->get_parameter("max_act2_speed", kMaxAct2Speed); + this->get_parameter("max_elbow_yaw_speed", kMaxElbowYawSpeed); } int main(int argc, char **argv) { rclcpp::init(argc, argv); - // Replace 'arm' with your actual class name if different auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); diff --git a/src/teleop_modeless/src/drive.cpp b/src/teleop_modeless/src/drive.cpp index 41e00531..09e9c538 100644 --- a/src/teleop_modeless/src/drive.cpp +++ b/src/teleop_modeless/src/drive.cpp @@ -39,7 +39,6 @@ void drive::load_parameters() { int main(int argc, char **argv) { rclcpp::init(argc, argv); - // Replace 'arm' with your actual class name if different auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); From a69f545fbc7c3bad1dbdfff6f9715a16e6ea3e1a Mon Sep 17 00:00:00 2001 From: tomaswilliston Date: Sun, 25 Jan 2026 16:46:01 -0500 Subject: [PATCH 06/23] New joystick control package --- .../joystick_control/CMakeLists.txt | 61 +++--- .../joystick_control}/config/pxn.yaml | 0 .../joystick_control}/include/arm.hpp | 0 .../joystick_control}/include/drive.hpp | 0 .../include/joystick_control/ArmDummyMode.hpp | 134 ------------- .../include/joystick_control/ArmHelpers.hpp | 10 - .../include/joystick_control/ArmIKMode.hpp | 75 -------- .../joystick_control/ArmManualMode.hpp | 125 ------------ .../include/joystick_control/DriveMode.hpp | 128 ------------- .../joystick_control/FlightstickControl.hpp | 131 ------------- .../include/joystick_control/Mode.hpp | 44 ----- .../include/joystick_control/ScienceMode.hpp | 65 ------- .../launch/controller.launch.py | 75 ++++++-- .../joystick_control/package.xml | 10 +- .../joystick_control/src/ArmDummyMode.cpp | 178 ------------------ .../joystick_control/src/ArmIKMode.cpp | 130 ------------- .../joystick_control/src/ArmManualMode.cpp | 177 ----------------- .../joystick_control/src/DriveMode.cpp | 167 ---------------- .../src/FlightstickControl.cpp | 163 ---------------- .../joystick_control/src/ScienceMode.cpp | 137 -------------- .../joystick_control}/src/arm.cpp | 0 .../joystick_control}/src/drive.cpp | 0 src/teleop_modeless/CMakeLists.txt | 71 ------- src/teleop_modeless/include/ArmHelpers.hpp | 10 - .../launch/controller.launch.py | 75 -------- src/teleop_modeless/package.xml | 18 -- src/teleop_modeless/src/ArmHelpers.cpp | 20 -- 27 files changed, 97 insertions(+), 1907 deletions(-) rename src/{teleop_modeless => Teleop-Control/joystick_control}/config/pxn.yaml (100%) rename src/{teleop_modeless => Teleop-Control/joystick_control}/include/arm.hpp (100%) rename src/{teleop_modeless => Teleop-Control/joystick_control}/include/drive.hpp (100%) delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp delete mode 100644 src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp delete mode 100644 src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp delete mode 100644 src/Teleop-Control/joystick_control/src/ArmIKMode.cpp delete mode 100644 src/Teleop-Control/joystick_control/src/ArmManualMode.cpp delete mode 100644 src/Teleop-Control/joystick_control/src/DriveMode.cpp delete mode 100644 src/Teleop-Control/joystick_control/src/FlightstickControl.cpp delete mode 100644 src/Teleop-Control/joystick_control/src/ScienceMode.cpp rename src/{teleop_modeless => Teleop-Control/joystick_control}/src/arm.cpp (100%) rename src/{teleop_modeless => Teleop-Control/joystick_control}/src/drive.cpp (100%) delete mode 100644 src/teleop_modeless/CMakeLists.txt delete mode 100644 src/teleop_modeless/include/ArmHelpers.hpp delete mode 100644 src/teleop_modeless/launch/controller.launch.py delete mode 100644 src/teleop_modeless/package.xml delete mode 100644 src/teleop_modeless/src/ArmHelpers.cpp diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 70be6c21..c31c639c 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -1,61 +1,70 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(joystick_control) -# Find the required packages +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(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) find_package(joy REQUIRED) find_package(ros_phoenix REQUIRED) find_package(interfaces REQUIRED) find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(controller_manager_msgs REQUIRED) -# Specify the C++ standard set(CMAKE_CXX_STANDARD 14) -# Include directories for ROS 2 -include_directories(include/${PROJECT_NAME}) - -# Declare the executables for the nodes -add_executable(flightstick_control - src/FlightstickControl.cpp - src/DriveMode.cpp - src/ArmManualMode.cpp - src/ArmIKMode.cpp - src/ArmHelpers.cpp - src/ArmDummyMode.cpp - src/ScienceMode.cpp) - -# Link the dependencies to the executable -ament_target_dependencies(flightstick_control +include_directories(include) + + +add_executable(drive + src/drive.cpp +) + +add_executable(arm + src/arm.cpp +) + +ament_target_dependencies(drive + rclcpp + geometry_msgs + joy + ros_phoenix + interfaces + control_msgs + std_srvs +) + +ament_target_dependencies(arm rclcpp geometry_msgs - nav_msgs joy ros_phoenix interfaces control_msgs std_srvs - controller_manager_msgs ) -# Install the executable install(TARGETS -flightstick_control + drive + arm DESTINATION lib/${PROJECT_NAME}) # Install header files install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) +#install config files +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME} +) + # Install launch files install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories("include/${PROJECT_NAME}") - ament_package() diff --git a/src/teleop_modeless/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml similarity index 100% rename from src/teleop_modeless/config/pxn.yaml rename to src/Teleop-Control/joystick_control/config/pxn.yaml diff --git a/src/teleop_modeless/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp similarity index 100% rename from src/teleop_modeless/include/arm.hpp rename to src/Teleop-Control/joystick_control/include/arm.hpp diff --git a/src/teleop_modeless/include/drive.hpp b/src/Teleop-Control/joystick_control/include/drive.hpp similarity index 100% rename from src/teleop_modeless/include/drive.hpp rename to src/Teleop-Control/joystick_control/include/drive.hpp diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp deleted file mode 100644 index 1fe160e2..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp +++ /dev/null @@ -1,134 +0,0 @@ -#ifndef JOYSTICK_CONTROL__ARMDUMMY_MODE_HPP_ -#define JOYSTICK_CONTROL__ARMDUMMY_MODE_HPP_ - -#include - -#include "Mode.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "interfaces/srv/move_servo.hpp" -#include "rclcpp/rclcpp.hpp" -#include "ros_phoenix/msg/motor_control.hpp" -#include "std_msgs/msg/float32.hpp" - -/** - * @class ArmDummyMode - * @brief A class that handles the manual arm control using a joystick - * - * This class inherits from the Mode class and processes joystick input to - * control the arm manually, meaning each join is controlled individually. - */ -class ArmDummyMode : public Mode { -public: - /** - * @brief Constructor to the ArmDummyMode class. - * - * @param node A pointer to the rclcpp::Node instance. - */ - ArmDummyMode(rclcpp::Node *node); - - /** - * @brief Processes the joystick input. - * - * @param joystickMsg A shared pointer to the incoming sensor_msgs::msg::Joy - * message. - */ - void processJoystickInput( - std::shared_ptr joystickMsg) override; - - static void declareParameters(rclcpp::Node *node); - - /** - * @brief Sends a request to move a servo - * @param port The servo port number - * @param pos The target position - * @param min The minimum position limit of the servo - * @param max The maximum position limit of the servo - * @return The service response - */ - interfaces::srv::MoveServo::Response sendRequest(int port, int pos, int min, - int max) const; - - /** - * @brief Wrapper for servo control - * @param position The target position - */ - void setServoPosition(double position) const; - -private: - /** - * @brief creates and publishes the MotorControl msg based on joystickinput. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handleTwist(std::shared_ptr joystickMsg) const; - - /** - * @brief Gets the throttle value from the joystick input. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - * @return The throttle value as a double between 0 and one inclusive. - */ - double - getThrottleValue(std::shared_ptr joystickMsg) const; - - void loadParameters(); - - // Parameters - int8_t kBaseAxis; ///< Axis for movement of the base - int8_t kWristRoll; ///< Axis for the roll of the wrist joint - int8_t kWristYawPositive; ///< Button for the wrist join to rotate in the - ///< positive direction - int8_t kWristYawNegative; ///< Button for the wrist join to rotate in the - ///< negative direction - int8_t kAct1Axis; ///< Axis for the upper linear actuator control - int8_t kAct2Axis; ///< Axis for the lower linear actuator control - int8_t kElbowYaw; ///< Axis for the yaw of the elbow joint - - int8_t kThrottleAxis; ///< Axis for the throttle value - double kThrottleMax; ///< Maximum throttle value from joystick. - double kThrottleMin; ///< Minimum throttle value from joystick. - - int8_t kClawOpen; ///< Button for claw to open. - int8_t kClawClose; ///< Button for claw to close. - - int8_t kSimpleForward; ///< Button to move the end effector forward - int8_t kSimpleBackward; ///< Button to move the end effector backward - - // Publishers - rclcpp::Publisher::SharedPtr base_pub_; - rclcpp::Publisher::SharedPtr act1_pub_; - rclcpp::Publisher::SharedPtr act2_pub_; - rclcpp::Publisher::SharedPtr elbow_pub_; - rclcpp::Publisher::SharedPtr wristTilt_pub_; - rclcpp::Publisher::SharedPtr wristTurn_pub_; - - rclcpp::Publisher::SharedPtr - twist_pub_; ///< Publisher for Twist messages. - - rclcpp::Publisher::SharedPtr - servo_pub_; ///< Publisher for servo - - // MotorControl Messages - mutable ros_phoenix::msg::MotorControl base_; - mutable ros_phoenix::msg::MotorControl act1_; - mutable ros_phoenix::msg::MotorControl act2_; - mutable ros_phoenix::msg::MotorControl elbow_; - mutable ros_phoenix::msg::MotorControl wristTilt_; - mutable ros_phoenix::msg::MotorControl wristTurn_; - - // Servo Constants - std::string servoName; - double kServoMin; - double kServoMax; - double kClawMax; - double kClawMin; - mutable double act1Scaler; - mutable double act2Scaler; - mutable double servoPos; - mutable bool buttonPressed; - - // constant for radians - const double rad_multiplier = M_PI / 180; -}; - -#endif // JOYSTICK_CONTROL__ARMDUMMY_MODE_HPP_ \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp deleted file mode 100644 index 770ee3f9..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef JOYSTICK_CONTROL__ARM_HELPERS_HPP_ -#define JOYSTICK_CONTROL__ARM_HELPERS_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace ArmHelpers { -bool start_moveit_servo(rclcpp::Node *node, int attempts = 3); -} - -#endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp deleted file mode 100644 index ccd53d09..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef JOYSTICK_CONTROL__ARMIK_MODE_HPP_ -#define JOYSTICK_CONTROL__ARMIK_MODE_HPP_ - -#include -#include - -#include "Mode.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "interfaces/srv/move_servo.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float32.hpp" - -const std::string CAM_FRAME_ID = "Link_6"; -const std::string BASE_FRAME_ID = "Link_2"; - -class ArmIKMode : public Mode { -public: - ArmIKMode(rclcpp::Node *node); - - void processJoystickInput( - std::shared_ptr joystickMsg) override; - - void handleTwist(std::shared_ptr joystickMsg); - void handleGripper(std::shared_ptr joystickMsg); - - static void declareParameters(rclcpp::Node *node); - void loadParameters(); - - /** - * @brief Sends a request to move a servo - * @param port The servo port number - * @param pos The target position - * @return The service response - */ - interfaces::srv::MoveServo::Response sendRequest(int port, int pos) const; - - /** - * @brief Wrapper for servo control - * @param position The target position - */ - void setServoPosition(double position) const; - -private: - // Servo Members - rclcpp::Client::SharedPtr servo_client_; - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr servo_pub_; - - // Servo Constants - double kServoMin; - double kServoMax; - double kClawMax; - double kClawMin; - int8_t kxAxis; - int8_t kyAxis; - int8_t kUpBut; - int8_t kDownBut; - int8_t kAroundX; - int8_t kAroundY; - int8_t kAroundZ; - int8_t kBase; - int8_t kEEF; - int8_t kClawOpen; - int8_t kClawClose; - double servoPos_; - bool buttonPressed_; - bool swapButton_; - std::string servoName; - std::string frame_to_publish_; - - // constant for radians - const double rad_multiplier = M_PI / 180; -}; - -#endif // JOYSTICK_CONTROL__ARMIK_MODE_HPP_ \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp deleted file mode 100644 index 99776370..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp +++ /dev/null @@ -1,125 +0,0 @@ -#ifndef JOYSTICK_CONTROL__ARMMANUAL_MODE_HPP_ -#define JOYSTICK_CONTROL__ARMMANUAL_MODE_HPP_ - -#include - -#include "Mode.hpp" -#include "control_msgs/msg/joint_jog.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "interfaces/srv/move_servo.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joy.hpp" -#include "std_msgs/msg/float32.hpp" - -/** - * @class ArmManualMode - * @brief A class that handles the manual arm control using a joystick - * - * This class inherits from the Mode class and processes joystick input to - * control the arm manually, meaning each join is controlled individually. - */ -class ArmManualMode : public Mode { -public: - /** - * @brief Constructor to the ArmManualMode class. - * - * @param node A pointer to the rclcpp::Node instance. - */ - ArmManualMode(rclcpp::Node *node); - - /** - * @brief Processes the joystick input. - * - * @param joystickMsg A shared pointer to the incoming sensor_msgs::msg::Joy - * message. - */ - void processJoystickInput( - std::shared_ptr joystickMsg) override; - - static void declareParameters(rclcpp::Node *node); - - /** - * @brief Sends a request to move a servo - * @param port The servo port number - * @param pos The target position - * @param min The minimum position limit of the servo - * @param max The maximum position limit of the servo - * @return The service response - */ - interfaces::srv::MoveServo::Response sendRequest(int port, int pos) const; - - /** - * @brief Wrapper for servo control - * @param req_port The servo port number - * @param req_pos The target position - * @param req_min The minimum position limit - * @param req_max The maximum position limit - */ - void setServoPosition(double position) const; - -private: - /** - * @brief creates and publishes the MotorControl msg based on joystickinput. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handleTwist(std::shared_ptr joystickMsg); - - /** - * @brief Gets the throttle value from the joystick input. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - * @return The throttle value as a double between 0 and one inclusive. - */ - double - getThrottleValue(std::shared_ptr joystickMsg) const; - - void loadParameters(); - - // Parameters - int8_t kBaseAxis; ///< Axis for movement of the base - int8_t kWristRoll; ///< Axis for the roll of the wrist joint - int8_t kWristYawPositive; ///< Button for the wrist join to rotate in the - ///< positive direction - int8_t kWristYawNegative; ///< Button for the wrist join to rotate in the - ///< negative direction - int8_t kAct1Axis; ///< Axis for the upper linear actuator control - int8_t kAct2Axis; ///< Axis for the lower linear actuator control - int8_t kElbowYaw; ///< Axis for the yaw of the elbow joint - - int8_t kThrottleAxis; ///< Axis for the throttle value - double kThrottleMax; ///< Maximum throttle value from joystick. - double kThrottleMin; ///< Minimum throttle value from joystick. - - int8_t kClawOpen; ///< Button for claw to open. - int8_t kClawClose; ///< Button for claw to close. - - int8_t kSimpleForward; ///< Button to move the end effector forward - int8_t kSimpleBackward; ///< Button to move the end effector backward - - // constant for radians - const double rad_multiplier = M_PI / 180; - - // Publishers - rclcpp::Publisher::SharedPtr - joint_pub_; ///< Publisher for joint jog messages. - - rclcpp::Publisher::SharedPtr - servo_pub_; ///< Publisher for servo - - // Message Messages - control_msgs::msg::JointJog joint_msg_; - - // Servo Constants - std::string servoName; - double kServoMin; - double kServoMax; - double kClawMax; - double kClawMin; - double act1Scaler_; - double act2Scaler_; - double servoPos_; - bool buttonPressed_; -}; - -#endif // JOYSTICK_CONTROL__ARMMANUAL_MODE_HPP_ \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp deleted file mode 100644 index d3834f54..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp +++ /dev/null @@ -1,128 +0,0 @@ -#ifndef JOYSTICK_CONTROL__DRIVEMODE_HPP_ -#define JOYSTICK_CONTROL__DRIVEMODE_HPP_ - -#include - -#include "Mode.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "interfaces/srv/move_servo.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float32.hpp" - -/** - * @class DriveMode - * @brief A class that handles the teleop drive mode of the rover using joystick - * input. - * - * This class inherits from the Mode class and processes joystick input to - * control the rover's movement, camera, and video functionalities. - */ -class DriveMode : public Mode { -public: - /** - * @brief Constructor for the DriveMode class. - * - * @param node A pointer to the rclcpp::Node instance. - */ - DriveMode(rclcpp::Node *node); - /** - * @brief Processes the joystick input. - * - * @param joystickMsg A shared pointer to the incoming sensor_msgs::msg::Joy - * message. - */ - void processJoystickInput( - std::shared_ptr joystickMsg) override; - /** - * @brief Declare the parameters for the mode. - * @param node Pointer to the ROS2 node. - */ - static void declareParameters(rclcpp::Node *node); - -private: - /** - * @brief creates and publishes the twist msg based on joystickinput. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handleTwist(std::shared_ptr joystickMsg) const; - - /** - * @brief Handles the camera control based on joystick input. Not implemented - * yet. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handleCam(std::shared_ptr joystickMsg); - - /** - * @brief Handles the video control based on joystick input. Not implemented - * yet. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handleVideo(std::shared_ptr joystickMsg) const; - /** - * @brief Handles the PWM control for lights based on joystick input. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - */ - void handlePWM(std::shared_ptr joystickMsg); - - /** - * @brief Gets the throttle value from the joystick input. - * - * @param joystickMsg A shared pointer to the sensor_msgs::msg::Joy message. - * @return The throttle value as a double between 0 and one inclusive. - */ - double - getThrottleValue(std::shared_ptr joystickMsg) const; - /** - * @brief Sets the position of the servo. - * - * @param name The servo name. - * @param position The target position for the servo. - */ - void setServoPosition(std::string name, double position); - - void loadParameters(); - - // Parameters - int8_t kForwardAxis; ///< Axis for forward movement. - int8_t kYawAxis; ///< Axis for yaw (rotation). - int8_t kCamTiltAxis; ///< Axis for camera tilt. - int8_t kCamPanAxis; ///< Axis for camera pan. - int8_t kCamReset; ///< Button for resetting the camera. - int8_t kLightsUp; ///< Button for switching to the next camera. - int8_t kLightsDown; ///< Button for switching to the previous camera. - int8_t kCruiseControl; ///< Button for enabling cruise control. - int8_t kThrottleAxis; ///< Axis for throttle control. - std::string camTiltMotor; ///< Motor name for camera tilt servo. - std::string camPanMotor; ///< Motor name for camera pan servo. - - double kThrottleMax; ///< Maximum throttle value from joystick. - double kThrottleMin; ///< Minimum throttle value from joystick. - double kMaxLinear; ///< Maximum linear velocity. - double kMaxAngular; ///< Maximum angular velocity. - double kMaxIncrement; ///< Maximum increment for speed changes. - double kMinSpeed; ///< Minimum speed value. - double kDefaultCamPan = M_PI / 4; ///< Default camera pan position. - double kDefaultCamTilt = M_PI / 4; ///< Default camera tilt position. - double kCameraSpeed = 1.0; ///< Speed for camera movement. - - double current_light_pwm_; ///< Current PWM value for lights. - - bool camera_service_available_; - - std::vector motor_names; ///< Vector for motor names. - - rclcpp::Publisher::SharedPtr - twist_pub_; ///< Publisher for Twist messages. - rclcpp::Publisher::SharedPtr - pwm_pub_; ///< Publisher for video messages. - - std::map::SharedPtr> - motor_pubs; ///< Map that maps motor names to publishers -}; - -#endif // JOYSTICK_CONTROL__DRIVEMODE_HPP_ \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp deleted file mode 100644 index 20eaee61..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp +++ /dev/null @@ -1,131 +0,0 @@ -#ifndef FLIGHTSTICK_CONTROL_HPP -#define FLIGHTSTICK_CONTROL_HPP - -#include - -#include "ArmDummyMode.hpp" -#include "ArmIKMode.hpp" -#include "ArmManualMode.hpp" -#include "DriveMode.hpp" -#include "ScienceMode.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joy.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int8.hpp" -#include "std_msgs/msg/string.hpp" - -/** - * @class FlightstickControl - * @brief A class to handle joystick inputs and control modes for the rover. - * - * The FlightstickControl class subscribes to joystick messages and processes - * them to control different modes of the rover, such as driving, arm control, - * navigation, and science operations. - */ -class FlightstickControl : public rclcpp::Node { -public: - /** - * @brief Constructor for FlightstickControl. - * - * Initializes the node and sets up the joystick subscription. - */ - FlightstickControl(); - - /** - * @brief Processes joystick messages. - * - * This function is called whenever a new joystick message is received. It - * processes the joystick inputs and performs actions based on the current - * mode. - * - * @param joystickMsg A shared pointer to the received joystick message. - */ - void processJoystick(std::shared_ptr joystickMsg); - -private: - /* - * @brief Declares parameters for the node and modes. - */ - void declareParameters(); - /** - * @brief Loads parameters from the parameter server. - * - * This function loads configuration parameters such as button mappings from - * the parameter server. - */ - void loadParameters(); - - /** - * @enum ModeType - * @brief Enumeration of the different control modes. - * - * This enumeration defines the different control modes that the rover can be - * in. - */ - enum class ModeType { - NONE = 0, - DRIVE, - ARM_IK, - ARM_MANUAL, - NAV, - SCIENCE, - ARM_DUMMY - }; - - uint8_t kDriveModeButton; ///< Button index for drive mode. - uint8_t kArmIKModeButton; ///< Button index for inverse kinematics arm - ///< control mode. - uint8_t kArmManualModeButton; ///< Button index for manual arm control mode. - uint8_t kNavModeButton; ///< Button index for navigation mode. - uint8_t kScienceModeButton; ///< Button index for science mode. - uint8_t kArmDummyButton; - - uint8_t kTeleopLightMode; ///< Button index for teleoperation light mode. - - ModeType currentMode_; ///< The current control mode of the rover. - - /** - * @brief Checks for mode change based on joystick input. - * - * This function checks if a mode change is requested based on the joystick - * input. - * - * @param joystickMsg A shared pointer to the received joystick message. - * @return True if a mode change is requested, false otherwise. - */ - bool checkForModeChange(std::shared_ptr joystickMsg); - - /** - * @brief Checks all axis and button values - * - * This function checks if all relevant axis and button values are 0 - * - * @param joystickMsg A shared pointer to the received joystick message. - * @param nextMode The mode to check against. - * @return True if all axes (except slider) are 0, false otherwise - */ - bool checkAxes(std::shared_ptr joystickMsg, - ModeType nextMode); - - /** - * @brief Changes the control mode. - * - * This function changes the control mode of the rover to the specified mode. - * - * @param mode The new control mode to switch to. - * @return True if the mode change was successful, false otherwise. - */ - bool changeMode(ModeType mode); - - rclcpp::Subscription::SharedPtr - joy_sub_; ///< Subscription to joystick messages. - rclcpp::Publisher::SharedPtr - status_pub_; ///< Publisher for status messages. - rclcpp::Publisher::SharedPtr - light_pub_; ///< Publisher for light control messages. - - std::unique_ptr mode_; ///< Pointer to the current mode object. -}; - -#endif // FLIGHTSTICK_CONTROL_HPP diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp deleted file mode 100644 index 09513156..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file Mode.hpp - * @brief Defines the Mode class for handling joystick input in different modes. - */ - -#ifndef MODE_HPP -#define MODE_HPP - -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joy.hpp" - -/** - * @class Mode - * @brief Abstract base class for different joystick control modes. - */ -class Mode { -public: - /** - * @brief Constructor for the Mode class. - * @param name The name of the mode. - * @param node Pointer to the ROS2 node. - */ - Mode(std::string name, rclcpp::Node *node) : name_(name), node_(node) {} - virtual ~Mode() = default; - - /** - * @brief Pure virtual function to process joystick input. - * @param joystickMsg Shared pointer to the joystick message. - */ - virtual void - processJoystickInput(std::shared_ptr joystickMsg) = 0; - - /** - * @brief Get the name of the mode. - * @return The name of the mode. - */ - std::string getName() { return name_; } - -protected: - std::string name_; ///< The name of the mode. - rclcpp::Node *node_; ///< Pointer to the ROS2 node. -}; - -#endif // MODE_HPP \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp deleted file mode 100644 index 30dd2772..00000000 --- a/src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef JOYSTICK_CONTROL__SCIENCEMODE_HPP_ -#define JOYSTICK_CONTROL__SCIENCEMODE_HPP_ - -#include - -#include "Mode.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "interfaces/srv/move_servo.hpp" -#include "rclcpp/rclcpp.hpp" -#include "ros_phoenix/msg/motor_control.hpp" -#include "ros_phoenix/msg/motor_status.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_srvs/srv/set_bool.hpp" - -class ScienceMode : public Mode { -public: - ScienceMode(rclcpp::Node *node); - - void processJoystickInput( - std::shared_ptr joystickMsg) override; - - static void declareParameters(rclcpp::Node *node); - -private: - void handlePlatform(std::shared_ptr joystickMsg) const; - void handleDrill(std::shared_ptr joystickMsg) const; - void handleMicroscope(std::shared_ptr joystickMsg); - void - handlePanoramic(std::shared_ptr joystickMsg) const; - void handleSoilCollection(std::shared_ptr joystickMsg); - - void setServoPosition(std::string name, double position); - void toggleLights() const; - - void loadParameters(); - - // constant for radians - const double rad_multiplier = M_PI / 180; - - int8_t kPlatformAxis; - int8_t kDrillButton; - int8_t kMicroscopeAxis; - int8_t kThrottleAxis; - int8_t kCollectionButton; - int8_t kCancelCollectionButton; - int8_t kSoilTestButton; - int8_t kSoilLockButton; - int8_t kMicroscopeLightButton; - double kCollectionSample; - double kCollectionOpen; - double kCollectionClose; - double kCollectionLock; - std::string collectionServo; - std::string microscopeServo; - - rclcpp::Publisher::SharedPtr platform_pub_; - rclcpp::Publisher::SharedPtr drill_pub_; - rclcpp::Client::SharedPtr led_client_; - - // Map that maps motor names to publishers - std::map::SharedPtr> - motor_pubs; -}; - -#endif diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 1604716f..c8e2f7a1 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -1,30 +1,75 @@ 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 -# Make launch configeration, defult false, launch talon on rover. Defult True for xbox +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(): - """Generate launch description with multiple components.""" - pkg_drive = get_package_share_directory("drive_cpp") - parameters_file = os.path.join(pkg_drive, "config", "pxn.yaml") + 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 launch.LaunchDescription( + return LaunchDescription( [ - launch_ros.actions.Node( + 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", "/arm/joy")], + ), + Node( package="joystick_control", - executable="flightstick_control", - name="flightstick_control", - output="screen", + executable="arm", + name="arm_teleop_node", parameters=[parameters_file], - arguments=["--ros-args", "--log-level", "INFO"], ), - launch_ros.actions.Node( - package="gpio_controller", - executable="lights", - name="lights", - output="screen", + Node( + package="joystick_control", + executable="drive", + name="drive_teleop_node", + parameters=[parameters_file], ), ] ) diff --git a/src/Teleop-Control/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml index dd1877cd..30e8d407 100644 --- a/src/Teleop-Control/joystick_control/package.xml +++ b/src/Teleop-Control/joystick_control/package.xml @@ -4,17 +4,11 @@ joystick_control 0.0.0 TODO: Package description - connor + vscode TODO: License declaration ament_cmake - rclcpp - std_msgs - geometry_msgs - sensor_msgs - ros_phoenix - interfaces - joy + ament_lint_auto ament_lint_common diff --git a/src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp b/src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp deleted file mode 100644 index 18791514..00000000 --- a/src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp +++ /dev/null @@ -1,178 +0,0 @@ -#include "ArmDummyMode.hpp" - -#include "controller_manager_msgs/srv/switch_controller.hpp" -#include "std_msgs/msg/bool.hpp" - -ArmDummyMode::ArmDummyMode(rclcpp::Node *node) : Mode("Dummy Arm", node) { - RCLCPP_INFO(node_->get_logger(), "Arm Dumb Mode"); - loadParameters(); - twist_pub_ = - node_->create_publisher("/cmd_vel", 10); - base_pub_ = - node_->create_publisher("/base/set", 10); - act1_pub_ = - node_->create_publisher("/act1/set", 10); - act2_pub_ = - node_->create_publisher("/act2/set", 10); - elbow_pub_ = - node_->create_publisher("/elbow/set", 10); - wristTilt_pub_ = node_->create_publisher( - "/wristTilt/set", 10); - wristTurn_pub_ = node_->create_publisher( - "/wristTurn/set", 10); - servo_pub_ = node_->create_publisher( - "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - auto stop_hw_interface_pub = - node_->create_publisher("/arm_active", 10); - auto msg = std_msgs::msg::Bool(); - msg.data = false; - stop_hw_interface_pub->publish(msg); - - kServoMin = 0.0; - kServoMax = M_PI; - kClawMax = 63 * rad_multiplier; - kClawMin = 8 * rad_multiplier; - servoPos = kClawMax; - buttonPressed = false; -} - -double ArmDummyMode::getThrottleValue( - std::shared_ptr joystickMsg) const { - if (kThrottleAxis != -1) { - double throttle = joystickMsg->axes[kThrottleAxis]; - throttle = std::max(kThrottleMin, std::min(kThrottleMax, throttle)); - // Normalize the throttle value to be between 0.2 and 1.0 - return 0.2 + - ((throttle - kThrottleMin) / (kThrottleMax - kThrottleMin) * 0.80); - } - return 1.0; -} - -void ArmDummyMode::processJoystickInput( - std::shared_ptr joystickMsg) { - handleTwist(joystickMsg); -} - -void ArmDummyMode::handleTwist( - std::shared_ptr joystickMsg) const { - double throttle = getThrottleValue(joystickMsg); - - base_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - act1_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - act2_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - elbow_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - wristTilt_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - wristTurn_.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - act1Scaler = 0.75; - act2Scaler = 0.80; - - // Base (might want a deadzone. TBD) - base_.value = -joystickMsg->axes[kBaseAxis] * throttle; - - // Simple straight movement (NOT inverse kin) - // some scaling to move in a straight line - if (joystickMsg->buttons[kSimpleForward] == 1) { - act1_.value = -joystickMsg->buttons[kSimpleForward] * act1Scaler * throttle; - act2_.value = -joystickMsg->buttons[kSimpleForward] * throttle; - } else if (joystickMsg->buttons[kSimpleBackward] == 1) { - act1_.value = joystickMsg->buttons[kSimpleBackward] * act1Scaler * throttle; - act2_.value = joystickMsg->buttons[kSimpleBackward] * act2Scaler * throttle; - } else { - // act1 - act1_.value = -joystickMsg->axes[kAct1Axis] * throttle; - - // act2 - act2_.value = -joystickMsg->axes[kAct2Axis] * throttle; - } - - // Elbow - // Deadzone, easy to turn this one by accident. - if (joystickMsg->axes[kElbowYaw] < 0.2 && - joystickMsg->axes[kElbowYaw] > -0.2) { - elbow_.value = 0; - } else { - elbow_.value = joystickMsg->axes[kElbowYaw] * throttle; - } - - // Wrist Tilt - wristTilt_.value = (joystickMsg->buttons[kWristYawPositive] - - joystickMsg->buttons[kWristYawNegative]) * - throttle; - - // Wrist Turn - wristTurn_.value = -joystickMsg->axes[kWristRoll] * throttle; - // Gripper. Will cycle between open, half open, and close on button release. - if (joystickMsg->buttons[kClawOpen] == 1 && !buttonPressed) { - if (servoPos + ((kClawMax - kClawMin) / 2) < kClawMax + rad_multiplier) { - buttonPressed = true; - servoPos = servoPos + ((kClawMax - kClawMin) / 2); - setServoPosition(servoPos); - } else { - buttonPressed = true; - RCLCPP_INFO(node_->get_logger(), "Max Open"); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos); - } - } else if (joystickMsg->buttons[kClawClose] == 1 && !buttonPressed) { - if (servoPos - ((kClawMax - kClawMin) / 2) > kClawMin - rad_multiplier) { - buttonPressed = true; - servoPos = servoPos - ((kClawMax - kClawMin) / 2); - setServoPosition(servoPos); - } else { - buttonPressed = true; - RCLCPP_INFO(node_->get_logger(), "Max Close"); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos); - } - } else if ((joystickMsg->buttons[kClawClose] == 0) && - (joystickMsg->buttons[kClawOpen] == 0)) { - buttonPressed = false; - } - - base_pub_->publish(base_); - act1_pub_->publish(act1_); - act2_pub_->publish(act2_); - elbow_pub_->publish(elbow_); - wristTilt_pub_->publish(wristTilt_); - wristTurn_pub_->publish(wristTurn_); -} - -void ArmDummyMode::declareParameters(rclcpp::Node *node) { - node->declare_parameter("arm_manual_mode.base_axis", 0); - node->declare_parameter("arm_manual_mode.wrist_roll", 1); - node->declare_parameter("arm_manual_mode.wrist_yaw_positive", 2); - node->declare_parameter("arm_manual_mode.wrist_yaw_negative", 3); - node->declare_parameter("arm_manual_mode.act1_axis", 4); - node->declare_parameter("arm_manual_mode.act2_axis", 5); - node->declare_parameter("arm_manual_mode.elbow_yaw", 6); - node->declare_parameter("arm_manual_mode.claw_open", 8); - node->declare_parameter("arm_manual_mode.claw_close", 9); - node->declare_parameter("arm_manual_mode.simple_forward", 10); - node->declare_parameter("arm_manual_mode.simple_backward", 11); - node->declare_parameter("arm_manual_mode.servo_name", "manual"); - node->declare_parameter("arm_manual_mode.throttle.axis", 7); - node->declare_parameter("arm_manual_mode.throttle.min", -1.0); - node->declare_parameter("arm_manual_mode.throttle.max", 1.0); -} - -void ArmDummyMode::loadParameters() { - node_->get_parameter("arm_manual_mode.base_axis", kBaseAxis); - node_->get_parameter("arm_manual_mode.wrist_roll", kWristRoll); - node_->get_parameter("arm_manual_mode.wrist_yaw_positive", kWristYawPositive); - node_->get_parameter("arm_manual_mode.wrist_yaw_negative", kWristYawNegative); - node_->get_parameter("arm_manual_mode.act1_axis", kAct1Axis); - node_->get_parameter("arm_manual_mode.act2_axis", kAct2Axis); - node_->get_parameter("arm_manual_mode.elbow_yaw", kElbowYaw); - node_->get_parameter("arm_manual_mode.claw_open", kClawOpen); - node_->get_parameter("arm_manual_mode.claw_close", kClawClose); - node_->get_parameter("arm_manual_mode.simple_forward", kSimpleForward); - node_->get_parameter("arm_manual_mode.simple_backward", kSimpleBackward); - node_->get_parameter("arm_manual_mode.servo_name", servoName); - node_->get_parameter("arm_manual_mode.throttle.axis", kThrottleAxis); - node_->get_parameter("arm_manual_mode.throttle.max", kThrottleMax); - node_->get_parameter("arm_manual_mode.throttle.min", kThrottleMin); -} - -void ArmDummyMode::setServoPosition(double position) const { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = position; - servo_pub_->publish(servo_msg); -} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/ArmIKMode.cpp b/src/Teleop-Control/joystick_control/src/ArmIKMode.cpp deleted file mode 100644 index 7bfc7c38..00000000 --- a/src/Teleop-Control/joystick_control/src/ArmIKMode.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "ArmIKMode.hpp" - -#include "ArmHelpers.hpp" -#include "std_msgs/msg/bool.hpp" - -ArmIKMode::ArmIKMode(rclcpp::Node *node) : Mode("IK Arm", node) { - RCLCPP_INFO(node_->get_logger(), "IK Arm Mode"); - loadParameters(); - twist_pub_ = node_->create_publisher( - "/servo_node/delta_twist_cmds", 10); - if (!ArmHelpers::start_moveit_servo(node_)) { - RCLCPP_ERROR(node_->get_logger(), - "Failed to start MoveIt servo service, IK mode will not work"); - } - - auto stop_hw_interface_pub = - node_->create_publisher("/arm_active", 10); - servo_pub_ = node_->create_publisher( - "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - - auto msg = std_msgs::msg::Bool(); - msg.data = true; - stop_hw_interface_pub->publish(msg); - - frame_to_publish_ = CAM_FRAME_ID; - kServoMin = 0.0; - kServoMax = M_PI; - kClawMax = 63 * rad_multiplier; - kClawMin = 8 * rad_multiplier; - servoPos_ = kClawMax; - buttonPressed_ = false; - swapButton_ = false; -} - -void ArmIKMode::processJoystickInput( - std::shared_ptr joystickMsg) { - handleTwist(joystickMsg); - handleGripper(joystickMsg); -} - -void ArmIKMode::handleTwist( - std::shared_ptr joystickMsg) { - geometry_msgs::msg::TwistStamped twist_msg; - twist_msg.header.stamp = node_->now(); - twist_msg.header.frame_id = frame_to_publish_; - - twist_msg.twist.linear.x = joystickMsg->axes[kxAxis]; - twist_msg.twist.linear.y = joystickMsg->axes[kyAxis]; - twist_msg.twist.linear.z = - joystickMsg->buttons[kUpBut] - joystickMsg->buttons[kDownBut]; - twist_msg.twist.angular.x = joystickMsg->axes[kAroundX]; - twist_msg.twist.angular.y = joystickMsg->axes[kAroundY]; - twist_msg.twist.angular.z = joystickMsg->axes[kAroundZ]; - - if (joystickMsg->buttons[kBase] == 1 && !swapButton_) { - frame_to_publish_ = BASE_FRAME_ID; - swapButton_ = true; - } else if (joystickMsg->buttons[kEEF] == 1 && !swapButton_) { - frame_to_publish_ = CAM_FRAME_ID; - swapButton_ = true; - } else if (!joystickMsg->buttons[kEEF] == 1 && - !joystickMsg->buttons[kBase] == 1) { - swapButton_ = false; - } - twist_pub_->publish(twist_msg); -} -void ArmIKMode::handleGripper( - std::shared_ptr joystickMsg) { - // Gripper. Will cycle between open, half open, and close on button release. - if (joystickMsg->buttons[kClawOpen] == 1 && !buttonPressed_) { - if (servoPos_ + ((kClawMax - kClawMin) / 2) < kClawMax + rad_multiplier) { - buttonPressed_ = true; - servoPos_ = servoPos_ + ((kClawMax - kClawMin) / 2); - setServoPosition(servoPos_); - } else { - buttonPressed_ = true; - RCLCPP_INFO(node_->get_logger(), "Max Open"); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos_); - } - } else if (joystickMsg->buttons[kClawClose] == 1 && !buttonPressed_) { - if (servoPos_ - ((kClawMax - kClawMin) / 2) > kClawMin - rad_multiplier) { - buttonPressed_ = true; - servoPos_ = servoPos_ - ((kClawMax - kClawMin) / 2); - setServoPosition(servoPos_); - } else { - buttonPressed_ = true; - RCLCPP_INFO(node_->get_logger(), "Max Close"); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos_); - } - } else if ((joystickMsg->buttons[kClawClose] == 0) && - (joystickMsg->buttons[kClawOpen] == 0)) { - buttonPressed_ = false; - } -} - -void ArmIKMode::declareParameters(rclcpp::Node *node) { - node->declare_parameter("arm_ik_mode.x_axis", 0); - node->declare_parameter("arm_ik_mode.y_axis", 1); - node->declare_parameter("arm_ik_mode.up_button", 2); - node->declare_parameter("arm_ik_mode.down_button", 3); - node->declare_parameter("arm_ik_mode.rotate_around_y", 4); - node->declare_parameter("arm_ik_mode.rotate_around_x", 5); - node->declare_parameter("arm_ik_mode.rotate_around_z", 6); - node->declare_parameter("arm_ik_mode.open_claw", 7); - node->declare_parameter("arm_ik_mode.close_claw", 8); - node->declare_parameter("arm_ik_mode.base_frame", 9); - node->declare_parameter("arm_ik_mode.eef_frame", 10); - node->declare_parameter("arm_ik_mode.servo_name", "arm"); -} - -void ArmIKMode::loadParameters() { - node_->get_parameter("arm_ik_mode.x_axis", kxAxis); - node_->get_parameter("arm_ik_mode.y_axis", kyAxis); - node_->get_parameter("arm_ik_mode.up_button", kUpBut); - node_->get_parameter("arm_ik_mode.down_button", kDownBut); - node_->get_parameter("arm_ik_mode.rotate_around_y", kAroundY); - node_->get_parameter("arm_ik_mode.rotate_around_x", kAroundX); - node_->get_parameter("arm_ik_mode.rotate_around_z", kAroundZ); - node_->get_parameter("arm_ik_mode.open_claw", kClawOpen); - node_->get_parameter("arm_ik_mode.close_claw", kClawClose); - node_->get_parameter("arm_ik_mode.base_frame", kBase); - node_->get_parameter("arm_ik_mode.eef_frame", kEEF); - node_->get_parameter("arm_ik_mode.servo_name", servoName); -} - -void ArmIKMode::setServoPosition(double position) const { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = position; - servo_pub_->publish(servo_msg); -} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/ArmManualMode.cpp b/src/Teleop-Control/joystick_control/src/ArmManualMode.cpp deleted file mode 100644 index a8a2a98f..00000000 --- a/src/Teleop-Control/joystick_control/src/ArmManualMode.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#include "ArmManualMode.hpp" - -#include "ArmHelpers.hpp" -#include "control_msgs/msg/joint_jog.hpp" -#include "std_msgs/msg/bool.hpp" - -ArmManualMode::ArmManualMode(rclcpp::Node *node) : Mode("Manual Arm", node) { - RCLCPP_INFO(node_->get_logger(), "Arm Manual Mode"); - loadParameters(); - joint_pub_ = node_->create_publisher( - "/servo_node/delta_joint_cmds", 10); - if (!ArmHelpers::start_moveit_servo(node_)) { - RCLCPP_ERROR( - node_->get_logger(), - "Failed to start MoveIt servo service, Manual mode will not work"); - } - servo_pub_ = node_->create_publisher( - "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - - auto stop_hw_interface_pub = - node_->create_publisher("/arm_active", 10); - auto msg = std_msgs::msg::Bool(); - msg.data = true; - stop_hw_interface_pub->publish(msg); - - kServoMin = 0.0; - kServoMax = M_PI; - kClawMax = 63 * rad_multiplier; - kClawMin = 8 * rad_multiplier; - servoPos_ = kClawMax; - buttonPressed_ = false; - joint_msg_ = control_msgs::msg::JointJog(); - joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", - "Joint_4", "Joint_5", "Joint_6"}; - joint_msg_.velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; -} - -double ArmManualMode::getThrottleValue( - std::shared_ptr joystickMsg) const { - if (kThrottleAxis != -1) { - double throttle = joystickMsg->axes[kThrottleAxis]; - throttle = std::max(kThrottleMin, std::min(kThrottleMax, throttle)); - // Normalize the throttle value to be between 0.2 and 1.0 - return 0.2 + - ((throttle - kThrottleMin) / (kThrottleMax - kThrottleMin) * 0.80); - } - return 1.0; -} - -void ArmManualMode::processJoystickInput( - std::shared_ptr joystickMsg) { - handleTwist(joystickMsg); -} - -void ArmManualMode::handleTwist( - std::shared_ptr joystickMsg) { - double throttle = getThrottleValue(joystickMsg); - joint_msg_.header = joystickMsg->header; - - // Base (might want a deadzone. TBD) - joint_msg_.velocities[0] = -joystickMsg->axes[kBaseAxis] * throttle; - - // Simple straight movement (NOT inverse kin) - // some scaling to move in a straight line - if (joystickMsg->buttons[kSimpleForward] == 1) { - joint_msg_.velocities[1] = -0.78 * throttle; - joint_msg_.velocities[2] = 0.92 * throttle; - joint_msg_.velocities[4] = -1.0 * throttle; - } else if (joystickMsg->buttons[kSimpleBackward] == 1) { - joint_msg_.velocities[1] = 0.90 * throttle; - joint_msg_.velocities[2] = -0.80 * throttle; - joint_msg_.velocities[4] = 1.0 * throttle; - } else { - // act1 - if (joystickMsg->axes[kAct1Axis] > 0.1 || - joystickMsg->axes[kAct1Axis] < -0.1) { - joint_msg_.velocities[1] = -joystickMsg->axes[kAct1Axis] * throttle; - } else { - joint_msg_.velocities[1] = 0; - } - - // act2 - joint_msg_.velocities[2] = joystickMsg->axes[kAct2Axis] * throttle; - } - - // Elbow - // Deadzone, easy to turn this one by accident. - if (joystickMsg->axes[kElbowYaw] < 0.2 && - joystickMsg->axes[kElbowYaw] > -0.2) { - joint_msg_.velocities[3] = 0; - } else { - joint_msg_.velocities[3] = joystickMsg->axes[kElbowYaw] * throttle; - } - - // Wrist Tilt - if (joystickMsg->buttons[kSimpleForward] == 0 && - joystickMsg->buttons[kSimpleForward] == 0) { - joint_msg_.velocities[4] = (joystickMsg->buttons[kWristYawPositive] - - joystickMsg->buttons[kWristYawNegative]) * - throttle; - } - - // Wrist Turn - joint_msg_.velocities[5] = joystickMsg->axes[kWristRoll] * throttle; - - // Gripper. Will cycle between open, half open, and close on button release. - if (joystickMsg->buttons[kClawOpen] == 1 && !buttonPressed_) { - if (servoPos_ + ((kClawMax - kClawMin) / 2) < kClawMax + rad_multiplier) { - buttonPressed_ = true; - servoPos_ = servoPos_ + ((kClawMax - kClawMin) / 2); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos_); - setServoPosition(servoPos_); - } else { - buttonPressed_ = true; - RCLCPP_INFO(node_->get_logger(), "Max Open"); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos_); - } - } else if (joystickMsg->buttons[kClawClose] == 1 && !buttonPressed_) { - if (servoPos_ - ((kClawMax - kClawMin) / 2) > kClawMin - rad_multiplier) { - buttonPressed_ = true; - servoPos_ = servoPos_ - ((kClawMax - kClawMin) / 2); - RCLCPP_INFO(node_->get_logger(), "Position: %f", servoPos_); - setServoPosition(servoPos_); - } else { - buttonPressed_ = true; - RCLCPP_INFO(node_->get_logger(), "Max Close"); - RCLCPP_INFO(node_->get_logger(), "%f", servoPos_); - } - } else if ((joystickMsg->buttons[kClawClose] == 0) && - (joystickMsg->buttons[kClawOpen] == 0)) { - buttonPressed_ = false; - } - - joint_pub_->publish(joint_msg_); -} - -void ArmManualMode::declareParameters(rclcpp::Node *node) { - node->declare_parameter("arm_manual_mode.base_axis", 0); - node->declare_parameter("arm_manual_mode.wrist_roll", 1); - node->declare_parameter("arm_manual_mode.wrist_yaw_positive", 2); - node->declare_parameter("arm_manual_mode.wrist_yaw_negative", 3); - node->declare_parameter("arm_manual_mode.act1_axis", 4); - node->declare_parameter("arm_manual_mode.act2_axis", 5); - node->declare_parameter("arm_manual_mode.elbow_yaw", 6); - node->declare_parameter("arm_manual_mode.claw_open", 8); - node->declare_parameter("arm_manual_mode.claw_close", 9); - node->declare_parameter("arm_manual_mode.simple_forward", 10); - node->declare_parameter("arm_manual_mode.simple_backward", 11); - node->declare_parameter("arm_manual_mode.servo_name", "arm"); - node->declare_parameter("arm_manual_mode.throttle.axis", 7); - node->declare_parameter("arm_manual_mode.throttle.min", -1.0); - node->declare_parameter("arm_manual_mode.throttle.max", 1.0); -} - -void ArmManualMode::loadParameters() { - node_->get_parameter("arm_manual_mode.base_axis", kBaseAxis); - node_->get_parameter("arm_manual_mode.wrist_roll", kWristRoll); - node_->get_parameter("arm_manual_mode.wrist_yaw_positive", kWristYawPositive); - node_->get_parameter("arm_manual_mode.wrist_yaw_negative", kWristYawNegative); - node_->get_parameter("arm_manual_mode.act1_axis", kAct1Axis); - node_->get_parameter("arm_manual_mode.act2_axis", kAct2Axis); - node_->get_parameter("arm_manual_mode.elbow_yaw", kElbowYaw); - node_->get_parameter("arm_manual_mode.claw_open", kClawOpen); - node_->get_parameter("arm_manual_mode.claw_close", kClawClose); - node_->get_parameter("arm_manual_mode.simple_forward", kSimpleForward); - node_->get_parameter("arm_manual_mode.simple_backward", kSimpleBackward); - node_->get_parameter("arm_manual_mode.servo_name", servoName); - node_->get_parameter("arm_manual_mode.throttle.axis", kThrottleAxis); - node_->get_parameter("arm_manual_mode.throttle.max", kThrottleMax); - node_->get_parameter("arm_manual_mode.throttle.min", kThrottleMin); -} - -void ArmManualMode::setServoPosition(double position) const { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = position; - servo_pub_->publish(servo_msg); -} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/DriveMode.cpp b/src/Teleop-Control/joystick_control/src/DriveMode.cpp deleted file mode 100644 index 0af24c19..00000000 --- a/src/Teleop-Control/joystick_control/src/DriveMode.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include "DriveMode.hpp" - -#include - -DriveMode::DriveMode(rclcpp::Node *node) - : Mode("Drive", node), camera_service_available_(false) { - RCLCPP_INFO(node_->get_logger(), "Drive Mode"); - loadParameters(); - std::vector motor_names = {camPanMotor, camTiltMotor}; - twist_pub_ = - node_->create_publisher("/cmd_vel", 10); - - for (const auto &topic : motor_names) { - std::string topic_name = "/" + topic; - motor_pubs[topic] = node_->create_publisher( - topic_name, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - } - pwm_pub_ = - node_->create_publisher("servo_pwm_control", 10); -} - -void DriveMode::processJoystickInput( - std::shared_ptr joystickMsg) { - handleTwist(joystickMsg); - handleCam(joystickMsg); - handleVideo(joystickMsg); - handlePWM(joystickMsg); -} - -double DriveMode::getThrottleValue( - std::shared_ptr joystickMsg) const { - if (kThrottleAxis != -1) { - double throttle = joystickMsg->axes[kThrottleAxis]; - throttle = std::max(kThrottleMin, std::min(kThrottleMax, throttle)); - // Normalize the throttle value to be between 0 and 1 - return (throttle - kThrottleMin) / (kThrottleMax - kThrottleMin); - } - return 1.0; -} - -void DriveMode::handleTwist( - std::shared_ptr joystickMsg) const { - static double last_forward = 0; - double forward, yaw, throttle; - throttle = getThrottleValue(joystickMsg); - if (joystickMsg->buttons[kCruiseControl] == 1) { - forward = last_forward; - } else { - forward = joystickMsg->axes[kForwardAxis] * throttle * kMaxLinear; - } - yaw = joystickMsg->axes[kYawAxis] * kMaxAngular; - if (std::abs(forward) < kMinSpeed) { - forward = 0; - } - if (std::abs(yaw) < kMinSpeed) { - yaw = 0; - } - if (forward >= last_forward) { - forward = std::min(forward, last_forward + kMaxIncrement); - } else { - forward = std::max(forward, last_forward - kMaxIncrement); - } - last_forward = forward; - auto twist = geometry_msgs::msg::Twist(); - twist.linear.x = forward; - twist.angular.z = yaw; - twist_pub_->publish(twist); -} - -void DriveMode::handleCam(std::shared_ptr joystickMsg) { - static double tilt_pos = kDefaultCamTilt; - static double pan_pos = kDefaultCamPan; - static double timestamp = 0; - double tilt = joystickMsg->axes[kCamTiltAxis]; - double pan = joystickMsg->axes[kCamPanAxis]; - tilt_pos += tilt * kCameraSpeed; - pan_pos += pan * kCameraSpeed; - if (joystickMsg->buttons[kCamReset] == 1) { - tilt_pos = kDefaultCamTilt; - pan_pos = kDefaultCamPan; - // return to only send once button is released - return; - } - double current_time = node_->now().seconds(); - if (current_time - timestamp > 0.2) { - pan_pos = std::max(0.0, std::min(2 * M_PI, pan_pos)); - tilt_pos = std::max(0.0, std::min(2 * M_PI, tilt_pos)); - timestamp = current_time; - - setServoPosition(camTiltMotor, tilt_pos); - setServoPosition(camPanMotor, pan_pos); - } -} - -void DriveMode::handlePWM(std::shared_ptr joystickMsg) { - bool publish_pwm = false; - if (joystickMsg->buttons[kLightsUp] == 1) { - current_light_pwm_ = std::min(current_light_pwm_ + 1, 100.0); - publish_pwm = true; - } else if (joystickMsg->buttons[kLightsDown] == 1) { - current_light_pwm_ = std::max(current_light_pwm_ - 1, 0.0); - publish_pwm = true; - } - if (publish_pwm) { - auto pwm_msg = std_msgs::msg::Float32(); - pwm_msg.data = current_light_pwm_; - pwm_pub_->publish(pwm_msg); - } -} - -void DriveMode::setServoPosition(std::string name, double position) { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = position; - motor_pubs[name]->publish(servo_msg); -} - -void DriveMode::handleVideo( - std::shared_ptr joystickMsg) const { - // TODO: Implement stream control -} - -void DriveMode::declareParameters(rclcpp::Node *node) { - - node->declare_parameter("drive_mode.forward_axis", 1); - node->declare_parameter("drive_mode.yaw_axis", 2); - node->declare_parameter("drive_mode.cam_tilt_axis", 3); - node->declare_parameter("drive_mode.cam_pan_axis", 4); - node->declare_parameter("drive_mode.cam_reset", 5); - node->declare_parameter("drive_mode.lights_up", 6); - node->declare_parameter("drive_mode.lights_down", 7); - node->declare_parameter("drive_mode.cruise_control", 8); - node->declare_parameter("drive_mode.max_linear", 2.0); - node->declare_parameter("drive_mode.max_angular", 2.0); - node->declare_parameter("drive_mode.max_increment", 0.1); - node->declare_parameter("drive_mode.min_speed", 0.1); - node->declare_parameter("drive_mode.throttle.axis", 0); - node->declare_parameter("drive_mode.throttle.max", 1.0); - node->declare_parameter("drive_mode.throttle.min", -1.0); - node->declare_parameter("drive_mode.default_pan", M_PI / 4); - node->declare_parameter("drive_mode.default_tilt", M_PI / 4); - node->declare_parameter("drive_mode.camera_speed", 1.0); - node->declare_parameter("drive_mode.cam_tilt_servo", "tilt"); - node->declare_parameter("drive_mode.cam_pan_servo", "pan"); -} - -void DriveMode::loadParameters() { - node_->get_parameter("drive_mode.forward_axis", kForwardAxis); - node_->get_parameter("drive_mode.yaw_axis", kYawAxis); - node_->get_parameter("drive_mode.cam_tilt_axis", kCamTiltAxis); - node_->get_parameter("drive_mode.cam_pan_axis", kCamPanAxis); - node_->get_parameter("drive_mode.cam_reset", kCamReset); - node_->get_parameter("drive_mode.lights_up", kLightsUp); - node_->get_parameter("drive_mode.lights_down", kLightsDown); - node_->get_parameter("drive_mode.cruise_control", kCruiseControl); - node_->get_parameter("drive_mode.max_linear", kMaxLinear); - node_->get_parameter("drive_mode.max_angular", kMaxAngular); - node_->get_parameter("drive_mode.max_increment", kMaxIncrement); - node_->get_parameter("drive_mode.min_speed", kMinSpeed); - node_->get_parameter("drive_mode.throttle.axis", kThrottleAxis); - node_->get_parameter("drive_mode.throttle.max", kThrottleMax); - node_->get_parameter("drive_mode.throttle.min", kThrottleMin); - node_->get_parameter("drive_mode.cam_tilt_servo", camTiltMotor); - node_->get_parameter("drive_mode.cam_pan_servo", camPanMotor); - node_->get_parameter("drive_mode.default_pan", kDefaultCamPan); - node_->get_parameter("drive_mode.default_tilt", kDefaultCamTilt); - node_->get_parameter("drive_mode.camera_speed", kCameraSpeed); -} diff --git a/src/Teleop-Control/joystick_control/src/FlightstickControl.cpp b/src/Teleop-Control/joystick_control/src/FlightstickControl.cpp deleted file mode 100644 index cc0ac5d1..00000000 --- a/src/Teleop-Control/joystick_control/src/FlightstickControl.cpp +++ /dev/null @@ -1,163 +0,0 @@ -#include "FlightstickControl.hpp" - -FlightstickControl::FlightstickControl() - : Node("flightstick_control"), mode_(nullptr), - currentMode_(ModeType::NONE) { - declareParameters(); - loadParameters(); - changeMode(currentMode_); - joy_sub_ = this->create_subscription( - "/joy", 10, - std::bind(&FlightstickControl::processJoystick, this, - std::placeholders::_1)); - - status_pub_ = this->create_publisher( - "/rover_mode", rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - rclcpp::QoS qos(rclcpp::KeepLast(10)); - qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); - light_pub_ = this->create_publisher("/light", qos); -} - -void FlightstickControl::processJoystick( - std::shared_ptr joystickMsg) { - if (checkForModeChange(joystickMsg)) { - return; - } - if (mode_ != nullptr && currentMode_ != ModeType::NONE) { - mode_->processJoystickInput(joystickMsg); - } -} - -bool FlightstickControl::checkForModeChange( - std::shared_ptr joystickMsg) { - static const std::map buttonToMode = { - {kDriveModeButton, ModeType::DRIVE}, - {kArmIKModeButton, ModeType::ARM_IK}, - {kArmManualModeButton, ModeType::ARM_MANUAL}, - {kArmDummyButton, ModeType::ARM_DUMMY}, - {kNavModeButton, ModeType::NAV}, - {kScienceModeButton, ModeType::SCIENCE}}; - for (const auto &[buttonIndex, mode] : buttonToMode) { - if (joystickMsg->buttons[buttonIndex]) { - if (!checkAxes(joystickMsg, mode)) { - RCLCPP_WARN(this->get_logger(), - "At least one axis is not 0, can't switch modes"); - return false; - } - return changeMode(mode); - } - } - - return false; -} - -bool FlightstickControl::checkAxes( - std::shared_ptr joystickMsg, ModeType nextMode) { - // key is mode, vector is axes - static const std::map> modeParameters = { - {ModeType::DRIVE, {0, 1, 3, 4}}, - {ModeType::ARM_IK, {0, 1, 4, 5, 6}}, - {ModeType::ARM_MANUAL, {0, 1, 4, 5, 6, 7}}, - {ModeType::SCIENCE, {1, 3}}, - {ModeType::ARM_DUMMY, {0, 1, 4, 5, 6, 7}}}; // add nav? - auto it = modeParameters.find(nextMode); - if (it == modeParameters.end()) { - return true; - } - const auto &axesIndices = it->second; - for (const int &index : axesIndices) { - if (index >= joystickMsg->axes.size()) { - continue; - } - if (joystickMsg->axes[index] > 0.01) { - return false; - } - } - return true; -} -bool FlightstickControl::changeMode(ModeType mode) { - if (currentMode_ == mode) { - return false; - } - currentMode_ = mode; - auto message = std_msgs::msg::String(); - switch (mode) { - case ModeType::NONE: - mode_ = nullptr; - message.data = "Idle"; - status_pub_->publish(message); - break; - case ModeType::DRIVE: - RCLCPP_INFO(this->get_logger(), "Entering Drive Mode"); - mode_ = std::make_unique(this); - message.data = "Drive"; - status_pub_->publish(message); - break; - case ModeType::ARM_MANUAL: - RCLCPP_INFO(this->get_logger(), "Entering Manual Mode"); - mode_ = std::make_unique(this); - message.data = "Manual"; - status_pub_->publish(message); - break; - case ModeType::SCIENCE: - RCLCPP_INFO(this->get_logger(), "Entering Science Mode"); - mode_ = std::make_unique(this); - message.data = "Science"; - status_pub_->publish(message); - break; - case ModeType::ARM_IK: - RCLCPP_INFO(this->get_logger(), "Entering IK Mode"); - mode_ = std::make_unique(this); - message.data = "IK"; - status_pub_->publish(message); - break; - case ModeType::ARM_DUMMY: - RCLCPP_INFO(this->get_logger(), "Entering Arm Dumb Mode"); - mode_ = std::make_unique(this); - message.data = "Dummy"; - status_pub_->publish(message); - break; - default: - RCLCPP_WARN(this->get_logger(), "Mode not implemented, returning to NONE"); - currentMode_ = ModeType::NONE; - mode_ = nullptr; - message.data = "Idle"; - status_pub_->publish(message); - return false; - } - auto msg = std_msgs::msg::Int8(); - msg.data = kTeleopLightMode; - light_pub_->publish(msg); - return true; -} - -void FlightstickControl::declareParameters() { - this->declare_parameter("drive_mode_button", 12); - this->declare_parameter("arm_ik_mode_button", 11); - this->declare_parameter("arm_manual_mode_button", 10); - this->declare_parameter("nav_mode_button", 13); - this->declare_parameter("science_mode_button", 14); - this->declare_parameter("arm_dummy_mode_button", 8); - this->declare_parameter("teleop_light_mode", 1); - DriveMode::declareParameters(this); - ArmManualMode::declareParameters(this); - ArmIKMode::declareParameters(this); - ScienceMode::declareParameters(this); -} - -void FlightstickControl::loadParameters() { - this->get_parameter("drive_mode_button", kDriveModeButton); - this->get_parameter("arm_ik_mode_button", kArmIKModeButton); - this->get_parameter("arm_manual_mode_button", kArmManualModeButton); - this->get_parameter("nav_mode_button", kNavModeButton); - this->get_parameter("science_mode_button", kScienceModeButton); - this->get_parameter("arm_dummy_mode_button", kArmDummyButton); - this->get_parameter("teleop_light_mode", kTeleopLightMode); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/ScienceMode.cpp b/src/Teleop-Control/joystick_control/src/ScienceMode.cpp deleted file mode 100644 index f3f2c4e3..00000000 --- a/src/Teleop-Control/joystick_control/src/ScienceMode.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include "ScienceMode.hpp" - -ScienceMode::ScienceMode(rclcpp::Node *node) : Mode("Science", node) { - RCLCPP_INFO(node_->get_logger(), "Science Mode"); - loadParameters(); - - platform_pub_ = node_->create_publisher( - "/platform/set", 10); - drill_pub_ = - node_->create_publisher("/drill/set", 10); - - std::vector motor_names = {collectionServo, microscopeServo}; - - for (const auto &topic : motor_names) { - std::string topic_name = "/" + topic; - motor_pubs[topic] = node_->create_publisher( - topic_name, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); - } - led_client_ = - node_->create_client("/microscope_light"); -} - -void ScienceMode::processJoystickInput( - std::shared_ptr joystickMsg) { - handlePlatform(joystickMsg); - handleDrill(joystickMsg); - handleMicroscope(joystickMsg); - handleSoilCollection(joystickMsg); -} - -void ScienceMode::handlePlatform( - std::shared_ptr joystickMsg) const { - // Process input and output linear component - double value = joystickMsg->axes[kPlatformAxis]; - ros_phoenix::msg::MotorControl platform_control; - platform_control.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - platform_control.value = value; - platform_pub_->publish(platform_control); -} - -void ScienceMode::handleDrill( - std::shared_ptr joystickMsg) const { - // Turn it on and off - double drill_value = - joystickMsg->buttons[kDrillButton] * joystickMsg->axes[kThrottleAxis]; - ros_phoenix::msg::MotorControl drill_control; - drill_control.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT; - drill_control.value = drill_value; - drill_pub_->publish(drill_control); -} - -void ScienceMode::handleMicroscope( - std::shared_ptr joystickMsg) { - // Process input and output linear component - static double position; - double value = joystickMsg->axes[kMicroscopeAxis] * rad_multiplier; - if (value != 0) { - position += value; - setServoPosition(microscopeServo, position); - } - if (joystickMsg->buttons[kMicroscopeLightButton]) { - toggleLights(); - } -} - -void ScienceMode::handleSoilCollection( - std::shared_ptr joystickMsg) { - if (joystickMsg->buttons[kCollectionButton]) { - RCLCPP_INFO(node_->get_logger(), "Collection"); - setServoPosition(collectionServo, kCollectionOpen); - } else if (joystickMsg->buttons[kCancelCollectionButton]) { - RCLCPP_INFO(node_->get_logger(), "Cancel collection"); - setServoPosition(collectionServo, kCollectionClose); - } else if (joystickMsg->buttons[kSoilTestButton]) { - RCLCPP_INFO(node_->get_logger(), "Soil test"); - setServoPosition(collectionServo, kCollectionSample); - } else if (joystickMsg->buttons[kSoilLockButton]) { - RCLCPP_INFO(node_->get_logger(), "Soil lock"); - setServoPosition(collectionServo, kCollectionLock); - } -} - -void ScienceMode::setServoPosition(std::string name, double position) { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = position; - motor_pubs[name]->publish(servo_msg); -} - -void ScienceMode::toggleLights() const { - static bool light_on = false; - auto request = std::make_shared(); - request->data = !light_on; - light_on = !light_on; - if (!led_client_->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_WARN(node_->get_logger(), "Service not available after waiting"); - return; - } - led_client_->async_send_request(request); -} - -void ScienceMode::declareParameters(rclcpp::Node *node) { - node->declare_parameter("science_mode.platform_axis", 1); - node->declare_parameter("science_mode.drill_button", 2); - node->declare_parameter("science_mode.microscope_axis", 3); - node->declare_parameter("science_mode.throttle_axis", 3); - node->declare_parameter("science_mode.soil_collection_button", 4); - node->declare_parameter("science_mode.cancel_collection_button", 5); - node->declare_parameter("science_mode.soil_test_button", 6); - node->declare_parameter("science_mode.microscope_light_button", 7); - node->declare_parameter("science_mode.collection_servo_name", "collection"); - node->declare_parameter("science_mode.microscope_servo_name", "microscope"); - node->declare_parameter("science_mode.collection_open", 0.0); - node->declare_parameter("science_mode.collection_dump", M_PI / 2); - node->declare_parameter("science_mode.collection_lock", M_PI); - node->declare_parameter("science_mode.collection_test", M_PI); -} - -void ScienceMode::loadParameters() { - node_->get_parameter("science_mode.platform_axis", kPlatformAxis); - node_->get_parameter("science_mode.drill_button", kDrillButton); - node_->get_parameter("science_mode.microscope_axis", kMicroscopeAxis); - node_->get_parameter("science_mode.throttle_axis", kThrottleAxis); - node_->get_parameter("science_mode.soil_collection_button", - kCollectionButton); - node_->get_parameter("science_mode.cancel_collection_button", - kCancelCollectionButton); - node_->get_parameter("science_mode.soil_test_button", kSoilTestButton); - node_->get_parameter("science_mode.lock_sample_button", kSoilLockButton); - node_->get_parameter("science_mode.microscope_light_button", - kMicroscopeLightButton); - node_->get_parameter("science_mode.collection_servo_name", collectionServo); - node_->get_parameter("science_mode.microscope_servo_name", microscopeServo); - node_->get_parameter("science_mode.collection_open", kCollectionOpen); - node_->get_parameter("science_mode.collection_dump", kCollectionClose); - node_->get_parameter("science_mode.collection_test", kCollectionSample); - node_->get_parameter("science_mode.collection_lock", kCollectionLock); -} diff --git a/src/teleop_modeless/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp similarity index 100% rename from src/teleop_modeless/src/arm.cpp rename to src/Teleop-Control/joystick_control/src/arm.cpp diff --git a/src/teleop_modeless/src/drive.cpp b/src/Teleop-Control/joystick_control/src/drive.cpp similarity index 100% rename from src/teleop_modeless/src/drive.cpp rename to src/Teleop-Control/joystick_control/src/drive.cpp diff --git a/src/teleop_modeless/CMakeLists.txt b/src/teleop_modeless/CMakeLists.txt deleted file mode 100644 index 3436796d..00000000 --- a/src/teleop_modeless/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(teleop_modeless) - -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(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(joy REQUIRED) -find_package(ros_phoenix REQUIRED) -find_package(interfaces REQUIRED) -find_package(control_msgs REQUIRED) -find_package(std_srvs REQUIRED) - -set(CMAKE_CXX_STANDARD 14) - -include_directories(include) - - -add_executable(drive - src/drive.cpp -) - -add_executable(arm - src/arm.cpp - src/ArmHelpers.cpp -) - -ament_target_dependencies(drive - rclcpp - geometry_msgs - joy - ros_phoenix - interfaces - control_msgs - std_srvs -) - -ament_target_dependencies(arm - rclcpp - geometry_msgs - joy - ros_phoenix - interfaces - control_msgs - std_srvs -) - -install(TARGETS - drive - arm - DESTINATION lib/${PROJECT_NAME}) - -# Install header files -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) -#install config files -install(DIRECTORY config/ - DESTINATION share/${PROJECT_NAME} -) - -# Install launch files -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/src/teleop_modeless/include/ArmHelpers.hpp b/src/teleop_modeless/include/ArmHelpers.hpp deleted file mode 100644 index ff010463..00000000 --- a/src/teleop_modeless/include/ArmHelpers.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef teleop_modeless__ARM_HELPERS_HPP_ -#define teleop_modeless__ARM_HELPERS_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace ArmHelpers { -bool start_moveit_servo(rclcpp::Node *node, int attempts = 3); -} - -#endif \ No newline at end of file diff --git a/src/teleop_modeless/launch/controller.launch.py b/src/teleop_modeless/launch/controller.launch.py deleted file mode 100644 index db2e00ed..00000000 --- a/src/teleop_modeless/launch/controller.launch.py +++ /dev/null @@ -1,75 +0,0 @@ -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_teleop_modeless = get_package_share_directory("teleop_modeless") - parameters_file = os.path.join(pkg_teleop_modeless, "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", "/arm/joy")], - ), - Node( - package="teleop_modeless", - executable="arm", - name="arm_teleop_node", - parameters=[parameters_file], - ), - Node( - package="teleop_modeless", - executable="drive", - name="drive_teleop_node", - parameters=[parameters_file], - ), - ] - ) diff --git a/src/teleop_modeless/package.xml b/src/teleop_modeless/package.xml deleted file mode 100644 index 3b97f8b8..00000000 --- a/src/teleop_modeless/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - teleop_modeless - 0.0.0 - TODO: Package description - vscode - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/teleop_modeless/src/ArmHelpers.cpp b/src/teleop_modeless/src/ArmHelpers.cpp deleted file mode 100644 index a0da874a..00000000 --- a/src/teleop_modeless/src/ArmHelpers.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "ArmHelpers.hpp" - -#include "std_srvs/srv/trigger.hpp" - -bool ArmHelpers::start_moveit_servo(rclcpp::Node *node, int attempts) { - auto start_moveit_client = - node->create_client("/servo_node/start_servo"); - int i = 0; - while (!start_moveit_client->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_WARN(node->get_logger(), "Service not available, Waiting..."); - if (++i >= attempts) { - RCLCPP_ERROR(node->get_logger(), - "Service not available after %d attempts. Giving up", i); - return false; - } - } - auto start_request = std::make_shared(); - start_moveit_client->async_send_request(start_request); - return true; -} From fbdd6d239e54dd2f461eb925362f03449555a867 Mon Sep 17 00:00:00 2001 From: Jetson Nano1 Date: Sun, 25 Jan 2026 22:29:14 +0000 Subject: [PATCH 07/23] get working --- .devcontainer/base-station/devcontainer.json | 1 + .../joystick_control/config/pxn.yaml | 6 --- .../joystick_control/include/arm.hpp | 3 -- .../joystick_control/include/drive.hpp | 2 + .../launch/controller.launch.py | 2 + .../joystick_control/src/ArmHelpers.cpp | 20 --------- .../joystick_control/src/arm.cpp | 44 +++++-------------- .../joystick_control/src/drive.cpp | 12 ++++- 8 files changed, 25 insertions(+), 65 deletions(-) delete mode 100644 src/Teleop-Control/joystick_control/src/ArmHelpers.cpp diff --git a/.devcontainer/base-station/devcontainer.json b/.devcontainer/base-station/devcontainer.json index 3e27fad1..639b3d42 100644 --- a/.devcontainer/base-station/devcontainer.json +++ b/.devcontainer/base-station/devcontainer.json @@ -17,6 +17,7 @@ "--ipc=host", "--group-add=video", "--group-add=render", + "--group-add=systemd-journal", "--env", "DISPLAY=${localEnv:DISPLAY}", "--volume", "/tmp/.X11-unix:/tmp/.X11-unix:rw" ], diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 536e4d49..c987df68 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -10,8 +10,6 @@ arm_teleop_node: ros__parameters: throttle: axis: 2 - min: -1.0 - max: 1.0 base_axis: 0 wrist_roll: 4 wrist_yaw_positive: 3 @@ -19,10 +17,6 @@ arm_teleop_node: act1_axis: 1 act2_axis: 5 elbow_yaw: 3 - claw: 1 - simple_forward: 2 - simple_backward: 4 - servo_name: "arm" max_base_speed: 1.0 max_wrist_roll_speed: 1.0 max_wrist_speed: 1.0 diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index 88c4e77e..ae5a9cf6 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -16,7 +16,6 @@ class arm : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr joint_pub_; - rclcpp::Publisher::SharedPtr servo_pub__; control_msgs::msg::JointJog joint_msg_; void declareParameters(); @@ -36,8 +35,6 @@ class arm : public rclcpp::Node { int kAct1Axis; int kAct2Axis; int kElbowYaw; - int kClaw; - std::string servoName; }; #endif \ 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 99b0c21b..2835aed0 100644 --- a/src/Teleop-Control/joystick_control/include/drive.hpp +++ b/src/Teleop-Control/joystick_control/include/drive.hpp @@ -23,6 +23,8 @@ class drive : public rclcpp::Node { int kForwardAxis; int kYawAxis; int kStrafeAxis; + + bool initialized_; }; #endif // DRIVE_HPP \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index c8e2f7a1..8fdc4989 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -64,12 +64,14 @@ def generate_launch_description(): executable="arm", name="arm_teleop_node", parameters=[parameters_file], + remappings=[("/joy", "/arm/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/ArmHelpers.cpp b/src/Teleop-Control/joystick_control/src/ArmHelpers.cpp deleted file mode 100644 index a0da874a..00000000 --- a/src/Teleop-Control/joystick_control/src/ArmHelpers.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "ArmHelpers.hpp" - -#include "std_srvs/srv/trigger.hpp" - -bool ArmHelpers::start_moveit_servo(rclcpp::Node *node, int attempts) { - auto start_moveit_client = - node->create_client("/servo_node/start_servo"); - int i = 0; - while (!start_moveit_client->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_WARN(node->get_logger(), "Service not available, Waiting..."); - if (++i >= attempts) { - RCLCPP_ERROR(node->get_logger(), - "Service not available after %d attempts. Giving up", i); - return false; - } - } - auto start_request = std::make_shared(); - start_moveit_client->async_send_request(start_request); - return true; -} diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 65317964..3a0c6137 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,5 +1,4 @@ #include "arm.hpp" -#include "ArmHelpers.hpp" arm::arm() : Node("arm_node") { @@ -7,21 +6,12 @@ arm::arm() : Node("arm_node") { loadParameters(); joy_sub_ = this->create_subscription( - "/controller_b/joy", 10, - std::bind(&arm::arm_control, this, std::placeholders::_1)); + "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub_ = this->create_publisher( "/servo_node/delta_joint_cmds", 10); - if (!ArmHelpers::start_moveit_servo(this)) { - RCLCPP_ERROR( - this->get_logger(), - "Failed to start MoveIt servo service, Manual mode will not work"); - } joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", - "Joint_4", "Joint_5", "Joint_6"}; - - servo_pub_ = this->create_publisher( - "/" + servoName, rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); + "Joint_4", "Joint_5", "Joint_6"}; RCLCPP_INFO(this->get_logger(), "Arm controller started"); } @@ -34,26 +24,16 @@ void arm::arm_control(std::shared_ptr joystickMsg) { joint_msg_.header = joystickMsg->header; - joint_msg_.velocities = {axes[kBaseAxis] * maxBaseSpeed, - axes[kAct1Axis] * maxAct1Speed, - axes[kAct2Axis] * maxAct2Speed, - axes[kWristRoll] * maxWristRollSpeed, - axes[kElbowYaw] * maxElbowYawSpeed, - buttons[kWristYaw_positive] - - buttons[kWristYaw_negative] * maxWristSpeed * - axes[kThrottleAxis]}; + joint_msg_.velocities = {axes[kBaseAxis] * kMaxBaseSpeed, + axes[kAct1Axis] * kMaxAct1Speed, + axes[kAct2Axis] * kMaxAct2Speed, + axes[kWristRoll] * kMaxWristRollSpeed, + axes[kElbowYaw] * kMaxElbowYawSpeed, + buttons[kWristYaw_positive] - + buttons[kWristYaw_negative] * kMaxWristSpeed * + axes[kThrottleAxis]}; joint_pub_->publish(joint_msg_); - - if (joystickMsg->buttons[kClaw] == 1) { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = 1.0; - servo_pub_->publish(servo_msg); - } else { - auto servo_msg = std_msgs::msg::Float32(); - servo_msg.data = 0.0; - servo_pub_->publish(servo_msg); - } } void arm::declareParameters() { @@ -65,8 +45,6 @@ void arm::declareParameters() { this->declare_parameter("act1_axis", 1); this->declare_parameter("act2_axis", 5); this->declare_parameter("elbow_yaw", 6); - this->declare_parameter("claw", 1); - this->declare_parameter("servo_name", "arm"); this->declare_parameter("max_base_speed", 1.0); this->declare_parameter("max_wrist_roll_speed", 1.0); this->declare_parameter("max_wrist_speed", 1.0); @@ -83,8 +61,6 @@ void arm::loadParameters() { this->get_parameter("act1_axis", kAct1Axis); this->get_parameter("act2_axis", kAct2Axis); this->get_parameter("elbow_yaw", kElbowYaw); - this->get_parameter("claw", kClaw); - this->get_parameter("servo_name", servoName); this->get_parameter("max_base_speed", kMaxBaseSpeed); this->get_parameter("max_wrist_roll_speed", kMaxWristRollSpeed); this->get_parameter("max_wrist_speed", kMaxWristSpeed); diff --git a/src/Teleop-Control/joystick_control/src/drive.cpp b/src/Teleop-Control/joystick_control/src/drive.cpp index 09e9c538..73f3c86a 100644 --- a/src/Teleop-Control/joystick_control/src/drive.cpp +++ b/src/Teleop-Control/joystick_control/src/drive.cpp @@ -1,16 +1,24 @@ #include "drive.hpp" -drive::drive() : Node("drive_node") { +drive::drive() : Node("drive_node"), initialized_(false) { declare_parameters(); load_parameters(); twist_pub = this->create_publisher("cmd_vel", 10); joy_sub = this->create_subscription( - "/controller_a/joy", 10, + "/joy", 10, std::bind(&drive::drive_control, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "Drive controller started"); }; void drive::drive_control(std::shared_ptr joystickMsg) { + if (!initialized_) { + if (std::abs(joystickMsg->axes[kForwardAxis]) < 0.01 && + std::abs(joystickMsg->axes[kStrafeAxis]) < 0.01 && + std::abs(joystickMsg->axes[kYawAxis]) < 0.01) { + initialized_ = true; + } + return; + } auto twist = geometry_msgs::msg::Twist(); twist.linear.x = joystickMsg->axes[kForwardAxis] * kMaxLinear; twist.linear.y = joystickMsg->axes[kStrafeAxis] * kMaxLinear; From 4c7d3796217fc867028667b0f31ae356a849ed58 Mon Sep 17 00:00:00 2001 From: Seysha Date: Tue, 27 Jan 2026 03:23:10 +0000 Subject: [PATCH 08/23] feat: add arm_ik controller for inverse kinematics handling --- WIP --- .gitignore | 3 +- .../joystick_control/CMakeLists.txt | 15 ++++ .../joystick_control/include/arm_ik.hpp | 37 +++++++++ .../launch/controller.launch.py | 2 +- .../joystick_control/src/arm_ik.cpp | 81 +++++++++++++++++++ 5 files changed, 136 insertions(+), 2 deletions(-) create mode 100644 src/Teleop-Control/joystick_control/include/arm_ik.hpp create mode 100644 src/Teleop-Control/joystick_control/src/arm_ik.cpp diff --git a/.gitignore b/.gitignore index b965222e..3f74b0e8 100644 --- a/.gitignore +++ b/.gitignore @@ -15,4 +15,5 @@ camera_setup/images/* # macOS Directory Information .DS_Store -upgrade_pkg.tar.* \ No newline at end of file +upgrade_pkg.tar.* +src/kindr \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index c31c639c..c5b40321 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -28,6 +28,10 @@ add_executable(arm src/arm.cpp ) +add_executable(arm_ik + src/arm_ik.cpp +) + ament_target_dependencies(drive rclcpp geometry_msgs @@ -48,9 +52,20 @@ ament_target_dependencies(arm std_srvs ) +ament_target_dependencies(arm_ik + rclcpp + geometry_msgs + joy + ros_phoenix + interfaces + control_msgs + std_srvs +) + install(TARGETS drive arm + arm_ik DESTINATION lib/${PROJECT_NAME}) # Install header files diff --git a/src/Teleop-Control/joystick_control/include/arm_ik.hpp b/src/Teleop-Control/joystick_control/include/arm_ik.hpp new file mode 100644 index 00000000..bfeb4617 --- /dev/null +++ b/src/Teleop-Control/joystick_control/include/arm_ik.hpp @@ -0,0 +1,37 @@ +#ifndef ARM_IK_HPP +#define ARM_IK_HPP + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joy.hpp" + +class arm_ik : public rclcpp::Node { +public: + arm_ik(); + void arm_ik_control(std::shared_ptr joystickMsg); + +private: + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr ik_pub_; + + void declareParameters(); + void loadParameters(); + + int kForwardAxis; + int kLateralAxis; + int kVerticalAxis; + int kRollAxis; + int kPitchAxis; + int kYawAxis; + + double kMaxForwardSpeed; + double kMaxLateralSpeed; + double kMaxVerticalSpeed; + double kMaxRollSpeed; + double kMaxPitchSpeed; + double kMaxYawSpeed; + + int kGripperButton; +}; + +#endif // ARM_IK_HPP \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 8fdc4989..a0f9d3b8 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): ), Node( package="joystick_control", - executable="arm", + executable="arm_ik", name="arm_teleop_node", parameters=[parameters_file], remappings=[("/joy", "/arm/joy")], diff --git a/src/Teleop-Control/joystick_control/src/arm_ik.cpp b/src/Teleop-Control/joystick_control/src/arm_ik.cpp new file mode 100644 index 00000000..ff71f0c1 --- /dev/null +++ b/src/Teleop-Control/joystick_control/src/arm_ik.cpp @@ -0,0 +1,81 @@ +#include "arm_ik.hpp" + +arm_ik::arm_ik() : Node("arm_ik_node") { + declareParameters(); + loadParameters(); + ik_pub_ = this->create_publisher( + "servo_node/delta_twist_cmds", 10); + joy_sub_ = this->create_subscription( + "/joy", 10, + std::bind(&arm_ik::arm_ik_control, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Arm IK controller started"); +} + +void arm_ik::arm_ik_control( + std::shared_ptr joystickMsg) { + auto twist_msg = geometry_msgs::msg::TwistStamped(); + + auto axes = joystickMsg->axes; + + auto buttons = joystickMsg->buttons; + + twist_msg.header = joystickMsg->header; + + //this may or may not be needed, I don't think so though but for now we are leaving it in + //twist_msg.header.stamp = this->get_clock()->now(); + + // this assumes that the twist stamped messages are velocity based. Are they? + // I'm not sure. Follow up on this. + // Did someone write this message above me or am I getting dimentia? ^ + + twist_msg.twist.linear.x = axes[kForwardAxis] * kMaxForwardSpeed; + twist_msg.twist.linear.y = axes[kLateralAxis] * kMaxLateralSpeed; + twist_msg.twist.linear.z = axes[kVerticalAxis] * kMaxVerticalSpeed; + twist_msg.twist.angular.x = axes[kRollAxis] * kMaxRollSpeed; + twist_msg.twist.angular.y = axes[kPitchAxis] * kMaxPitchSpeed; + twist_msg.twist.angular.z = axes[kYawAxis] * kMaxYawSpeed; + + ik_pub_->publish(twist_msg); +} + +void arm_ik::declareParameters() { + this->declare_parameter("forward_axis", 1); + this->declare_parameter("lateral_axis", 0); + this->declare_parameter("vertical_axis", 4); + this->declare_parameter("roll_axis", 3); + this->declare_parameter("pitch_axis", 2); + this->declare_parameter("yaw_axis", 5); + this->declare_parameter("gripper_button", 0); + + this->declare_parameter("max_forward_speed", 0.1); + this->declare_parameter("max_lateral_speed", 0.1); + this->declare_parameter("max_vertical_speed", 0.1); + this->declare_parameter("max_roll_speed", 0.1); + this->declare_parameter("max_pitch_speed", 0.1); + this->declare_parameter("max_yaw_speed", 0.1); +} + +void arm_ik::loadParameters() { + this->get_parameter("forward_axis", kForwardAxis); + this->get_parameter("lateral_axis", kLateralAxis); + this->get_parameter("vertical_axis", kVerticalAxis); + this->get_parameter("roll_axis", kRollAxis); + this->get_parameter("pitch_axis", kPitchAxis); + this->get_parameter("yaw_axis", kYawAxis); + this->get_parameter("gripper_button", kGripperButton); + + this->get_parameter("max_forward_speed", kMaxForwardSpeed); + this->get_parameter("max_lateral_speed", kMaxLateralSpeed); + this->get_parameter("max_vertical_speed", kMaxVerticalSpeed); + this->get_parameter("max_roll_speed", kMaxRollSpeed); + this->get_parameter("max_pitch_speed", kMaxPitchSpeed); + this->get_parameter("max_yaw_speed", kMaxYawSpeed); +} + +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 From 71ce87d6aeabac6ee128f703a86360883aff087a Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 5 Feb 2026 00:57:05 +0000 Subject: [PATCH 09/23] Added inverse kinematics control logic to arm joystick controll --- .../joystick_control/CMakeLists.txt | 13 -- .../joystick_control/config/pxn.yaml | 51 ++++--- .../joystick_control/include/arm.hpp | 29 ++++ .../joystick_control/include/arm_ik.hpp | 37 ----- .../launch/controller.launch.py | 4 +- .../joystick_control/src/arm.cpp | 132 ++++++++++++++---- .../joystick_control/src/arm_ik.cpp | 81 ----------- 7 files changed, 169 insertions(+), 178 deletions(-) delete mode 100644 src/Teleop-Control/joystick_control/include/arm_ik.hpp delete mode 100644 src/Teleop-Control/joystick_control/src/arm_ik.cpp diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 96ebf32f..ff3262ca 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -28,9 +28,6 @@ add_executable(arm src/arm.cpp ) -add_executable(arm_ik - src/arm_ik.cpp -) ament_target_dependencies(drive rclcpp @@ -52,16 +49,6 @@ ament_target_dependencies(arm std_srvs ) -ament_target_dependencies(arm_ik - rclcpp - geometry_msgs - joy - ros_phoenix - interfaces - control_msgs - std_srvs -) - install(TARGETS drive arm diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index c987df68..26bddacc 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -1,25 +1,42 @@ drive_teleop_node: ros__parameters: - #TODO change these for an actual PXN forward_axis: 1 strafe_axis: 0 - yaw_axis: 2 + yaw_axis: 3 max_linear: 2.0 max_angular: 1.0 arm_teleop_node: ros__parameters: - throttle: - axis: 2 - base_axis: 0 - wrist_roll: 4 - wrist_yaw_positive: 3 - wrist_yaw_negative: 5 - act1_axis: 1 - act2_axis: 5 - elbow_yaw: 3 - max_base_speed: 1.0 - max_wrist_roll_speed: 1.0 - 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 + mode_switch_button: 7 + arm_manual: + throttle: + axis: 2 + base_axis: 0 + wrist_roll: 4 + wrist_yaw_positive: 3 + wrist_yaw_negative: 5 + act1_axis: 1 + act2_axis: 5 + elbow_yaw: 3 + max_base_speed: 1.0 + max_wrist_roll_speed: 1.0 + max_wrist_speed: 1.0 + max_act1_speed: 1.0 + max_act2_speed: 1.0 + max_elbow_yaw_speed: 1.0 + arm_ik: + forward_axis: 1 + lateral_axis: 0 + vertical_axis: 4 + roll_axis: 3 + pitch_axis: 2 + yaw_axis: 5 + gripper_buton: 6 + max_forward_speed: 0.1 + max_lateral_speed: 0.1 + max_vertical_speed: 0.1 + max_roll_speed: 0.1 + max_pitch_speed: 0.1 + max_yaw_speed: 0.1 + + diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index ae5a9cf6..c1f73149 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -3,6 +3,7 @@ #include "control_msgs/msg/joint_jog.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joy.hpp" @@ -15,12 +16,24 @@ class arm : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr ik_pub_; rclcpp::Publisher::SharedPtr joint_pub_; control_msgs::msg::JointJog joint_msg_; + void arm_manual(std::shared_ptr joystickMsg); + void arm_ik(std::shared_ptr joystickMsg); + void mode_switch(std::shared_ptr joystickMsg); + + int mode_button_value = 0; + int mode_button_value_prev = 0; + int modes = 2; + int mode = 1; + void declareParameters(); void loadParameters(); + int mode_switch_button; + double kMaxBaseSpeed; double kMaxWristRollSpeed; double kMaxWristSpeed; @@ -35,6 +48,22 @@ class arm : public rclcpp::Node { int kAct1Axis; int kAct2Axis; int kElbowYaw; + + int kForwardAxis; + int kLateralAxis; + int kVerticalAxis; + int kRollAxis; + int kPitchAxis; + int kYawAxis; + + double kMaxForwardSpeed; + double kMaxLateralSpeed; + double kMaxVerticalSpeed; + double kMaxRollSpeed; + double kMaxPitchSpeed; + double kMaxYawSpeed; + + int kGripperButton; }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/arm_ik.hpp b/src/Teleop-Control/joystick_control/include/arm_ik.hpp deleted file mode 100644 index bfeb4617..00000000 --- a/src/Teleop-Control/joystick_control/include/arm_ik.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef ARM_IK_HPP -#define ARM_IK_HPP - -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/joy.hpp" - -class arm_ik : public rclcpp::Node { -public: - arm_ik(); - void arm_ik_control(std::shared_ptr joystickMsg); - -private: - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr ik_pub_; - - void declareParameters(); - void loadParameters(); - - int kForwardAxis; - int kLateralAxis; - int kVerticalAxis; - int kRollAxis; - int kPitchAxis; - int kYawAxis; - - double kMaxForwardSpeed; - double kMaxLateralSpeed; - double kMaxVerticalSpeed; - double kMaxRollSpeed; - double kMaxPitchSpeed; - double kMaxYawSpeed; - - int kGripperButton; -}; - -#endif // ARM_IK_HPP \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 8fdc4989..f0e08b65 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -30,8 +30,8 @@ def generate_launch_description(): 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) + id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 0) + id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 1) return LaunchDescription( [ diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 3a0c6137..ac91c782 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -9,6 +9,8 @@ arm::arm() : Node("arm_node") { "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub_ = this->create_publisher( "/servo_node/delta_joint_cmds", 10); + ik_pub_ = this->create_publisher( + "servo_node/delta_twist_cmds", 10); joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; @@ -17,6 +19,16 @@ arm::arm() : Node("arm_node") { } void arm::arm_control(std::shared_ptr joystickMsg) { + mode_switch(joystickMsg); + if (mode == 1) { + arm_manual(joystickMsg); + } + if (mode == 2) { + arm_ik(joystickMsg); + } +} + +void arm::arm_manual(std::shared_ptr joystickMsg) { auto joint_msg_ = control_msgs::msg::JointJog(); auto axes = joystickMsg->axes; @@ -36,37 +48,101 @@ void arm::arm_control(std::shared_ptr joystickMsg) { joint_pub_->publish(joint_msg_); } +void arm::mode_switch(std::shared_ptr joystickMsg) { + mode_button_value = joystickMsg->buttons[mode_switch_button]; + + if ((mode_button_value == 0) && (mode_button_value_prev == 1)) { + mode = mode + 1; + // This logic is here to allow us to add more modes like science perhaps + if (mode > modes) { // Modes is the number of modes we have + mode = 1; + } + } + + mode_button_value_prev = joystickMsg->buttons[mode_switch_button]; +} + +void arm::arm_ik(std::shared_ptr joystickMsg) { + auto twist_msg = geometry_msgs::msg::TwistStamped(); + + auto axes = joystickMsg->axes; + + auto buttons = joystickMsg->buttons; + + twist_msg.header = joystickMsg->header; + + twist_msg.twist.linear.x = axes[kForwardAxis] * kMaxForwardSpeed; + twist_msg.twist.linear.y = axes[kLateralAxis] * kMaxLateralSpeed; + twist_msg.twist.linear.z = axes[kVerticalAxis] * kMaxVerticalSpeed; + twist_msg.twist.angular.x = axes[kRollAxis] * kMaxRollSpeed; + twist_msg.twist.angular.y = axes[kPitchAxis] * kMaxPitchSpeed; + twist_msg.twist.angular.z = axes[kYawAxis] * kMaxYawSpeed; + + ik_pub_->publish(twist_msg); +} void arm::declareParameters() { - this->declare_parameter("throttle.axis", 2); - this->declare_parameter("base_axis", 0); - this->declare_parameter("wrist_roll", 4); - this->declare_parameter("wrist_yaw_positive", 3); - this->declare_parameter("wrist_yaw_negative", 5); - this->declare_parameter("act1_axis", 1); - this->declare_parameter("act2_axis", 5); - this->declare_parameter("elbow_yaw", 6); - this->declare_parameter("max_base_speed", 1.0); - this->declare_parameter("max_wrist_roll_speed", 1.0); - this->declare_parameter("max_wrist_speed", 1.0); - this->declare_parameter("max_act1_speed", 1.0); - this->declare_parameter("max_act2_speed", 1.0); - this->declare_parameter("max_elbow_yaw_speed", 1.0); + this->declare_parameter("arm_manual.throttle.axis", 2); + this->declare_parameter("arm_manual.base_axis", 0); + this->declare_parameter("arm_manual.wrist_roll", 4); + this->declare_parameter("arm_manual.wrist_yaw_positive", 3); + this->declare_parameter("arm_manual.wrist_yaw_negative", 5); + this->declare_parameter("arm_manual.act1_axis", 1); + this->declare_parameter("arm_manual.act2_axis", 5); + this->declare_parameter("arm_manual.elbow_yaw", 6); + this->declare_parameter("arm_manual.max_base_speed", 1.0); + this->declare_parameter("arm_manual.max_wrist_roll_speed", 1.0); + this->declare_parameter("arm_manual.max_wrist_speed", 1.0); + this->declare_parameter("arm_manual.max_act1_speed", 1.0); + this->declare_parameter("arm_manual.max_act2_speed", 1.0); + this->declare_parameter("arm_manual.max_elbow_yaw_speed", 1.0); + + this->declare_parameter("arm_ik.forward_axis", 1); + this->declare_parameter("arm_ik.lateral_axis", 0); + this->declare_parameter("arm_ik.vertical_axis", 4); + this->declare_parameter("arm_ik.roll_axis", 3); + this->declare_parameter("arm_ik.pitch_axis", 2); + this->declare_parameter("arm_ik.yaw_axis", 5); + this->declare_parameter("arm_ik.gripper_button", 0); + this->declare_parameter("arm_ik.max_forward_speed", 0.1); + this->declare_parameter("arm_ik.max_lateral_speed", 0.1); + this->declare_parameter("arm_ik.max_vertical_speed", 0.1); + this->declare_parameter("arm_ik.max_roll_speed", 0.1); + this->declare_parameter("arm_ik.max_pitch_speed", 0.1); + this->declare_parameter("arm_ik.max_yaw_speed", 0.1); + + this->declare_parameter("mode_switch_button", 7); } void arm::loadParameters() { - this->get_parameter("throttle.axis", kThrottleAxis); - this->get_parameter("base_axis", kBaseAxis); - this->get_parameter("wrist_roll", kWristRoll); - this->get_parameter("wrist_yaw_positive", kWristYaw_positive); - this->get_parameter("wrist_yaw_negative", kWristYaw_negative); - this->get_parameter("act1_axis", kAct1Axis); - this->get_parameter("act2_axis", kAct2Axis); - this->get_parameter("elbow_yaw", kElbowYaw); - this->get_parameter("max_base_speed", kMaxBaseSpeed); - this->get_parameter("max_wrist_roll_speed", kMaxWristRollSpeed); - this->get_parameter("max_wrist_speed", kMaxWristSpeed); - this->get_parameter("max_act1_speed", kMaxAct1Speed); - this->get_parameter("max_act2_speed", kMaxAct2Speed); - this->get_parameter("max_elbow_yaw_speed", kMaxElbowYawSpeed); + this->get_parameter("arm_manual.throttle.axis", kThrottleAxis); + this->get_parameter("arm_manual.base_axis", kBaseAxis); + this->get_parameter("arm_manual.wrist_roll", kWristRoll); + this->get_parameter("arm_manual.wrist_yaw_positive", kWristYaw_positive); + this->get_parameter("arm_manual.wrist_yaw_negative", kWristYaw_negative); + this->get_parameter("arm_manual.act1_axis", kAct1Axis); + this->get_parameter("arm_manual.act2_axis", kAct2Axis); + this->get_parameter("arm_manual.elbow_yaw", kElbowYaw); + this->get_parameter("arm_manual.max_base_speed", kMaxBaseSpeed); + this->get_parameter("arm_manual.max_wrist_roll_speed", kMaxWristRollSpeed); + this->get_parameter("arm_manual.max_wrist_speed", kMaxWristSpeed); + this->get_parameter("arm_manual.max_act1_speed", kMaxAct1Speed); + this->get_parameter("arm_manual.max_act2_speed", kMaxAct2Speed); + this->get_parameter("arm_manual.max_elbow_yaw_speed", kMaxElbowYawSpeed); + + this->get_parameter("arm_ik.forward_axis", kForwardAxis); + this->get_parameter("arm_ik.lateral_axis", kLateralAxis); + this->get_parameter("arm_ik.vertical_axis", kVerticalAxis); + this->get_parameter("arm_ik.roll_axis", kRollAxis); + this->get_parameter("arm_ik.pitch_axis", kPitchAxis); + this->get_parameter("arm_ik.yaw_axis", kYawAxis); + this->get_parameter("arm_ik.gripper_button", kGripperButton); + this->get_parameter("arm_ik.max_forward_speed", kMaxForwardSpeed); + this->get_parameter("arm_ik.max_lateral_speed", kMaxLateralSpeed); + this->get_parameter("arm_ik.max_vertical_speed", kMaxVerticalSpeed); + this->get_parameter("arm_ik.max_roll_speed", kMaxRollSpeed); + this->get_parameter("arm_ik.max_pitch_speed", kMaxPitchSpeed); + this->get_parameter("arm_ik.max_yaw_speed", kMaxYawSpeed); + + this->get_parameter("mode_switch_button", mode_switch_button); } int main(int argc, char **argv) { diff --git a/src/Teleop-Control/joystick_control/src/arm_ik.cpp b/src/Teleop-Control/joystick_control/src/arm_ik.cpp deleted file mode 100644 index ff71f0c1..00000000 --- a/src/Teleop-Control/joystick_control/src/arm_ik.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include "arm_ik.hpp" - -arm_ik::arm_ik() : Node("arm_ik_node") { - declareParameters(); - loadParameters(); - ik_pub_ = this->create_publisher( - "servo_node/delta_twist_cmds", 10); - joy_sub_ = this->create_subscription( - "/joy", 10, - std::bind(&arm_ik::arm_ik_control, this, std::placeholders::_1)); - RCLCPP_INFO(this->get_logger(), "Arm IK controller started"); -} - -void arm_ik::arm_ik_control( - std::shared_ptr joystickMsg) { - auto twist_msg = geometry_msgs::msg::TwistStamped(); - - auto axes = joystickMsg->axes; - - auto buttons = joystickMsg->buttons; - - twist_msg.header = joystickMsg->header; - - //this may or may not be needed, I don't think so though but for now we are leaving it in - //twist_msg.header.stamp = this->get_clock()->now(); - - // this assumes that the twist stamped messages are velocity based. Are they? - // I'm not sure. Follow up on this. - // Did someone write this message above me or am I getting dimentia? ^ - - twist_msg.twist.linear.x = axes[kForwardAxis] * kMaxForwardSpeed; - twist_msg.twist.linear.y = axes[kLateralAxis] * kMaxLateralSpeed; - twist_msg.twist.linear.z = axes[kVerticalAxis] * kMaxVerticalSpeed; - twist_msg.twist.angular.x = axes[kRollAxis] * kMaxRollSpeed; - twist_msg.twist.angular.y = axes[kPitchAxis] * kMaxPitchSpeed; - twist_msg.twist.angular.z = axes[kYawAxis] * kMaxYawSpeed; - - ik_pub_->publish(twist_msg); -} - -void arm_ik::declareParameters() { - this->declare_parameter("forward_axis", 1); - this->declare_parameter("lateral_axis", 0); - this->declare_parameter("vertical_axis", 4); - this->declare_parameter("roll_axis", 3); - this->declare_parameter("pitch_axis", 2); - this->declare_parameter("yaw_axis", 5); - this->declare_parameter("gripper_button", 0); - - this->declare_parameter("max_forward_speed", 0.1); - this->declare_parameter("max_lateral_speed", 0.1); - this->declare_parameter("max_vertical_speed", 0.1); - this->declare_parameter("max_roll_speed", 0.1); - this->declare_parameter("max_pitch_speed", 0.1); - this->declare_parameter("max_yaw_speed", 0.1); -} - -void arm_ik::loadParameters() { - this->get_parameter("forward_axis", kForwardAxis); - this->get_parameter("lateral_axis", kLateralAxis); - this->get_parameter("vertical_axis", kVerticalAxis); - this->get_parameter("roll_axis", kRollAxis); - this->get_parameter("pitch_axis", kPitchAxis); - this->get_parameter("yaw_axis", kYawAxis); - this->get_parameter("gripper_button", kGripperButton); - - this->get_parameter("max_forward_speed", kMaxForwardSpeed); - this->get_parameter("max_lateral_speed", kMaxLateralSpeed); - this->get_parameter("max_vertical_speed", kMaxVerticalSpeed); - this->get_parameter("max_roll_speed", kMaxRollSpeed); - this->get_parameter("max_pitch_speed", kMaxPitchSpeed); - this->get_parameter("max_yaw_speed", kMaxYawSpeed); -} - -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 From 39d5eef75070b764590b03cc3fcc97613f129e76 Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 5 Feb 2026 01:07:13 +0000 Subject: [PATCH 10/23] WIP adding actions for movement to point --- .../include/action/move_to_pose.action | 5 ++ .../include/movegroup_controller.hpp | 32 ++++++++ .../src/movegroup_controll.cpp | 78 +++++++++++++++++++ 3 files changed, 115 insertions(+) create mode 100644 src/Teleop-Control/joystick_control/include/action/move_to_pose.action create mode 100644 src/Teleop-Control/joystick_control/include/movegroup_controller.hpp create mode 100644 src/Teleop-Control/joystick_control/src/movegroup_controll.cpp diff --git a/src/Teleop-Control/joystick_control/include/action/move_to_pose.action b/src/Teleop-Control/joystick_control/include/action/move_to_pose.action new file mode 100644 index 00000000..c26ef8c8 --- /dev/null +++ b/src/Teleop-Control/joystick_control/include/action/move_to_pose.action @@ -0,0 +1,5 @@ +int32 poseID +--- +bool success +--- +float32 current_positions[6] \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp new file mode 100644 index 00000000..8f1da44c --- /dev/null +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -0,0 +1,32 @@ +#ifndef MOVEGROUP_CONTROLLER_HPP +#define MOVEGROUP_CONTROLLER_HPP +#include "" +#include "action/move_to_pose.action" +#include "geometry_msgs/msg/pose.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +class MoveGroupController : public rclcpp::Node { +public: + using MoveToPose = action::MoveToPose; + using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; + + explicit MoveGroupController( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + +private: + rclcpp_action::GoalResponse + handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + rclcpp_action::Server::SharedPtr action_server_; + void handle_move(geometry_msgs::msg::Pose target_pose); + const std::string PLANNING_GROUP = "rover_arm"; + moveit::planning_interface::MoveGroupInterface move_group_; + + void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, + moveit::planning_interface::MoveGroupInterfacePtr mgi); +} \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp new file mode 100644 index 00000000..9bd8ea8a --- /dev/null +++ b/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp @@ -0,0 +1,78 @@ +#include "movegroup_controller.hpp" + +MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) + : Node("movegroup_controller", options) { + this->action_server_ = rclcpp_action::create_server( + this, "move_to_pose", + std::bind(&MoveGroupController::handle_goal, this, _1, _2), + std::bind(&MoveGroupController::handle_cancel, this, _1), + std::bind(&MoveGroupController::handle_accepted, this, _1)); + + this->move_group_ = moveit::planning_interface::MoveGroupInterface( + this, this->PLANNING_GROUP); + RCLCPP_INFO(this->get_logger(), "MoveGroupController action server started"); +} + +rclcpp_action::GoalResponse +MoveGroupController::handle_goal(const rclcpp::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Received goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MoveGroupController::handle_cancel( + const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MoveGroupController::handle_accepted( + const std::shared_ptr goal_handle) { + std::thread{std::bind(&MoveGroupController::execute, this, goal_handle)} + .detach(); +} + +void MoveGroupController::execute( + const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + // This isn't real, just a placeholder. You (yes you Seysha) need to implement + // the movegroup go to a position + handle_move() + + for (int i = 0; i <= 100; ++i) { + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + feedback->progress = i; + goal_handle->publish_feedback(feedback); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + } + + result->success = true; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); +} + +void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { + // Placeholder for move group logic + RCLCPP_INFO(this->get_logger(), "Moving to the desired pose: %f, %f, %f", + target_pose.position.x, target_pose.position.y, + target_pose.position.z); + move_group_.setPoseTarget(target_pose); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_.plan(my_plan) == + moveit::planning_interface::MoveItErrorCode::SUCCESS); + if (success) { + move_group_->execute(my_plan); + RCLCPP_INFO(this->get_logger(), "Move executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to plan the move"); + } +} \ No newline at end of file From dae154d2f6946dad556096136f9c17087644f37b Mon Sep 17 00:00:00 2001 From: Seysha Date: Mon, 9 Feb 2026 05:08:17 +0000 Subject: [PATCH 11/23] Updating movegroup_controll action to contain movegroup logic --- .../joystick_control/src/movegroup_controll.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp index 9bd8ea8a..d66cfa3d 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp @@ -41,7 +41,7 @@ void MoveGroupController::execute( // This isn't real, just a placeholder. You (yes you Seysha) need to implement // the movegroup go to a position - handle_move() + handle_move(goal->poseID); for (int i = 0; i <= 100; ++i) { if (goal_handle->is_canceling()) { @@ -75,4 +75,4 @@ void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { } else { RCLCPP_ERROR(this->get_logger(), "Failed to plan the move"); } -} \ No newline at end of file +} From ff3d07c62c9cfeb77b13837252ac6ccaa64bbd1b Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 00:21:00 +0000 Subject: [PATCH 12/23] Added action to cmake lists --- src/Teleop-Control/joystick_control/CMakeLists.txt | 14 ++++++++++++++ ...group_controll.cpp => movegroup_controller.cpp} | 2 +- 2 files changed, 15 insertions(+), 1 deletion(-) rename src/Teleop-Control/joystick_control/src/{movegroup_controll.cpp => movegroup_controller.cpp} (98%) diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index ff3262ca..7b2f9e36 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -8,6 +8,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(geometry_msgs REQUIRED) find_package(joy REQUIRED) find_package(ros_phoenix REQUIRED) @@ -19,6 +21,9 @@ set(CMAKE_CXX_STANDARD 14) include_directories(include) +add_library(movegroup_action SHARED +src/movegroup_controller.cpp +) add_executable(drive src/drive.cpp @@ -28,6 +33,15 @@ add_executable(arm src/arm.cpp ) +target_compile_definitions(movegroup_action + PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") + + +ament_target_dependencies(movegroup_action + rclcpp + rclcpp_action + rclcpp_components +) ament_target_dependencies(drive rclcpp diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp similarity index 98% rename from src/Teleop-Control/joystick_control/src/movegroup_controll.cpp rename to src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index d66cfa3d..78e9c72d 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controll.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -43,7 +43,7 @@ void MoveGroupController::execute( // the movegroup go to a position handle_move(goal->poseID); - for (int i = 0; i <= 100; ++i) { + for (int i = 0; i <= 100; ++i) { if (goal_handle->is_canceling()) { result->success = false; goal_handle->canceled(result); From bf5c3715b78f076ea5f60a3e4bf0b8a330647bb3 Mon Sep 17 00:00:00 2001 From: MaybeWilli <51888361+MaybeWilli@users.noreply.github.com> Date: Wed, 11 Feb 2026 19:37:28 -0500 Subject: [PATCH 13/23] I dunno --- .../joystick_control/CMakeLists.txt | 19 +++++++++++++++++++ .../joystick_control/package.xml | 5 +++++ 2 files changed, 24 insertions(+) diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 7b2f9e36..26a9e13f 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -17,6 +17,11 @@ find_package(interfaces REQUIRED) find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) +#moveit things +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_servo REQUIRED) + set(CMAKE_CXX_STANDARD 14) include_directories(include) @@ -33,6 +38,10 @@ add_executable(arm src/arm.cpp ) +add_executable(movegroup_controller + src/movegroup_controller.cpp +) + target_compile_definitions(movegroup_action PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") @@ -63,9 +72,19 @@ ament_target_dependencies(arm std_srvs ) +ament_target_dependencies( + movegroup_controller + interfaces + "moveit_ros_planning_interface" + "moveit_core" + "rclcpp" +) + + install(TARGETS drive arm + movegroup_controller DESTINATION lib/${PROJECT_NAME}) # Install header files diff --git a/src/Teleop-Control/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml index 30e8d407..f86c799e 100644 --- a/src/Teleop-Control/joystick_control/package.xml +++ b/src/Teleop-Control/joystick_control/package.xml @@ -9,6 +9,11 @@ ament_cmake + moveit_ros_planning_interface + moveit_core + rclcpp + interfaces + ament_lint_auto ament_lint_common From 9161f69717d9cb170ad639abaac78fa61125f8b2 Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 01:42:50 +0000 Subject: [PATCH 14/23] Fixed movegroup dependencies + launch --- .../joystick_control/CMakeLists.txt | 5 ++ .../joystick_control/config/positions.yaml | 0 .../include/action/move_to_pose.action | 5 -- .../include/movegroup_controller.hpp | 17 ++--- .../launch/controller.launch.py | 10 ++- .../src/movegroup_controller.cpp | 63 ++++++++++++++----- src/interfaces/CMakeLists.txt | 3 + src/interfaces/action/MoveToPose.action | 5 ++ 8 files changed, 78 insertions(+), 30 deletions(-) create mode 100644 src/Teleop-Control/joystick_control/config/positions.yaml delete mode 100644 src/Teleop-Control/joystick_control/include/action/move_to_pose.action create mode 100644 src/interfaces/action/MoveToPose.action diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 26a9e13f..5831e742 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -50,6 +50,10 @@ ament_target_dependencies(movegroup_action rclcpp rclcpp_action rclcpp_components + moveit_ros_planning_interface + moveit_core + moveit_servo + interfaces ) ament_target_dependencies(drive @@ -78,6 +82,7 @@ ament_target_dependencies( "moveit_ros_planning_interface" "moveit_core" "rclcpp" + rclcpp_action ) diff --git a/src/Teleop-Control/joystick_control/config/positions.yaml b/src/Teleop-Control/joystick_control/config/positions.yaml new file mode 100644 index 00000000..e69de29b diff --git a/src/Teleop-Control/joystick_control/include/action/move_to_pose.action b/src/Teleop-Control/joystick_control/include/action/move_to_pose.action deleted file mode 100644 index c26ef8c8..00000000 --- a/src/Teleop-Control/joystick_control/include/action/move_to_pose.action +++ /dev/null @@ -1,5 +0,0 @@ -int32 poseID ---- -bool success ---- -float32 current_positions[6] \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp index 8f1da44c..d1011986 100644 --- a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -1,15 +1,16 @@ #ifndef MOVEGROUP_CONTROLLER_HPP -#define MOVEGROUP_CONTROLLER_HPP -#include "" -#include "action/move_to_pose.action" +#define MOVEGROUP_CONTROLLER_HPPsu #include "geometry_msgs/msg/pose.hpp" +#include "interfaces/action/move_to_pose.hpp" +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/robot_state/robot_state.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" class MoveGroupController : public rclcpp::Node { public: - using MoveToPose = action::MoveToPose; - using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; + using MoveToPose = interfaces::action::MoveToPose; + using GoalHandleMoveToPose = rclcpp_action::ServerGoalHandle; explicit MoveGroupController( const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); @@ -25,8 +26,8 @@ class MoveGroupController : public rclcpp::Node { rclcpp_action::Server::SharedPtr action_server_; void handle_move(geometry_msgs::msg::Pose target_pose); const std::string PLANNING_GROUP = "rover_arm"; - moveit::planning_interface::MoveGroupInterface move_group_; - + std::shared_ptr move_group_; void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, moveit::planning_interface::MoveGroupInterfacePtr mgi); -} \ No newline at end of file +}; +#endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index f0e08b65..adda3f76 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -28,12 +28,13 @@ def get_joy_id(dev_path, 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") + positions_file = os.path.join(pkg_joystick_control, "positions.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", 0) id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 1) - return LaunchDescription( + return LaunchDescription(s [ Node( package="joy", @@ -73,5 +74,12 @@ def generate_launch_description(): parameters=[parameters_file], remappings=[("/joy", "/drive/joy")], ), + Node( + package="joystick_control", + executable="movegroup_controller", + name="movegroup_control_action_server", + parameters=[positions_file], + + ) ] ) diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index 78e9c72d..528c4630 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -1,33 +1,48 @@ #include "movegroup_controller.hpp" +#include + +// Use placeholders for cleaner bind calls +using namespace std::placeholders; MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) : Node("movegroup_controller", options) { + + // 1. Initialize Action Server this->action_server_ = rclcpp_action::create_server( this, "move_to_pose", std::bind(&MoveGroupController::handle_goal, this, _1, _2), std::bind(&MoveGroupController::handle_cancel, this, _1), std::bind(&MoveGroupController::handle_accepted, this, _1)); - this->move_group_ = moveit::planning_interface::MoveGroupInterface( - this, this->PLANNING_GROUP); + // 2. Initialize MoveGroupInterface correctly for Humble + // Note: PLANNING_GROUP should be a string like "arm" + this->move_group_ = + std::make_shared( + shared_from_this(), "arm_group"); + RCLCPP_INFO(this->get_logger(), "MoveGroupController action server started"); } rclcpp_action::GoalResponse -MoveGroupController::handle_goal(const rclcpp::GoalUUID &uuid, +MoveGroupController::handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - RCLCPP_INFO(this->get_logger(), "Received goal request"); + (void)uuid; + // Use the lowercase pose_id we defined in the .action file + RCLCPP_INFO(this->get_logger(), "Received goal request for ID: %d", + goal->pose_id); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse MoveGroupController::handle_cancel( const std::shared_ptr goal_handle) { + (void)goal_handle; RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; } void MoveGroupController::handle_accepted( const std::shared_ptr goal_handle) { + // Use a detached thread to handle long-running execution std::thread{std::bind(&MoveGroupController::execute, this, goal_handle)} .detach(); } @@ -39,10 +54,11 @@ void MoveGroupController::execute( auto feedback = std::make_shared(); auto result = std::make_shared(); - // This isn't real, just a placeholder. You (yes you Seysha) need to implement - // the movegroup go to a position - handle_move(goal->poseID); + // Pass the ID to your movement logic + // (Assuming handle_move is updated to take int32_t pose_id) + // handle_move(goal->pose_id); + // Simulation loop for feedback for (int i = 0; i <= 100; ++i) { if (goal_handle->is_canceling()) { result->success = false; @@ -50,7 +66,10 @@ void MoveGroupController::execute( RCLCPP_INFO(this->get_logger(), "Goal canceled"); return; } - feedback->progress = i; + + // NOTE: Replace 'progress' with actual fields from your .action file + // feedback->current_positions = ... + goal_handle->publish_feedback(feedback); rclcpp::sleep_for(std::chrono::milliseconds(50)); } @@ -61,18 +80,30 @@ void MoveGroupController::execute( } void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { - // Placeholder for move group logic - RCLCPP_INFO(this->get_logger(), "Moving to the desired pose: %f, %f, %f", + RCLCPP_INFO(this->get_logger(), "Moving to pose: x:%f, y:%f, z:%f", target_pose.position.x, target_pose.position.y, target_pose.position.z); - move_group_.setPoseTarget(target_pose); + + // Use -> because move_group_ is now a shared_ptr + move_group_->setPoseTarget(target_pose); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group_.plan(my_plan) == - moveit::planning_interface::MoveItErrorCode::SUCCESS); - if (success) { + auto const ok = + (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + + if (ok) { move_group_->execute(my_plan); - RCLCPP_INFO(this->get_logger(), "Move executed successfully"); } else { - RCLCPP_ERROR(this->get_logger(), "Failed to plan the move"); + RCLCPP_ERROR(this->get_logger(), "Planning failed!"); } } + +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/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index f4df93bf..fd98b0a0 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(geometry_msgs REQUIRED) find_package(geographic_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/ArucoMarkers.msg" @@ -31,12 +32,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/VideoCapture.srv" "srv/GetCameras.srv" "srv/Raman.srv" + "action/MoveToPose.action" DEPENDENCIES builtin_interfaces geometry_msgs geographic_msgs # Add packages that above messages depend on sensor_msgs std_msgs + action_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/src/interfaces/action/MoveToPose.action b/src/interfaces/action/MoveToPose.action new file mode 100644 index 00000000..d886d4b8 --- /dev/null +++ b/src/interfaces/action/MoveToPose.action @@ -0,0 +1,5 @@ +int32 pose_id +--- +bool success +--- +float32[6] current_positions \ No newline at end of file From 3c380db26a1381d054779e49bfb2dceb6faee3d0 Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 01:50:01 +0000 Subject: [PATCH 15/23] small commit --for sharing-- added yaml file for positions --- src/Teleop-Control/joystick_control/config/positions.yaml | 1 + .../joystick_control/include/movegroup_controller.hpp | 1 + .../joystick_control/launch/controller.launch.py | 2 +- .../joystick_control/src/movegroup_controller.cpp | 5 ++++- 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/Teleop-Control/joystick_control/config/positions.yaml b/src/Teleop-Control/joystick_control/config/positions.yaml index e69de29b..159939fa 100644 --- a/src/Teleop-Control/joystick_control/config/positions.yaml +++ b/src/Teleop-Control/joystick_control/config/positions.yaml @@ -0,0 +1 @@ +movegroup_control_action_server: \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp index d1011986..020ca5b4 100644 --- a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -29,5 +29,6 @@ class MoveGroupController : public rclcpp::Node { std::shared_ptr move_group_; void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, moveit::planning_interface::MoveGroupInterfacePtr mgi); + geometry_msgs::msg::Pose destination; }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index adda3f76..38ad9e96 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 0) id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 1) - return LaunchDescription(s + return LaunchDescription( [ Node( package="joy", diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index 528c4630..83b62015 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -56,7 +56,10 @@ void MoveGroupController::execute( // Pass the ID to your movement logic // (Assuming handle_move is updated to take int32_t pose_id) - // handle_move(goal->pose_id); + + geometry_msgs::msg::Pose + + handle_move(goal->pose_id); // Simulation loop for feedback for (int i = 0; i <= 100; ++i) { From ae8f3ce6e3aa935dcc0e5042c7513a6daecc837d Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 03:08:37 +0000 Subject: [PATCH 16/23] Finished position movement and logic (UNTESTED) --- .../include/movegroup_controller.hpp | 5 +++ .../launch/controller.launch.py | 3 +- .../src/movegroup_controller.cpp | 42 ++++++++++++------- 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp index 020ca5b4..cd4edd7c 100644 --- a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -30,5 +30,10 @@ class MoveGroupController : public rclcpp::Node { void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, moveit::planning_interface::MoveGroupInterfacePtr mgi); geometry_msgs::msg::Pose destination; + + std::vector> positions; + + void declare_parameters(); + void load_parameters(); }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 38ad9e96..0add24a2 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -79,7 +79,6 @@ def generate_launch_description(): executable="movegroup_controller", name="movegroup_control_action_server", parameters=[positions_file], - - ) + ), ] ) diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index 83b62015..da874a71 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -7,15 +7,13 @@ using namespace std::placeholders; MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) : Node("movegroup_controller", options) { - // 1. Initialize Action Server + declare_parameters(); + load_parameters(); this->action_server_ = rclcpp_action::create_server( this, "move_to_pose", std::bind(&MoveGroupController::handle_goal, this, _1, _2), std::bind(&MoveGroupController::handle_cancel, this, _1), std::bind(&MoveGroupController::handle_accepted, this, _1)); - - // 2. Initialize MoveGroupInterface correctly for Humble - // Note: PLANNING_GROUP should be a string like "arm" this->move_group_ = std::make_shared( shared_from_this(), "arm_group"); @@ -27,7 +25,6 @@ rclcpp_action::GoalResponse MoveGroupController::handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { (void)uuid; - // Use the lowercase pose_id we defined in the .action file RCLCPP_INFO(this->get_logger(), "Received goal request for ID: %d", goal->pose_id); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -53,15 +50,18 @@ void MoveGroupController::execute( const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto result = std::make_shared(); + int dest_index = goal->pose_id; - // Pass the ID to your movement logic - // (Assuming handle_move is updated to take int32_t pose_id) - - geometry_msgs::msg::Pose + destination.position.x = positions[dest_index][0]; + destination.position.y = positions[dest_index][1]; + destination.position.z = positions[dest_index][2]; + destination.orientation.x = positions[dest_index][3]; + destination.orientation.y = positions[dest_index][4]; + destination.orientation.z = positions[dest_index][5]; + destination.orientation.w = positions[dest_index][6]; - handle_move(goal->pose_id); + handle_move(destination); - // Simulation loop for feedback for (int i = 0; i <= 100; ++i) { if (goal_handle->is_canceling()) { result->success = false; @@ -70,8 +70,8 @@ void MoveGroupController::execute( return; } - // NOTE: Replace 'progress' with actual fields from your .action file - // feedback->current_positions = ... + // NOTE: Fix this feedback + // feedback->current_positions = goal_handle->publish_feedback(feedback); rclcpp::sleep_for(std::chrono::milliseconds(50)); @@ -101,12 +101,24 @@ void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { } } +void MoveGroupController::declare_parameters() { + this->declare_parameter>( + "pose1", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}); +} + +void MoveGroupController::load_parameters() { + positions.push_back(this->get_parameter("pose1").as_double_array()); +} + int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); + // Use this instead of rclcpp::spin(node) + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); return 0; } \ No newline at end of file From aa44e862440bf376dbe6966c158a892facfc76bc Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 03:44:08 +0000 Subject: [PATCH 17/23] fixed feedback loop --- .../joystick_control/src/movegroup_controller.cpp | 6 ++---- src/interfaces/action/MoveToPose.action | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index da874a71..7f376660 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -69,9 +69,9 @@ void MoveGroupController::execute( RCLCPP_INFO(this->get_logger(), "Goal canceled"); return; } - // NOTE: Fix this feedback - // feedback->current_positions = + auto current_joints = move_group_->getCurrentJointValues(); + feedback->current_position = current_joints; goal_handle->publish_feedback(feedback); rclcpp::sleep_for(std::chrono::milliseconds(50)); @@ -87,7 +87,6 @@ void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { target_pose.position.x, target_pose.position.y, target_pose.position.z); - // Use -> because move_group_ is now a shared_ptr move_group_->setPoseTarget(target_pose); moveit::planning_interface::MoveGroupInterface::Plan my_plan; @@ -114,7 +113,6 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - // Use this instead of rclcpp::spin(node) rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); executor.spin(); diff --git a/src/interfaces/action/MoveToPose.action b/src/interfaces/action/MoveToPose.action index d886d4b8..df4a4bba 100644 --- a/src/interfaces/action/MoveToPose.action +++ b/src/interfaces/action/MoveToPose.action @@ -2,4 +2,4 @@ int32 pose_id --- bool success --- -float32[6] current_positions \ No newline at end of file +float64[] current_position \ No newline at end of file From 39ccf031f94ceb101d11e50d2a6d865232e6244f Mon Sep 17 00:00:00 2001 From: Yoseph Date: Thu, 12 Feb 2026 03:48:52 +0000 Subject: [PATCH 18/23] Update test joint poses in positions.yaml --- .../joystick_control/config/positions.yaml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/Teleop-Control/joystick_control/config/positions.yaml b/src/Teleop-Control/joystick_control/config/positions.yaml index 159939fa..8ff8b955 100644 --- a/src/Teleop-Control/joystick_control/config/positions.yaml +++ b/src/Teleop-Control/joystick_control/config/positions.yaml @@ -1 +1,8 @@ -movegroup_control_action_server: \ No newline at end of file +movegroup_control_action_server: + ros__parameters: + pos1: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pose2: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pos3: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] + pose4: [0.0, -0.3, 0.5, 0.6, 0.8, 0.7] + pose6: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pose7: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] \ No newline at end of file From ce23aaab983327238863be1f60841cd125e6251f Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 05:41:28 +0000 Subject: [PATCH 19/23] Created action client for movegroup controller action in arm control --- .../joystick_control/CMakeLists.txt | 1 + .../joystick_control/config/pxn.yaml | 2 + .../joystick_control/include/arm.hpp | 16 +++++ .../joystick_control/src/arm.cpp | 66 +++++++++++++++++++ .../src/movegroup_controller.cpp | 1 - 5 files changed, 85 insertions(+), 1 deletion(-) diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index 5831e742..cac7ffaf 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -74,6 +74,7 @@ ament_target_dependencies(arm interfaces control_msgs std_srvs + rclcpp_action ) ament_target_dependencies( diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 26bddacc..65381759 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -38,5 +38,7 @@ arm_teleop_node: max_roll_speed: 0.1 max_pitch_speed: 0.1 max_yaw_speed: 0.1 + arm_saves: + pose1: 0 diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index c1f73149..c2e93a1d 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -4,16 +4,22 @@ #include "control_msgs/msg/joint_jog.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "interfaces/action/move_to_pose.hpp" #include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "sensor_msgs/msg/joy.hpp" #include "std_msgs/msg/float32.hpp" +using MoveToPose = interfaces::action::MoveToPose; +using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; class arm : public rclcpp::Node { public: arm(); void arm_control(std::shared_ptr joystickMsg); + void send_goal_pose(int pose_id); + private: rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr ik_pub_; @@ -24,6 +30,16 @@ class arm : public rclcpp::Node { void arm_ik(std::shared_ptr joystickMsg); void mode_switch(std::shared_ptr joystickMsg); + rclcpp_action::Client::SharedPtr move_group_action_client_; + + void + movegroup_goal_response_callback(GoalHandleMoveToPose::SharedPtr goal_handle); + void movegroup_feedback_callback( + GoalHandleMoveToPose::SharedPtr, + const std::shared_ptr feedback); + void + movegroup_result_callback(const GoalHandleMoveToPose::WrappedResult &result); + int mode_button_value = 0; int mode_button_value_prev = 0; int modes = 2; diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index ac91c782..2c60e95c 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,5 +1,8 @@ #include "arm.hpp" +using MoveToPose = interfaces::action::MoveToPose; +using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; +using namespace std::placeholders; arm::arm() : Node("arm_node") { declareParameters(); @@ -15,6 +18,9 @@ arm::arm() : Node("arm_node") { joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; + this->move_group_action_client_ = + rclcpp_action::create_client(this, "move_to_pose"); + RCLCPP_INFO(this->get_logger(), "Arm controller started"); } @@ -80,6 +86,66 @@ void arm::arm_ik(std::shared_ptr joystickMsg) { ik_pub_->publish(twist_msg); } + +void arm::movegroup_goal_response_callback( + GoalHandleMoveToPose::SharedPtr goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), + "Goal accepted by server, waiting for result"); + } +} + +void arm::movegroup_feedback_callback( + GoalHandleMoveToPose::SharedPtr, + const std::shared_ptr feedback) { + std::stringstream ss; + ss << "Current position: "; + for (auto position : feedback->current_position) { + ss << position << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); +} + +void arm::movegroup_result_callback( + const GoalHandleMoveToPose::WrappedResult &result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + if (result.result->success) { + RCLCPP_INFO(this->get_logger(), "Move Completed"); + } + + rclcpp::shutdown(); +} + +void arm::send_goal_pose(int pose_id) { + auto goal_msg = MoveToPose::Goal(); + goal_msg.pose_id = pose_id; + + RCLCPP_INFO(this->get_logger(), "Sending position: %d", pose_id); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&arm::movegroup_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&arm::movegroup_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&arm::movegroup_result_callback, this, _1); + + this->move_group_action_client_->async_send_goal(goal_msg, send_goal_options); +} void arm::declareParameters() { this->declare_parameter("arm_manual.throttle.axis", 2); this->declare_parameter("arm_manual.base_axis", 0); diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index 7f376660..d376bfeb 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -1,7 +1,6 @@ #include "movegroup_controller.hpp" #include -// Use placeholders for cleaner bind calls using namespace std::placeholders; MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) From bf1b0b079bf8b763d9c6fc0546298d90ba316184 Mon Sep 17 00:00:00 2001 From: Seysha Date: Thu, 12 Feb 2026 19:16:22 +0000 Subject: [PATCH 20/23] Reworked movegroup_controller to take a pose instead of pose id --- .../joystick_control/config/positions.yaml | 8 ---- .../joystick_control/config/pxn.yaml | 9 ++++- .../joystick_control/include/arm.hpp | 6 ++- .../include/movegroup_controller.hpp | 6 --- .../joystick_control/src/arm.cpp | 40 +++++++++++++++++-- .../src/movegroup_controller.cpp | 22 +--------- src/interfaces/action/MoveToPose.action | 2 +- 7 files changed, 51 insertions(+), 42 deletions(-) delete mode 100644 src/Teleop-Control/joystick_control/config/positions.yaml diff --git a/src/Teleop-Control/joystick_control/config/positions.yaml b/src/Teleop-Control/joystick_control/config/positions.yaml deleted file mode 100644 index 8ff8b955..00000000 --- a/src/Teleop-Control/joystick_control/config/positions.yaml +++ /dev/null @@ -1,8 +0,0 @@ -movegroup_control_action_server: - ros__parameters: - pos1: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pose2: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] - pos3: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] - pose4: [0.0, -0.3, 0.5, 0.6, 0.8, 0.7] - pose6: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] - pose7: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 65381759..9ed1ca02 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -38,7 +38,12 @@ arm_teleop_node: max_roll_speed: 0.1 max_pitch_speed: 0.1 max_yaw_speed: 0.1 - arm_saves: - pose1: 0 + saves: + pose1: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pose2: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pose3: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] + pose4: [0.0, -0.3, 0.5, 0.6, 0.8, 0.7] + pose6: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pose7: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index c2e93a1d..d6a6ca16 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -2,6 +2,7 @@ #define ARM_HPP #include "control_msgs/msg/joint_jog.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "interfaces/action/move_to_pose.hpp" @@ -18,7 +19,7 @@ class arm : public rclcpp::Node { arm(); void arm_control(std::shared_ptr joystickMsg); - void send_goal_pose(int pose_id); + void send_goal_pose(geometry_msgs::msg::Pose pose); private: rclcpp::Subscription::SharedPtr joy_sub_; @@ -39,6 +40,9 @@ class arm : public rclcpp::Node { const std::shared_ptr feedback); void movegroup_result_callback(const GoalHandleMoveToPose::WrappedResult &result); + void request_position(int pose_id); + + std::vector> positions; int mode_button_value = 0; int mode_button_value_prev = 0; diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp index cd4edd7c..d1011986 100644 --- a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -29,11 +29,5 @@ class MoveGroupController : public rclcpp::Node { std::shared_ptr move_group_; void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, moveit::planning_interface::MoveGroupInterfacePtr mgi); - geometry_msgs::msg::Pose destination; - - std::vector> positions; - - void declare_parameters(); - void load_parameters(); }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 2c60e95c..1b58897a 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -97,6 +97,19 @@ void arm::movegroup_goal_response_callback( } } +void arm::request_position(int pose_id) { + geometry_msgs::msg::Pose destination; + destination.position.x = positions[pose_id][0]; + destination.position.y = positions[pose_id][1]; + destination.position.z = positions[pose_id][2]; + destination.orientation.x = positions[pose_id][3]; + destination.orientation.y = positions[pose_id][4]; + destination.orientation.z = positions[pose_id][5]; + destination.orientation.w = positions[pose_id][6]; + + send_goal_pose(destination); +} + void arm::movegroup_feedback_callback( GoalHandleMoveToPose::SharedPtr, const std::shared_ptr feedback) { @@ -130,11 +143,11 @@ void arm::movegroup_result_callback( rclcpp::shutdown(); } -void arm::send_goal_pose(int pose_id) { +void arm::send_goal_pose(geometry_msgs::msg::Pose pose) { auto goal_msg = MoveToPose::Goal(); - goal_msg.pose_id = pose_id; + goal_msg.pose_id = pose; - RCLCPP_INFO(this->get_logger(), "Sending position: %d", pose_id); + RCLCPP_INFO(this->get_logger(), "Sending position"); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.goal_response_callback = @@ -177,6 +190,18 @@ void arm::declareParameters() { this->declare_parameter("arm_ik.max_yaw_speed", 0.1); this->declare_parameter("mode_switch_button", 7); + + // If the code is failing to declare parameters correctly, look here first + + auto overrides = + this->get_node_parameters_interface()->get_parameter_overrides(); + for (auto const &[key, value] : overrides) { + if (key.find("saves.") == 0) { + this->declare_parameter(key, value.get_type()); + RCLCPP_INFO(this->get_logger(), "Manually declared save: %s", + key.c_str()); + } + } } void arm::loadParameters() { this->get_parameter("arm_manual.throttle.axis", kThrottleAxis); @@ -209,6 +234,15 @@ void arm::loadParameters() { this->get_parameter("arm_ik.max_yaw_speed", kMaxYawSpeed); this->get_parameter("mode_switch_button", mode_switch_button); + + // If the code is failing to load parameters properly, look here second + + auto result = + this->get_node_parameters_interface()->list_parameters({"saves"}, 0); + for (auto &name : result.names) { + positions.push_back(this->get_parameter(name).as_double_array()); + RCLCPP_INFO(this->get_logger(), "Found save: %s", name.c_str()); + } } int main(int argc, char **argv) { diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp index d376bfeb..e6131146 100644 --- a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -6,8 +6,6 @@ using namespace std::placeholders; MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) : Node("movegroup_controller", options) { - declare_parameters(); - load_parameters(); this->action_server_ = rclcpp_action::create_server( this, "move_to_pose", std::bind(&MoveGroupController::handle_goal, this, _1, _2), @@ -49,17 +47,8 @@ void MoveGroupController::execute( const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto result = std::make_shared(); - int dest_index = goal->pose_id; - destination.position.x = positions[dest_index][0]; - destination.position.y = positions[dest_index][1]; - destination.position.z = positions[dest_index][2]; - destination.orientation.x = positions[dest_index][3]; - destination.orientation.y = positions[dest_index][4]; - destination.orientation.z = positions[dest_index][5]; - destination.orientation.w = positions[dest_index][6]; - - handle_move(destination); + handle_move(goal->pose_id); for (int i = 0; i <= 100; ++i) { if (goal_handle->is_canceling()) { @@ -99,15 +88,6 @@ void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { } } -void MoveGroupController::declare_parameters() { - this->declare_parameter>( - "pose1", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}); -} - -void MoveGroupController::load_parameters() { - positions.push_back(this->get_parameter("pose1").as_double_array()); -} - int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); diff --git a/src/interfaces/action/MoveToPose.action b/src/interfaces/action/MoveToPose.action index df4a4bba..559025a1 100644 --- a/src/interfaces/action/MoveToPose.action +++ b/src/interfaces/action/MoveToPose.action @@ -1,4 +1,4 @@ -int32 pose_id +geometry_msgs/Pose pose_id --- bool success --- From bd48526e9e31bd0517f33cc47d60b20082f707c0 Mon Sep 17 00:00:00 2001 From: Yoseph Date: Thu, 12 Feb 2026 20:54:20 +0000 Subject: [PATCH 21/23] Cleaning up experimental logic in arm node --- .../joystick_control/src/arm.cpp | 72 +------------------ 1 file changed, 3 insertions(+), 69 deletions(-) diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 2c60e95c..38f32bc5 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,8 +1,5 @@ #include "arm.hpp" -using MoveToPose = interfaces::action::MoveToPose; -using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; -using namespace std::placeholders; arm::arm() : Node("arm_node") { declareParameters(); @@ -11,16 +8,13 @@ arm::arm() : Node("arm_node") { joy_sub_ = this->create_subscription( "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub_ = this->create_publisher( - "/servo_node/delta_joint_cmds", 10); + "/servo_node/delta_joint_cmds", 10);Joy ik_pub_ = this->create_publisher( "servo_node/delta_twist_cmds", 10); joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; - this->move_group_action_client_ = - rclcpp_action::create_client(this, "move_to_pose"); - RCLCPP_INFO(this->get_logger(), "Arm controller started"); } @@ -47,8 +41,8 @@ void arm::arm_manual(std::shared_ptr joystickMsg) { axes[kAct2Axis] * kMaxAct2Speed, axes[kWristRoll] * kMaxWristRollSpeed, axes[kElbowYaw] * kMaxElbowYawSpeed, - buttons[kWristYaw_positive] - - buttons[kWristYaw_negative] * kMaxWristSpeed * + (buttons[kWristYaw_positive] - + buttons[kWristYaw_negative]) * kMaxWristSpeed * axes[kThrottleAxis]}; joint_pub_->publish(joint_msg_); @@ -86,66 +80,6 @@ void arm::arm_ik(std::shared_ptr joystickMsg) { ik_pub_->publish(twist_msg); } - -void arm::movegroup_goal_response_callback( - GoalHandleMoveToPose::SharedPtr goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), - "Goal accepted by server, waiting for result"); - } -} - -void arm::movegroup_feedback_callback( - GoalHandleMoveToPose::SharedPtr, - const std::shared_ptr feedback) { - std::stringstream ss; - ss << "Current position: "; - for (auto position : feedback->current_position) { - ss << position << " "; - } - RCLCPP_INFO(this->get_logger(), ss.str().c_str()); -} - -void arm::movegroup_result_callback( - const GoalHandleMoveToPose::WrappedResult &result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; - } - if (result.result->success) { - RCLCPP_INFO(this->get_logger(), "Move Completed"); - } - - rclcpp::shutdown(); -} - -void arm::send_goal_pose(int pose_id) { - auto goal_msg = MoveToPose::Goal(); - goal_msg.pose_id = pose_id; - - RCLCPP_INFO(this->get_logger(), "Sending position: %d", pose_id); - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&arm::movegroup_goal_response_callback, this, _1); - send_goal_options.feedback_callback = - std::bind(&arm::movegroup_feedback_callback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&arm::movegroup_result_callback, this, _1); - - this->move_group_action_client_->async_send_goal(goal_msg, send_goal_options); -} void arm::declareParameters() { this->declare_parameter("arm_manual.throttle.axis", 2); this->declare_parameter("arm_manual.base_axis", 0); From 3610a05b4e65e9b4a4c700f932490e172c8ca24a Mon Sep 17 00:00:00 2001 From: Yoseph Date: Thu, 12 Feb 2026 21:10:09 +0000 Subject: [PATCH 22/23] Accidentally removed lines put back --- .../joystick_control/src/arm.cpp | 72 ++++++++++++++++++- 1 file changed, 69 insertions(+), 3 deletions(-) diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 38f32bc5..2c60e95c 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,5 +1,8 @@ #include "arm.hpp" +using MoveToPose = interfaces::action::MoveToPose; +using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; +using namespace std::placeholders; arm::arm() : Node("arm_node") { declareParameters(); @@ -8,13 +11,16 @@ arm::arm() : Node("arm_node") { joy_sub_ = this->create_subscription( "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1)); joint_pub_ = this->create_publisher( - "/servo_node/delta_joint_cmds", 10);Joy + "/servo_node/delta_joint_cmds", 10); ik_pub_ = this->create_publisher( "servo_node/delta_twist_cmds", 10); joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; + this->move_group_action_client_ = + rclcpp_action::create_client(this, "move_to_pose"); + RCLCPP_INFO(this->get_logger(), "Arm controller started"); } @@ -41,8 +47,8 @@ void arm::arm_manual(std::shared_ptr joystickMsg) { axes[kAct2Axis] * kMaxAct2Speed, axes[kWristRoll] * kMaxWristRollSpeed, axes[kElbowYaw] * kMaxElbowYawSpeed, - (buttons[kWristYaw_positive] - - buttons[kWristYaw_negative]) * kMaxWristSpeed * + buttons[kWristYaw_positive] - + buttons[kWristYaw_negative] * kMaxWristSpeed * axes[kThrottleAxis]}; joint_pub_->publish(joint_msg_); @@ -80,6 +86,66 @@ void arm::arm_ik(std::shared_ptr joystickMsg) { ik_pub_->publish(twist_msg); } + +void arm::movegroup_goal_response_callback( + GoalHandleMoveToPose::SharedPtr goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), + "Goal accepted by server, waiting for result"); + } +} + +void arm::movegroup_feedback_callback( + GoalHandleMoveToPose::SharedPtr, + const std::shared_ptr feedback) { + std::stringstream ss; + ss << "Current position: "; + for (auto position : feedback->current_position) { + ss << position << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); +} + +void arm::movegroup_result_callback( + const GoalHandleMoveToPose::WrappedResult &result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + if (result.result->success) { + RCLCPP_INFO(this->get_logger(), "Move Completed"); + } + + rclcpp::shutdown(); +} + +void arm::send_goal_pose(int pose_id) { + auto goal_msg = MoveToPose::Goal(); + goal_msg.pose_id = pose_id; + + RCLCPP_INFO(this->get_logger(), "Sending position: %d", pose_id); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&arm::movegroup_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&arm::movegroup_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&arm::movegroup_result_callback, this, _1); + + this->move_group_action_client_->async_send_goal(goal_msg, send_goal_options); +} void arm::declareParameters() { this->declare_parameter("arm_manual.throttle.axis", 2); this->declare_parameter("arm_manual.base_axis", 0); From 55977ff66dd18313715a7ac91e37191b8b69ff3a Mon Sep 17 00:00:00 2001 From: Yoseph Date: Thu, 12 Feb 2026 22:12:03 +0000 Subject: [PATCH 23/23] Added pose logic and updated launch confic --- .../joystick_control/config/pxn.yaml | 3 ++- .../joystick_control/include/arm.hpp | 3 +++ .../joystick_control/launch/controller.launch.py | 2 +- src/Teleop-Control/joystick_control/src/arm.cpp | 14 ++++++++++++++ 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 65381759..248d9de3 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -39,6 +39,7 @@ arm_teleop_node: max_pitch_speed: 0.1 max_yaw_speed: 0.1 arm_saves: - pose1: 0 + home_button: 10 + stow_button: 11 diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index c2e93a1d..698e56cb 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -45,6 +45,9 @@ class arm : public rclcpp::Node { int modes = 2; int mode = 1; + int kHomeButton; + int kStowButton; + void declareParameters(); void loadParameters(); diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 0add24a2..62f5f808 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -78,7 +78,7 @@ def generate_launch_description(): package="joystick_control", executable="movegroup_controller", name="movegroup_control_action_server", - parameters=[positions_file], + parameters=[parameters_file], ), ] ) diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 2c60e95c..f3e9dfb3 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -57,6 +57,13 @@ void arm::arm_manual(std::shared_ptr joystickMsg) { void arm::mode_switch(std::shared_ptr joystickMsg) { mode_button_value = joystickMsg->buttons[mode_switch_button]; + // Check which goal pose is pressed + if (joystickMsg->buttons[kHomeButton] == 1){ + send_goal_pose(10); + } else if (joystickMsg->buttons[kStowButton] == 1){ + send_goal_pose(11); + } + if ((mode_button_value == 0) && (mode_button_value_prev == 1)) { mode = mode + 1; // This logic is here to allow us to add more modes like science perhaps @@ -177,7 +184,11 @@ void arm::declareParameters() { this->declare_parameter("arm_ik.max_yaw_speed", 0.1); this->declare_parameter("mode_switch_button", 7); + + this->declare_parameter("arm_saves.home_button", 10); + this->declare_parameter("arm_saves.stow_button", 11); } + void arm::loadParameters() { this->get_parameter("arm_manual.throttle.axis", kThrottleAxis); this->get_parameter("arm_manual.base_axis", kBaseAxis); @@ -209,6 +220,9 @@ void arm::loadParameters() { this->get_parameter("arm_ik.max_yaw_speed", kMaxYawSpeed); this->get_parameter("mode_switch_button", mode_switch_button); + + this->get_parameter("arm_saves.home_button", kHomeButton); + this->get_parameter("arm_saves.stow_button", kStowButton); } int main(int argc, char **argv) {