Skip to content

Commit 6c31731

Browse files
Code style fixes for Jazzy (#1)
* Code styling updates for default tests * Add CI * Add missing dependency on can_msgs * Add repo dependency on clearpath_msgs for source CI * Add clearpath_ros2_socketcan_interface dependency repo
1 parent ab92ca8 commit 6c31731

File tree

8 files changed

+420
-452
lines changed

8 files changed

+420
-452
lines changed

.github/workflows/ci.yml

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
name: clearpath_motor_drivers_ci
2+
3+
on:
4+
push:
5+
pull_request:
6+
schedule:
7+
- cron: "0 0 * * *" # every day at midnight
8+
9+
jobs:
10+
build_and_test:
11+
name: jazzy
12+
strategy:
13+
matrix:
14+
env:
15+
- {ROS_DISTRO: jazzy, ROS_REPO: testing}
16+
- {ROS_DISTRO: jazzy, ROS_REPO: main}
17+
fail-fast: false
18+
runs-on: ubuntu-24.04
19+
steps:
20+
- uses: actions/checkout@v3
21+
- uses: 'ros-industrial/industrial_ci@master'
22+
env: ${{matrix.env}}
23+
24+
clearpath_motor_drivers_src_ci:
25+
name: Jazzy Clearpath Source
26+
runs-on: ubuntu-24.04
27+
steps:
28+
- uses: actions/checkout@v3
29+
- uses: ros-tooling/[email protected]
30+
with:
31+
required-ros-distributions: jazzy
32+
- uses: ros-tooling/[email protected]
33+
id: action_ros_ci_step
34+
with:
35+
target-ros2-distro: jazzy
36+
package-name: |
37+
puma_motor_driver
38+
vcs-repo-file-url: dependencies.repos

dependencies.repos

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
repositories:
2+
clearpath_msgs:
3+
type: git
4+
url: https://github.com/clearpathrobotics/clearpath_msgs.git
5+
version: jazzy-2.0RC
6+
clearpath_ros2_socketcan_interface:
7+
type: git
8+
url: https://github.com/clearpathrobotics/clearpath_ros2_socketcan_interface.git
9+
version: jazzy-2.0RC

puma_motor_driver/include/puma_motor_driver/can_proto.hpp

Lines changed: 41 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -130,8 +130,7 @@
130130
// The Stellaris Motor Class Control Voltage API definitions.
131131
//
132132
//*****************************************************************************
133-
#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
134-
CAN_API_MC_VOLTAGE)
133+
#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE)
135134
#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S))
136135
#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S))
137136
#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S))
@@ -157,8 +156,7 @@
157156
// The Stellaris Motor Class Speed Control API definitions.
158157
//
159158
//*****************************************************************************
160-
#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
161-
CAN_API_MC_SPD)
159+
#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD)
162160
#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S))
163161
#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S))
164162
#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S))
@@ -173,8 +171,7 @@
173171
// The Stellaris Motor Control Voltage Compensation Control API definitions.
174172
//
175173
//*****************************************************************************
176-
#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
177-
CAN_API_MC_VCOMP)
174+
#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP)
178175
#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S))
179176
#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S))
180177
#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S))
@@ -187,8 +184,7 @@
187184
// The Stellaris Motor Class Position Control API definitions.
188185
//
189186
//*****************************************************************************
190-
#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
191-
CAN_API_MC_POS)
187+
#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS)
192188
#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S))
193189
#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S))
194190
#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S))
@@ -203,8 +199,7 @@
203199
// The Stellaris Motor Class Current Control API definitions.
204200
//
205201
//*****************************************************************************
206-
#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
207-
CAN_API_MC_ICTRL)
202+
#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL)
208203
#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S))
209204
#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S))
210205
#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S))
@@ -232,8 +227,7 @@
232227
// The Stellaris Motor Class Status API definitions.
233228
//
234229
//*****************************************************************************
235-
#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
236-
CAN_API_MC_STATUS)
230+
#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS)
237231
#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S))
238232
#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S))
239233
#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S))
@@ -274,8 +268,7 @@
274268
// The Stellaris Motor Class Configuration API definitions.
275269
//
276270
//*****************************************************************************
277-
#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
278-
CAN_API_MC_CFG)
271+
#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG)
279272
#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S))
280273
#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S))
281274
#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S))
@@ -298,8 +291,7 @@
298291
// The Stellaris ACK API definition.
299292
//
300293
//*****************************************************************************
301-
#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
302-
CAN_API_MC_ACK)
294+
#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK)
303295

304296
//*****************************************************************************
305297
//
@@ -350,8 +342,7 @@
350342
// The Stellaris Motor Class Periodic Status API definitions.
351343
//
352344
//*****************************************************************************
353-
#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
354-
CAN_API_MC_PSTAT)
345+
#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT)
355346
#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S))
356347
#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S))
357348
#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S))
@@ -372,39 +363,37 @@
372363
// little-endian, therefore B0 is the least significant byte.
373364
//
374365
//*****************************************************************************
375-
#define LM_PSTAT_END 0
376-
#define LM_PSTAT_VOLTOUT_B0 1
377-
#define LM_PSTAT_VOLTOUT_B1 2
378-
#define LM_PSTAT_VOLTBUS_B0 3
379-
#define LM_PSTAT_VOLTBUS_B1 4
380-
#define LM_PSTAT_CURRENT_B0 5
381-
#define LM_PSTAT_CURRENT_B1 6
382-
#define LM_PSTAT_TEMP_B0 7
383-
#define LM_PSTAT_TEMP_B1 8
384-
#define LM_PSTAT_POS_B0 9
385-
#define LM_PSTAT_POS_B1 10
386-
#define LM_PSTAT_POS_B2 11
387-
#define LM_PSTAT_POS_B3 12
388-
#define LM_PSTAT_SPD_B0 13
389-
#define LM_PSTAT_SPD_B1 14
390-
#define LM_PSTAT_SPD_B2 15
391-
#define LM_PSTAT_SPD_B3 16
392-
#define LM_PSTAT_LIMIT_NCLR 17
393-
#define LM_PSTAT_LIMIT_CLR 18
394-
#define LM_PSTAT_FAULT 19
395-
#define LM_PSTAT_STKY_FLT_NCLR 20
396-
#define LM_PSTAT_STKY_FLT_CLR 21
397-
#define LM_PSTAT_VOUT_B0 22
398-
#define LM_PSTAT_VOUT_B1 23
399-
#define LM_PSTAT_FLT_COUNT_CURRENT \
400-
24
401-
#define LM_PSTAT_FLT_COUNT_TEMP 25
402-
#define LM_PSTAT_FLT_COUNT_VOLTBUS \
403-
26
404-
#define LM_PSTAT_FLT_COUNT_GATE 27
405-
#define LM_PSTAT_FLT_COUNT_COMM 28
406-
#define LM_PSTAT_CANSTS 29
407-
#define LM_PSTAT_CANERR_B0 30
408-
#define LM_PSTAT_CANERR_B1 31
366+
#define LM_PSTAT_END 0
367+
#define LM_PSTAT_VOLTOUT_B0 1
368+
#define LM_PSTAT_VOLTOUT_B1 2
369+
#define LM_PSTAT_VOLTBUS_B0 3
370+
#define LM_PSTAT_VOLTBUS_B1 4
371+
#define LM_PSTAT_CURRENT_B0 5
372+
#define LM_PSTAT_CURRENT_B1 6
373+
#define LM_PSTAT_TEMP_B0 7
374+
#define LM_PSTAT_TEMP_B1 8
375+
#define LM_PSTAT_POS_B0 9
376+
#define LM_PSTAT_POS_B1 10
377+
#define LM_PSTAT_POS_B2 11
378+
#define LM_PSTAT_POS_B3 12
379+
#define LM_PSTAT_SPD_B0 13
380+
#define LM_PSTAT_SPD_B1 14
381+
#define LM_PSTAT_SPD_B2 15
382+
#define LM_PSTAT_SPD_B3 16
383+
#define LM_PSTAT_LIMIT_NCLR 17
384+
#define LM_PSTAT_LIMIT_CLR 18
385+
#define LM_PSTAT_FAULT 19
386+
#define LM_PSTAT_STKY_FLT_NCLR 20
387+
#define LM_PSTAT_STKY_FLT_CLR 21
388+
#define LM_PSTAT_VOUT_B0 22
389+
#define LM_PSTAT_VOUT_B1 23
390+
#define LM_PSTAT_FLT_COUNT_CURRENT 24
391+
#define LM_PSTAT_FLT_COUNT_TEMP 25
392+
#define LM_PSTAT_FLT_COUNT_VOLTBUS 26
393+
#define LM_PSTAT_FLT_COUNT_GATE 27
394+
#define LM_PSTAT_FLT_COUNT_COMM 28
395+
#define LM_PSTAT_CANSTS 29
396+
#define LM_PSTAT_CANERR_B0 30
397+
#define LM_PSTAT_CANERR_B1 31
409398

410399
#endif // __CAN_PROTO_H__

puma_motor_driver/include/puma_motor_driver/driver.hpp

Lines changed: 20 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,11 @@ namespace puma_motor_driver
4040
class Driver
4141
{
4242
public:
43-
Driver(const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
43+
Driver(
44+
const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
4445
std::shared_ptr<rclcpp::Node> nh,
45-
const uint8_t& device_number,
46-
const std::string& device_name);
46+
const uint8_t & device_number,
47+
const std::string & device_name);
4748

4849
void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg);
4950

@@ -394,21 +395,21 @@ class Driver
394395
*
395396
* @return pointer to raw 4 bytes of the P gain response.
396397
*/
397-
uint8_t* getRawP();
398+
uint8_t * getRawP();
398399
/**
399400
* Process the last received I gain
400401
* for the current control mode.
401402
*
402403
* @return pointer to raw 4 bytes of the I gain response.
403404
*/
404-
uint8_t* getRawI();
405+
uint8_t * getRawI();
405406
/**
406407
* Process the last received I gain
407408
* for the current control mode.
408409
*
409410
* @return pointer to raw 4 bytes of the I gain response.
410411
*/
411-
uint8_t* getRawD();
412+
uint8_t * getRawD();
412413
/**
413414
* Process the last received set-point response
414415
* in voltage open-loop control.
@@ -438,11 +439,9 @@ class Driver
438439
*/
439440
double statusPositionGet();
440441

442+
std::string deviceName() const {return device_name_;}
441443

442-
443-
std::string deviceName() const { return device_name_; }
444-
445-
uint8_t deviceNumber() const { return device_number_; }
444+
uint8_t deviceNumber() const {return device_number_;}
446445

447446
// Only used internally but is used for testing.
448447
struct Field
@@ -452,16 +451,15 @@ class Driver
452451

453452
float interpretFixed8x8()
454453
{
455-
return *(reinterpret_cast<int16_t*>(data)) / static_cast<float>(1<<8);
454+
return *(reinterpret_cast<int16_t *>(data)) / static_cast<float>(1 << 8);
456455
}
457456

458457
double interpretFixed16x16()
459458
{
460-
return *(reinterpret_cast<int32_t*>(data)) / static_cast<double>(1<<16);
459+
return *(reinterpret_cast<int32_t *>(data)) / static_cast<double>(1 << 16);
461460
}
462461
};
463462

464-
465463
private:
466464
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
467465
std::shared_ptr<rclcpp::Node> nh_;
@@ -497,15 +495,15 @@ class Driver
497495
*
498496
* @return boolean if received is equal to expected.
499497
*/
500-
bool verifyRaw16x16(const uint8_t* received, const double expected);
498+
bool verifyRaw16x16(const uint8_t * received, const double expected);
501499

502500
/**
503501
* Comparing the raw bytes of the 8x8 fixed-point numbers
504502
* to avoid comparing the floating point values.
505503
*
506504
* @return boolean if received is equal to expected.
507505
*/
508-
bool verifyRaw8x8(const uint8_t* received, const float expected);
506+
bool verifyRaw8x8(const uint8_t * received, const float expected);
509507

510508
Field voltage_fields_[4];
511509
Field spd_fields_[7];
@@ -515,13 +513,13 @@ class Driver
515513
Field status_fields_[15];
516514
Field cfg_fields_[15];
517515

518-
Field* voltageFieldForMessage(uint32_t api);
519-
Field* spdFieldForMessage(uint32_t api);
520-
Field* vcompFieldForMessage(uint32_t api);
521-
Field* posFieldForMessage(uint32_t api);
522-
Field* ictrlFieldForMessage(uint32_t api);
523-
Field* statusFieldForMessage(uint32_t api);
524-
Field* cfgFieldForMessage(uint32_t api);
516+
Field * voltageFieldForMessage(uint32_t api);
517+
Field * spdFieldForMessage(uint32_t api);
518+
Field * vcompFieldForMessage(uint32_t api);
519+
Field * posFieldForMessage(uint32_t api);
520+
Field * ictrlFieldForMessage(uint32_t api);
521+
Field * statusFieldForMessage(uint32_t api);
522+
Field * cfgFieldForMessage(uint32_t api);
525523
};
526524

527525
} // namespace puma_motor_driver

puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp

Lines changed: 24 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -38,33 +38,35 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
3838
#include "puma_motor_driver/driver.hpp"
3939
// #include "puma_motor_driver/diagnostic_updater.hpp"
4040

41-
namespace FeedbackBit{
42-
enum
43-
{
44-
DutyCycle,
45-
Current,
46-
Position,
47-
Speed,
48-
Setpoint,
49-
Count,
50-
};
41+
namespace FeedbackBit
42+
{
43+
enum
44+
{
45+
DutyCycle,
46+
Current,
47+
Position,
48+
Speed,
49+
Setpoint,
50+
Count,
5151
};
52+
}
5253

53-
namespace StatusBit{
54-
enum
55-
{
56-
BusVoltage,
57-
OutVoltage,
58-
AnalogInput,
59-
Temperature,
60-
Mode,
61-
Fault,
62-
Count,
63-
};
54+
namespace StatusBit
55+
{
56+
enum
57+
{
58+
BusVoltage,
59+
OutVoltage,
60+
AnalogInput,
61+
Temperature,
62+
Mode,
63+
Fault,
64+
Count,
6465
};
66+
}
6567

6668
class MultiPumaNode
67-
: public rclcpp::Node
69+
: public rclcpp::Node
6870
{
6971
public:
7072
MultiPumaNode(const std::string node_name);

puma_motor_driver/package.xml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,9 +9,10 @@
99

1010
<buildtool_depend>ament_cmake</buildtool_depend>
1111

12+
<depend>can_msgs</depend>
1213
<depend>clearpath_motor_msgs</depend>
13-
<depend>rclcpp</depend>
1414
<depend>clearpath_ros2_socketcan_interface</depend>
15+
<depend>rclcpp</depend>
1516
<depend>sensor_msgs</depend>
1617
<depend>std_msgs</depend>
1718

0 commit comments

Comments
 (0)