Skip to content

Commit e4a18de

Browse files
RoozkiRoozki
authored
Feat/rowzaw/arm pipeline refactor (#122)
* ps4 controller map added fix deps fix setup script * happy with current state * nit * added cyborg and nintendo switch definitions
1 parent 10158a8 commit e4a18de

File tree

16 files changed

+401
-89
lines changed

16 files changed

+401
-89
lines changed

‎setup_scripts/install_libsweepsdk.sh‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,9 @@ mkdir -p build
1111
cd build
1212

1313
# build and install the libsweep library
14-
cmake .. -DCMAKE_BUILD_TYPE=Release
14+
echo CONFIGUIRING SWEEPSDK
15+
cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_POLICY_VERSION_MINIMUM=3.5
16+
echo BUILDNG SWEEP
1517
cmake --build .
1618
sudo cmake --build . --target install
1719
sudo ldconfig

‎setup_scripts/setup_everything_common.sh‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ echo This script will NOT install discord. run install-discord.sh if you want di
3333
sleep 1.0
3434
echo starting with ros2...
3535

36-
#sudo apt update
36+
sudo apt update
3737
cd $ROVERFLAKE_ROOT/setup_scripts/
3838
bash install_dependencies.sh
3939
bash install-ros2-humble.sh # also runs apt update, if ros2 is not installed
@@ -67,13 +67,8 @@ done
6767
cd $ROVERFLAKE_ROOT
6868

6969
source /opt/ros/humble/setup.bash
70-
#install ros2 packages
71-
sudo rosdep init
72-
rosdep update
73-
bash setup_scripts/install_rosdeps.sh
74-
7570
# setup user enviroment
76-
mkdir ${ROVERFLAKE_ROOT}/random_install_files
71+
mkdir -p ${ROVERFLAKE_ROOT}/random_install_files
7772
echo "source ${ROVERFLAKE_ROOT}/setup_scripts/rover_env/rover_env_common.sh " >> ~/.bashrc
7873

7974

@@ -85,6 +80,13 @@ cd ${ROVERFLAKE_ROOT}
8580
bash setup_scripts/submodule_update.sh
8681

8782

83+
#install ros2 packages
84+
sudo rosdep init
85+
rosdep update
86+
bash setup_scripts/install_rosdeps.sh
87+
88+
89+
8890

8991

9092
# for package in "${ros_packages_to_install[@]}"; do

‎src/arm_control/CMakeLists.txt‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
2929
trajectory_msgs
3030
rover_msgs
3131
moteus_msgs
32+
arm_hardware_interface
3233
)
3334

3435
find_package(ament_cmake REQUIRED)
@@ -60,7 +61,9 @@ endforeach()
6061
# EXECUTABLE servo_node
6162
# )
6263
include_directories(include)
63-
64+
add_library(${PROJECT_NAME}_lib src/armconfig.cpp
65+
include/armControlParams.h include/controller_config.h)
66+
ament_target_dependencies(${PROJECT_NAME}_lib sensor_msgs ${THIS_PACKAGE_INCLUDE_DEPENDS})
6467

6568
# uncomment the following section in order to fill in
6669
# further dependencies manually.
@@ -80,6 +83,10 @@ ament_target_dependencies(cbs_arm_interface rclcpp rover_msgs sensor_msgs std_ms
8083

8184
add_executable(moteus_control_adapter src/moteus_control_adapter_node.cpp include/moteus_control_adapter_node.h)
8285
ament_target_dependencies(moteus_control_adapter ${THIS_PACKAGE_INCLUDE_DEPENDS})
86+
install(
87+
DIRECTORY include/
88+
DESTINATION include/${PROJECT_NAME}
89+
)
8390
install(
8491
TARGETS
8592
# old_arm_servo
@@ -89,14 +96,13 @@ install(
8996
sim_helper
9097
moteus_control_adapter
9198
# servo_cpp_interface_demo
99+
${PROJECT_NAME}_lib # the library
92100
DESTINATION
93101
lib/${PROJECT_NAME}
102+
ARCHIVE DESTINATION lib
103+
LIBRARY DESTINATION lib
94104
)
95-
install(
96-
DIRECTORY include/
97-
DESTINATION include/${PROJECT_NAME}/include
98-
)
99-
100105

106+
ament_export_libraries(${PROJECT_NAME})
101107
ament_export_include_directories(include)
102108
ament_package()

‎src/arm_control/include/armControlParams.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
#define POSITION_BASED 2
1313
#define NUM_JOINTS 7
1414
#define NUM_JOINTS_NO_EE 6
15-
#define EE_INDEX 6
1615
#define EE_SPEED_SCALE 1
1716
#define PI 3.14
1817

‎src/arm_control/include/controller_config.h‎

Lines changed: 246 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -7,21 +7,260 @@
77
* To switch controllers, change the ACTIVE_CONTROLLER define below.
88
* Button/axis indices can be verified by running: ros2 topic echo /joy
99
*
10+
* Joy -> Ros2 package that works with event based (/dev/input/eventx)
11+
* - This has bugs? Cannot seem to set parameters
12+
* Joy_linux -> Ros2 package that does the same thing as joy, but uses /dev/input/by-id
13+
* - This was the ros1 joy package and used to be the standard. We are able to set parameters and stuff
14+
*
1015
*/
1116
#pragma once
1217

13-
// ============ Controller Selection ============
14-
// Set ONE of these as the active controller:
15-
#define CONTROLLER_PRO_CONTROLLER 1
16-
#define CONTROLLER_CYBORG_STICK 2
17-
#define CONTROLLER_PS4_JOY_LINUX 3 //! Use joy_linux package, not joy!
18+
#include <sensor_msgs/msg/joy.hpp>
19+
#include <arm_hardware_interface/ArmSerialProtocol.h>
20+
inline static constexpr int MAX_BUTTONS = 20; // can be decreased
21+
inline static constexpr int MAX_AXES = 10;
22+
23+
24+
// ============================================================
25+
// PS4 Controller (Using Joy Linux)
26+
// ============================================================
27+
namespace ps4_index {
28+
namespace axes {
29+
static inline constexpr int LEFT_JOYSTICK_X = 0;
30+
static inline constexpr int LEFT_JOYSTICK_Y = 1;
31+
static inline constexpr int RIGHT_JOYSTICK_X = 3;
32+
static inline constexpr int RIGHT_JOYSTICK_Y = 4;
33+
34+
static inline constexpr int L2 = 2; // Left Trigger
35+
static inline constexpr int R2 = 5; // Right trigger
36+
37+
static inline constexpr int DPAD_X = 6;
38+
static inline constexpr int DPAD_Y = 7;
39+
40+
}
41+
42+
namespace buttons { // Can also be switches
43+
static inline constexpr int X = 0;
44+
static inline constexpr int CIRCLE = 1;
45+
static inline constexpr int TRIANGLE = 2;
46+
static inline constexpr int SQUARE = 3;
47+
static inline constexpr int L1 = 4; // Left bumper
48+
static inline constexpr int R1 = 5; // Right bumber
49+
static inline constexpr int SHARE = 8; // Share button (to the left of the middle trackpad)
50+
static inline constexpr int OPTIONS = 9; // Options button (to the right of the middle trackpad)
51+
static inline constexpr int PS_BUTTON = 10; // Middle playstation button in between joysticks
52+
static inline constexpr int L3 = 11; // Left joystick button
53+
static inline constexpr int R3 = 12; // Right joystic button
54+
55+
56+
}
57+
58+
//Note for PS4 Controller: Currently there is no
59+
60+
// If we need to support both joy and joy_linux, maybe something like:
61+
/*
62+
namespace your_controller
63+
{
64+
namespace joy_event {
65+
axes...
66+
buttons...
67+
}
68+
69+
namespace joy_linux {
70+
axes...
71+
buttons...
72+
}
73+
}
74+
75+
*/
76+
77+
}
78+
79+
80+
// ============================================================
81+
// Nintendo Switch Pro Controller (joy, event based - NOT JOY LINUX WE CANT GET SWITCH WORKING WITH JOY LINUX)
82+
// ============================================================
83+
84+
namespace switch_index {
85+
namespace axes {
86+
static inline constexpr int LEFT_JOYSTICK_X = 0; // Positive is Left
87+
static inline constexpr int LEFT_JOYSTICK_Y = 1; // Positive is up
88+
static inline constexpr int RIGHT_JOYSTICK_X = 2; // Positive is left
89+
static inline constexpr int RIGHT_JOYSTICK_Y = 3; // Positive is up
90+
static inline constexpr int LEFT_TRIGGER_ZL = 4; // Positive is unclicked. Negative is clicked. Only -1 and 1
91+
static inline constexpr int RIGHT_TRIGGER_ZR = 5; // Positive is unclicked. Negative is clicked. Only -1 and 1
92+
93+
}
94+
95+
namespace buttons {
96+
static inline constexpr int A = 0;
97+
static inline constexpr int B = 1;
98+
static inline constexpr int X = 2;
99+
static inline constexpr int Y = 3;
100+
static inline constexpr int MINUS = 4; // the (-) button to the left of the switch logo
101+
static inline constexpr int HOME = 5; // the (home) button to the bottom right of the switch logo
102+
static inline constexpr int PLUS = 6; // the (+) button to the right of the switch logo
103+
static inline constexpr int L3 = 7; // Left joystick button
104+
static inline constexpr int R3 = 8; // Right joystic button
105+
static inline constexpr int L1 = 9; // Left bumper
106+
static inline constexpr int R1 = 10; // Right bumper
107+
108+
static inline constexpr int DPAD_UP = 11;
109+
static inline constexpr int DPAD_DOWN = 12;
110+
static inline constexpr int DPAD_LEFT = 13;
111+
static inline constexpr int DPAD_RIGHT = 14;
112+
static inline constexpr int SQURE_CICLE = 15; // Weird button to the bottom left of the switch logo
113+
114+
}
115+
}
18116

19-
#define ACTIVE_CONTROLLER CONTROLLER_PRO_CONTROLLER
20117

21118
// ============================================================
22-
// Nintendo Switch Pro Controller
119+
// Cyborg Joystick (joy linux)
23120
// ============================================================
24121

122+
namespace cyborg_index {
123+
namespace axes {
124+
static inline constexpr int JOYSTICK_X = 0; // Left is positive
125+
static inline constexpr int JOYSTICK_Y = 1; // up is positive
126+
static inline constexpr int THROTTLE = 2; // up is 1, all the way down is -1
127+
static inline constexpr int JOYSTICK_Z = 3; // CCW is positive
128+
129+
// Thumb 4way is the dongle thing between buttons 5 and 6
130+
static inline constexpr int THUMB_4WAY_X = 4; // left is positive
131+
static inline constexpr int THUMB_4WAY_Y = 5; // left is positive
132+
}
133+
134+
namespace buttons {
135+
static inline constexpr int TRIGGER = 0;
136+
static inline constexpr int BUTTON_2 = 1;
137+
static inline constexpr int BUTTON_3 = 2;
138+
static inline constexpr int BUTTON_4 = 3;
139+
static inline constexpr int BUTTON_5 = 4; // kinda sticky
140+
static inline constexpr int BUTTON_6 = 5; // kinda sticky
141+
static inline constexpr int F1 = 6;
142+
static inline constexpr int F2 = 7;
143+
static inline constexpr int F3 = 8;
144+
static inline constexpr int F4 = 9;
145+
static inline constexpr int LEFT_UPARROW = 10;
146+
static inline constexpr int RIGHT_UPARROW = 11;
147+
}
148+
}
149+
150+
151+
namespace ArmControllerConfig { // Can make into a class later?
152+
153+
enum class GameController {
154+
PS4_JOY_LINUX, // Dualshock 4 (PS4) controller, ran from joy_linux (not joy!)
155+
SWITCH_PRO_CONTROLLER,
156+
CYBORG_JOYSTICK
157+
158+
//Also possible here:
159+
// AARON_STYLE_SWITCH_PRO
160+
// ROWAN_PRECISE_PS4
161+
// (custom mappings based on which user likes what)
162+
};
163+
164+
// Arm control input, Modelled off of control base joysticks:
165+
/* axes:
166+
- 0: left joystick x
167+
- 1: left joystick y
168+
- 2: left joystick z
169+
- 3: right joystick x
170+
- 4: right joystick y
171+
- 5: right joystick z
172+
173+
example: For a game controller, the below function will do its best to map the game controller inputs to sub out for the arm joysticks
174+
175+
Note: the above is based on control base arm joysticks "default", we can create other
176+
*/
177+
178+
float ee_speed_scale = 60;
179+
float axis_speed_scale = 10;
180+
181+
struct ArmControlInput {
182+
// Static arrays, not vectors here
183+
float fk_axes[MAX_AXES] = {}; // All are standardized from -1 to 1.
184+
float ik_axes[MAX_AXES] = {}; // All are standardized from -1 to 1.
185+
float end_effector = 0; // Standardized from -1 to 1
186+
int home = 0; // Pulse to home
187+
int kinematics_mode_switch = 0; // Pulse to change from IK to FK
188+
};
189+
190+
inline static bool process_joy_input(GameController controller, const sensor_msgs::msg::Joy::SharedPtr joy_msg, ArmControlInput &arm_control_msg)
191+
{
192+
switch (controller) {
193+
case GameController::PS4_JOY_LINUX:
194+
{
195+
using namespace ps4_index;
196+
arm_control_msg.fk_axes[AXIS_1_INDEX] = ((joy_msg->axes[axes::L2] - joy_msg->axes[axes::R2])) / (2.0f);
197+
arm_control_msg.fk_axes[AXIS_2_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_Y];
198+
arm_control_msg.fk_axes[AXIS_3_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_Y];
199+
arm_control_msg.fk_axes[AXIS_4_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_X];
200+
arm_control_msg.fk_axes[AXIS_5_INDEX] = joy_msg->axes[axes::DPAD_Y];
201+
arm_control_msg.fk_axes[AXIS_6_INDEX] = joy_msg->axes[axes::DPAD_X];
202+
203+
arm_control_msg.ik_axes[IK_ANG_Z_INDEX] = ((joy_msg->axes[axes::L2] - joy_msg->axes[axes::R2])) / (2.0f);
204+
arm_control_msg.ik_axes[IK_LIN_X_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_Y];
205+
arm_control_msg.ik_axes[IK_LIN_Z_INDEX] = joy_msg->axes[axes::RIGHT_JOYSTICK_Y];
206+
arm_control_msg.ik_axes[IK_LIN_Y_INDEX] = joy_msg->axes[axes::LEFT_JOYSTICK_X];
207+
arm_control_msg.ik_axes[IK_ANG_X_INDEX] = joy_msg->axes[axes::DPAD_X];
208+
arm_control_msg.ik_axes[IK_ANG_Y_INDEX] = joy_msg->axes[axes::DPAD_Y];
209+
210+
arm_control_msg.end_effector = joy_msg->buttons[buttons::L1] - joy_msg->buttons[buttons::R1];
211+
212+
arm_control_msg.home = joy_msg->buttons[buttons::SHARE];
213+
arm_control_msg.kinematics_mode_switch = joy_msg->buttons[buttons::CIRCLE];
214+
}
215+
break;
216+
case GameController::SWITCH_PRO_CONTROLLER:
217+
{
218+
using namespace switch_index;
219+
// TODO
220+
221+
222+
}
223+
break;
224+
case GameController::CYBORG_JOYSTICK:
225+
{
226+
using namespace cyborg_index;
227+
// TODO
228+
229+
230+
}
231+
break;
232+
default:
233+
return false; // Return Error
234+
}
235+
return true; // Return Success
236+
}
237+
238+
// Overload for drive control (later refactor)
239+
// inline static bool process_joy_input(GameController controller, sensor_msgs::msg::Joy joy_msg, DriveControlInput &input)
240+
// {
241+
242+
// switch (controller)
243+
// {
244+
// case GameController::PS4_JOY_LINUX:
245+
246+
// break;
247+
// case GameController::SWITCH_PRO_CONTROLLER:
248+
249+
// break;
250+
// case GameController::CYBORG_JOYSTICK:
251+
252+
// break;
253+
// default:
254+
// return false;
255+
256+
// return true;
257+
// }
258+
// }
259+
}
260+
261+
262+
263+
25264
// Button names (used):
26265
// 0 = BTN_EAST 1 = BTN_SOUTH
27266
// 2 = BTN_NORTH 3 = BTN_WEST
@@ -160,14 +399,3 @@ namespace ControllerConfig {
160399
#else
161400
#pragma warning "Warning: No valid ACTIVE_CONTROLLER defined in controller_config.h"
162401
#endif
163-
164-
// enum class GameController {
165-
// PS4_JOY_LINUX, // Dualshock 4 (PS4) controller, ran from joy_linux (not joy!)
166-
// SWITCH_PRO_CONTROLLER
167-
// };
168-
169-
// struct GameControllerMap {
170-
171-
// };
172-
173-
// inline static GameController

0 commit comments

Comments
 (0)