Merge branch 'MoteusStuff' into MoteusStuff2

Author: jodhsinghnahal

src/arm_hardware_interface/CMakeLists.txt

@@ -12,64 +12,53 @@ find_package(sensor_msgs REQUIRED)
 find_package(serial REQUIRED)
 find_package(rover_msgs REQUIRED)
 find_package(arm_control REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-include_directories(
-  include
-  ${arm_control_INCLUDE_DIRS}
-)
+# uncomment the following section in order to fill in further dependencies
+# manually. find_package(<dependency> REQUIRED)
+include_directories(include ${arm_control_INCLUDE_DIRS})
 
 add_subdirectory(moteus)
-add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
-
+add_library(${PROJECT_NAME} src/armprotocol.cpp
+                            include/arm_hardware_interface/ArmSerialProtocol.h)
 
 # For old and new arm
-add_executable(serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
-add_executable(moteus_arm_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+add_executable(
+  serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h
+                    include/arm_hardware_interface/ArmSerialProtocol.h)
+add_executable(
+  moteus_arm_driver src/MoteusInterface.cpp include/ArmMoteusInterface.h
+                    include/arm_hardware_interface/ArmSerialProtocol.h)
 
-target_compile_definitions(moteus_arm_driver
-    PRIVATE
-    BUILDING_NEW_DRIVER=1
-)
+target_compile_definitions(moteus_arm_driver PRIVATE BUILDING_NEW_DRIVER=1)
 
 target_link_libraries(moteus_arm_driver moteus::cpp)
-ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
-ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
+ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial
+                          rover_msgs arm_control)
+ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial
+                          rover_msgs arm_control)
 
 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
+  # 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
+  # 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()
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/launch
-)
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch)
 
 # Install headers for other packages (for arm serial protocol)
-install(
-  DIRECTORY include/
-  DESTINATION include/
-)
+install(DIRECTORY include/ DESTINATION include/)
 
-install(TARGETS
-  serial_arm_driver
-  moteus_arm_driver
-  ${PROJECT_NAME}
+install(
+  TARGETS serial_arm_driver moteus_arm_driver ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-)
+  LIBRARY DESTINATION lib)
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
-
 ament_package()

src/arm_hardware_interface/include/ArmMoteusInterface.h

@@ -0,0 +1,99 @@
+#include "arm_control/include/armControlParams.h"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include <rclcpp/rclcpp.hpp>
+
+#include "rover_msgs/msg/arm_command.hpp"
+#include <chrono>
+#include <thread>
+
+#include "moteus.h"
+using namespace mjbots;
+
+#include <arm_hardware_interface/ArmSerialProtocol.h>
+#include <serial/serial.h>
+
+#define SIMULATE false
+#define PI 3.14159
+
+#define TX_UART_BUFF 128
+#define RX_UART_BUFF 128
+
+#define AXIS_1_DIR 1
+#define AXIS_2_DIR 1
+#define AXIS_3_DIR 1
+#define AXIS_4_DIR 1
+#define AXIS_5_DIR 1
+#define AXIS_6_DIR 1
+
+using std::string;
+
+class ArmSerial : public rclcpp::Node {
+public:
+  ArmSerial();
+
+  rover_msgs::msg::ArmCommand current_arm_status;
+
+  string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3",           "joint_4",
+                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"};
+
+private:
+  unsigned long baud = 115200;
+  string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
+
+  serial::Serial teensy;
+  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000);
+
+  std::map<int, std::shared_ptr<moteus::Controller>> controllers;
+  std::map<int, moteus::Query::Result> servo_data;
+  std::shared_ptr<moteus::Transport> transport;
+
+  bool send_angles = true;
+
+  struct Axis {
+    float curr_pos;
+    float target_pos;
+    float speed;
+    float zero_rad;
+    float max_rad;
+    int dir;
+  };
+
+  Axis axes[NUM_JOINTS];
+
+  float target_position[NUM_JOINTS];
+  float target_velocities[NUM_JOINTS];
+
+  int homed = 0;
+  bool homing = false;
+  float EE = 0;
+  volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
+  volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
+  volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
+
+  rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
+  void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
+
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
+
+  sensor_msgs::msg::JointState prev_joint_states;
+  rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::thread serialRxThread;
+
+  void serial_rx();
+  float degToRad(float deg);
+  float firmToMoveitOffsetPos(float deg, int axis);
+  float firmToMoveitOffsetVel(float deg, int axis);
+  void send_position_command(float pos[NUM_JOINTS]);
+  void send_velocity_command(float vel[NUM_JOINTS]);
+  void ConfigureMotor(int axis_number, mjbots::moteus::Controller &controller);
+  void send_test_limits_command();
+  void sendHomeCmd(int target_axis);
+  void sendCommCmd(int target_state);
+  void sendMsg(std::string outMsg);
+  void parseArmAngleUart(std::string msg);
+  void parseLimitSwitchTest(std::string msg);
+};

src/arm_hardware_interface/include/ArmSerialInterface.h

@@ -7,13 +7,7 @@
 #include <chrono>
 #include <thread>
 
-#ifdef BUILDING_NEW_DRIVER
-#include "moteus.h"
-using namespace mjbots;
-#endif
-
 #include <arm_hardware_interface/ArmSerialProtocol.h>
-
 #include <serial/serial.h>
 
 #define SIMULATE false
@@ -35,31 +29,18 @@ class ArmSerial : public rclcpp::Node {
 public:
   ArmSerial();
 
-  // float firm
-
   rover_msgs::msg::ArmCommand current_arm_status;
 
-  //   angle_echo.positions.resize(NUM_JOINTS);
-  // void start_rx() {
-  //     serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
-  // }
-  // string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm
-  // urdf
   string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3",           "joint_4",
-                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"}; //? newest (Sep 2024) arm urdf
+                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"};
 
 private:
-  unsigned long baud = 115200; // 19200;
+  unsigned long baud = 115200;
   string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
 
   serial::Serial teensy;
-  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
+  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000);
 
-#ifdef BUILDING_NEW_DRIVER
-  std::map<int, std::shared_ptr<moteus::Controller>> controllers;
-  std::map<int, moteus::Query::Result> servo_data;
-  std::shared_ptr<moteus::Transport> transport;
-#endif
   bool send_angles = true;
 
   struct Axis {
@@ -72,7 +53,6 @@ class ArmSerial : public rclcpp::Node {
   };
 
   Axis axes[NUM_JOINTS];
-  // Axis axis_EE;
 
   float target_position[NUM_JOINTS];
   float target_velocities[NUM_JOINTS];
@@ -89,19 +69,20 @@ class ArmSerial : public rclcpp::Node {
 
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
 
-  sensor_msgs::msg::JointState prev_joint_states; // for sim
+  sensor_msgs::msg::JointState prev_joint_states;
   rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
 
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::thread serialRxThread;
 
-  void serial_rx(); //? should be static inline?
+  void serial_rx();
   float degToRad(float deg);
   float firmToMoveitOffsetPos(float deg, int axis);
   float firmToMoveitOffsetVel(float deg, int axis);
   void send_position_command(float pos[NUM_JOINTS]);
   void send_velocity_command(float vel[NUM_JOINTS]);
+
   void send_test_limits_command();
   void sendHomeCmd(int target_axis);
   void sendCommCmd(int target_state);