Skip to content

Commit 72666ea

Browse files
jodhsinghnahaljodhsinghnahal
committed
fix cmake and vel cmd working
1 parent 89561a5 commit 72666ea

File tree

4 files changed

+203
-151
lines changed

4 files changed

+203
-151
lines changed

‎src/arm_hardware_interface/CMakeLists.txt‎

Lines changed: 14 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -20,22 +20,22 @@ include_directories(
2020
${arm_control_INCLUDE_DIRS}
2121
)
2222

23-
include(FetchContent)
24-
FetchContent_Declare(
25-
moteus
26-
GIT_REPOSITORY https://github.com/mjbots/moteus.git
27-
GIT_TAG aa96bf0ae847921a79d2d3d241d65505ee10e209)
28-
29-
FetchContent_MakeAvailable(moteus)
30-
23+
add_subdirectory(moteus)
3124
add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
3225

26+
3327
# For old and new arm
34-
# add_executable(arm_serial_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
35-
add_executable(arm_serial_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
28+
add_executable(serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
29+
add_executable(moteus_arm_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
30+
31+
target_compile_definitions(moteus_arm_driver
32+
PRIVATE
33+
BUILDING_NEW_DRIVER=1
34+
)
3635

37-
target_link_libraries(arm_serial_driver moteus::cpp)
38-
ament_target_dependencies(arm_serial_driver rclcpp sensor_msgs serial rover_msgs arm_control)
36+
target_link_libraries(moteus_arm_driver moteus::cpp)
37+
ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
38+
ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
3939

4040
if(BUILD_TESTING)
4141
find_package(ament_lint_auto REQUIRED)
@@ -60,7 +60,8 @@ install(
6060
)
6161

6262
install(TARGETS
63-
arm_serial_driver
63+
serial_arm_driver
64+
moteus_arm_driver
6465
${PROJECT_NAME}
6566
DESTINATION lib/${PROJECT_NAME}
6667
ARCHIVE DESTINATION lib

‎src/arm_hardware_interface/include/ArmSerialInterface.h‎

Lines changed: 71 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,18 @@
1-
#include <rclcpp/rclcpp.hpp>
2-
#include "sensor_msgs/msg/joy.hpp"
3-
#include "sensor_msgs/msg/joint_state.hpp"
41
#include "arm_control/include/armControlParams.h"
2+
#include "sensor_msgs/msg/joint_state.hpp"
3+
#include "sensor_msgs/msg/joy.hpp"
4+
#include <rclcpp/rclcpp.hpp>
55

66
#include "rover_msgs/msg/arm_command.hpp"
7-
#include <thread>
87
#include <chrono>
8+
#include <thread>
99

10+
#ifdef BUILDING_NEW_DRIVER
1011
#include "moteus.h"
11-
#include <arm_hardware_interface/ArmSerialProtocol.h>
12+
using namespace mjbots;
13+
#endif
1214

15+
#include <arm_hardware_interface/ArmSerialProtocol.h>
1316

1417
#include <serial/serial.h>
1518

@@ -22,91 +25,87 @@
2225
#define AXIS_1_DIR 1
2326
#define AXIS_2_DIR 1
2427
#define AXIS_3_DIR 1
25-
#define AXIS_4_DIR 1
28+
#define AXIS_4_DIR 1
2629
#define AXIS_5_DIR 1
2730
#define AXIS_6_DIR 1
2831

2932
using std::string;
30-
using namespace mjbots;
3133

3234
class ArmSerial : public rclcpp::Node {
3335
public:
34-
ArmSerial();
35-
36-
// float firm
36+
ArmSerial();
3737

38-
rover_msgs::msg::ArmCommand current_arm_status;
38+
// float firm
3939

40-
// angle_echo.positions.resize(NUM_JOINTS);
41-
// void start_rx() {
42-
// serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
43-
// }
44-
// string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm urdf
45-
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
40+
rover_msgs::msg::ArmCommand current_arm_status;
4641

42+
// angle_echo.positions.resize(NUM_JOINTS);
43+
// void start_rx() {
44+
// serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
45+
// }
46+
// string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm
47+
// urdf
48+
string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3", "joint_4",
49+
"joint_5", "joint_6", "finger_left_joint", "finger_right_joint"}; //? newest (Sep 2024) arm urdf
4750

4851
private:
52+
unsigned long baud = 115200; // 19200;
53+
string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
4954

50-
unsigned long baud = 115200;//19200;
51-
string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
52-
53-
serial::Serial teensy;
54-
serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
55-
55+
serial::Serial teensy;
56+
serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
5657

58+
#ifdef BUILDING_NEW_DRIVER
5759
std::map<int, std::shared_ptr<moteus::Controller>> controllers;
5860
std::map<int, moteus::Query::Result> servo_data;
5961
std::shared_ptr<moteus::Transport> transport;
60-
bool send_angles = true;
61-
62-
struct Axis{
63-
float curr_pos;
64-
float target_pos;
65-
float speed;
66-
float zero_rad;
67-
float max_rad;
68-
int dir;
69-
};
70-
71-
Axis axes[NUM_JOINTS];
72-
// Axis axis_EE;
73-
74-
float target_position[NUM_JOINTS];
75-
float target_velocities[NUM_JOINTS];
76-
77-
int homed = 0;
78-
bool homing = false;
79-
float EE = 0;
80-
volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
81-
volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
82-
volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
83-
84-
rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
85-
void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
86-
87-
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
88-
89-
sensor_msgs::msg::JointState prev_joint_states; //for sim
90-
rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
91-
92-
rclcpp::TimerBase::SharedPtr timer_;
93-
94-
std::thread serialRxThread;
95-
96-
void serial_rx(); //? should be static inline?
97-
float degToRad(float deg);
98-
float firmToMoveitOffsetPos(float deg, int axis);
99-
float firmToMoveitOffsetVel(float deg, int axis);
100-
void send_position_command(float pos[NUM_JOINTS]);
101-
void send_velocity_command(float vel[NUM_JOINTS]);
102-
void send_test_limits_command();
103-
void sendHomeCmd(int target_axis);
104-
void sendCommCmd(int target_state);
105-
void sendMsg(std::string outMsg);
106-
void parseArmAngleUart(std::string msg);
107-
void parseLimitSwitchTest(std::string msg);
62+
#endif
63+
bool send_angles = true;
64+
65+
struct Axis {
66+
float curr_pos;
67+
float target_pos;
68+
float speed;
69+
float zero_rad;
70+
float max_rad;
71+
int dir;
10872
};
109-
11073

74+
Axis axes[NUM_JOINTS];
75+
// Axis axis_EE;
76+
77+
float target_position[NUM_JOINTS];
78+
float target_velocities[NUM_JOINTS];
79+
80+
int homed = 0;
81+
bool homing = false;
82+
float EE = 0;
83+
volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
84+
volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
85+
volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
86+
87+
rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
88+
void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
89+
90+
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
91+
92+
sensor_msgs::msg::JointState prev_joint_states; // for sim
93+
rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
94+
95+
rclcpp::TimerBase::SharedPtr timer_;
11196

97+
std::thread serialRxThread;
11298

99+
void serial_rx(); //? should be static inline?
100+
float degToRad(float deg);
101+
float firmToMoveitOffsetPos(float deg, int axis);
102+
float firmToMoveitOffsetVel(float deg, int axis);
103+
void send_position_command(float pos[NUM_JOINTS]);
104+
void send_velocity_command(float vel[NUM_JOINTS]);
105+
void send_test_limits_command();
106+
void sendHomeCmd(int target_axis);
107+
void sendCommCmd(int target_state);
108+
void sendMsg(std::string outMsg);
109+
void parseArmAngleUart(std::string msg);
110+
void parseLimitSwitchTest(std::string msg);
111+
};

‎src/arm_hardware_interface/moteus‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
Subproject commit 1ed7b671b943d2e3e9601428d90366586168b680

0 commit comments

Comments
 (0)