Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Code style fixes for Jazzy #1

Open
wants to merge 7 commits into
base: jazzy-2.0RC
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 38 additions & 0 deletions .github/workflows/ci.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
name: clearpath_motor_drivers_ci

on:
push:
pull_request:
schedule:
- cron: "0 0 * * *" # every day at midnight

jobs:
build_and_test:
name: jazzy
strategy:
matrix:
env:
- {ROS_DISTRO: jazzy, ROS_REPO: testing}
- {ROS_DISTRO: jazzy, ROS_REPO: main}
fail-fast: false
runs-on: ubuntu-24.04
steps:
- uses: actions/checkout@v3
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}

clearpath_motor_drivers_src_ci:
name: Jazzy Clearpath Source
runs-on: ubuntu-24.04
steps:
- uses: actions/checkout@v3
- uses: ros-tooling/[email protected]
with:
required-ros-distributions: jazzy
- uses: ros-tooling/[email protected]
id: action_ros_ci_step
with:
target-ros2-distro: jazzy
package-name: |
puma_motor_driver
vcs-repo-file-url: dependencies.repos
9 changes: 9 additions & 0 deletions dependencies.repos
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
repositories:
clearpath_msgs:
type: git
url: https://github.com/clearpathrobotics/clearpath_msgs.git
version: jazzy-2.0RC
clearpath_ros2_socketcan_interface:
type: git
url: https://github.com/clearpathrobotics/clearpath_ros2_socketcan_interface.git
version: jazzy-2.0RC
93 changes: 41 additions & 52 deletions puma_motor_driver/include/puma_motor_driver/can_proto.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,8 +130,7 @@
// The Stellaris Motor Class Control Voltage API definitions.
//
//*****************************************************************************
#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_VOLTAGE)
#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE)
#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S))
#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S))
#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S))
Expand All @@ -157,8 +156,7 @@
// The Stellaris Motor Class Speed Control API definitions.
//
//*****************************************************************************
#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_SPD)
#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD)
#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S))
#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S))
#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S))
Expand All @@ -173,8 +171,7 @@
// The Stellaris Motor Control Voltage Compensation Control API definitions.
//
//*****************************************************************************
#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_VCOMP)
#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP)
#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S))
#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S))
#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S))
Expand All @@ -187,8 +184,7 @@
// The Stellaris Motor Class Position Control API definitions.
//
//*****************************************************************************
#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_POS)
#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS)
#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S))
#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S))
#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S))
Expand All @@ -203,8 +199,7 @@
// The Stellaris Motor Class Current Control API definitions.
//
//*****************************************************************************
#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_ICTRL)
#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL)
#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S))
#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S))
#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S))
Expand Down Expand Up @@ -232,8 +227,7 @@
// The Stellaris Motor Class Status API definitions.
//
//*****************************************************************************
#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_STATUS)
#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS)
#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S))
#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S))
#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S))
Expand Down Expand Up @@ -274,8 +268,7 @@
// The Stellaris Motor Class Configuration API definitions.
//
//*****************************************************************************
#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_CFG)
#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG)
#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S))
#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S))
#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S))
Expand All @@ -298,8 +291,7 @@
// The Stellaris ACK API definition.
//
//*****************************************************************************
#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_ACK)
#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK)

//*****************************************************************************
//
Expand Down Expand Up @@ -350,8 +342,7 @@
// The Stellaris Motor Class Periodic Status API definitions.
//
//*****************************************************************************
#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \
CAN_API_MC_PSTAT)
#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT)
#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S))
#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S))
#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S))
Expand All @@ -372,39 +363,37 @@
// little-endian, therefore B0 is the least significant byte.
//
//*****************************************************************************
#define LM_PSTAT_END 0
#define LM_PSTAT_VOLTOUT_B0 1
#define LM_PSTAT_VOLTOUT_B1 2
#define LM_PSTAT_VOLTBUS_B0 3
#define LM_PSTAT_VOLTBUS_B1 4
#define LM_PSTAT_CURRENT_B0 5
#define LM_PSTAT_CURRENT_B1 6
#define LM_PSTAT_TEMP_B0 7
#define LM_PSTAT_TEMP_B1 8
#define LM_PSTAT_POS_B0 9
#define LM_PSTAT_POS_B1 10
#define LM_PSTAT_POS_B2 11
#define LM_PSTAT_POS_B3 12
#define LM_PSTAT_SPD_B0 13
#define LM_PSTAT_SPD_B1 14
#define LM_PSTAT_SPD_B2 15
#define LM_PSTAT_SPD_B3 16
#define LM_PSTAT_LIMIT_NCLR 17
#define LM_PSTAT_LIMIT_CLR 18
#define LM_PSTAT_FAULT 19
#define LM_PSTAT_STKY_FLT_NCLR 20
#define LM_PSTAT_STKY_FLT_CLR 21
#define LM_PSTAT_VOUT_B0 22
#define LM_PSTAT_VOUT_B1 23
#define LM_PSTAT_FLT_COUNT_CURRENT \
24
#define LM_PSTAT_FLT_COUNT_TEMP 25
#define LM_PSTAT_FLT_COUNT_VOLTBUS \
26
#define LM_PSTAT_FLT_COUNT_GATE 27
#define LM_PSTAT_FLT_COUNT_COMM 28
#define LM_PSTAT_CANSTS 29
#define LM_PSTAT_CANERR_B0 30
#define LM_PSTAT_CANERR_B1 31
#define LM_PSTAT_END 0
#define LM_PSTAT_VOLTOUT_B0 1
#define LM_PSTAT_VOLTOUT_B1 2
#define LM_PSTAT_VOLTBUS_B0 3
#define LM_PSTAT_VOLTBUS_B1 4
#define LM_PSTAT_CURRENT_B0 5
#define LM_PSTAT_CURRENT_B1 6
#define LM_PSTAT_TEMP_B0 7
#define LM_PSTAT_TEMP_B1 8
#define LM_PSTAT_POS_B0 9
#define LM_PSTAT_POS_B1 10
#define LM_PSTAT_POS_B2 11
#define LM_PSTAT_POS_B3 12
#define LM_PSTAT_SPD_B0 13
#define LM_PSTAT_SPD_B1 14
#define LM_PSTAT_SPD_B2 15
#define LM_PSTAT_SPD_B3 16
#define LM_PSTAT_LIMIT_NCLR 17
#define LM_PSTAT_LIMIT_CLR 18
#define LM_PSTAT_FAULT 19
#define LM_PSTAT_STKY_FLT_NCLR 20
#define LM_PSTAT_STKY_FLT_CLR 21
#define LM_PSTAT_VOUT_B0 22
#define LM_PSTAT_VOUT_B1 23
#define LM_PSTAT_FLT_COUNT_CURRENT 24
#define LM_PSTAT_FLT_COUNT_TEMP 25
#define LM_PSTAT_FLT_COUNT_VOLTBUS 26
#define LM_PSTAT_FLT_COUNT_GATE 27
#define LM_PSTAT_FLT_COUNT_COMM 28
#define LM_PSTAT_CANSTS 29
#define LM_PSTAT_CANERR_B0 30
#define LM_PSTAT_CANERR_B1 31

#endif // __CAN_PROTO_H__
42 changes: 20 additions & 22 deletions puma_motor_driver/include/puma_motor_driver/driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,11 @@ namespace puma_motor_driver
class Driver
{
public:
Driver(const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
Driver(
const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
std::shared_ptr<rclcpp::Node> nh,
const uint8_t& device_number,
const std::string& device_name);
const uint8_t & device_number,
const std::string & device_name);

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

Expand Down Expand Up @@ -394,21 +395,21 @@ class Driver
*
* @return pointer to raw 4 bytes of the P gain response.
*/
uint8_t* getRawP();
uint8_t * getRawP();
/**
* Process the last received I gain
* for the current control mode.
*
* @return pointer to raw 4 bytes of the I gain response.
*/
uint8_t* getRawI();
uint8_t * getRawI();
/**
* Process the last received I gain
* for the current control mode.
*
* @return pointer to raw 4 bytes of the I gain response.
*/
uint8_t* getRawD();
uint8_t * getRawD();
/**
* Process the last received set-point response
* in voltage open-loop control.
Expand Down Expand Up @@ -438,11 +439,9 @@ class Driver
*/
double statusPositionGet();

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


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

uint8_t deviceNumber() const { return device_number_; }
uint8_t deviceNumber() const {return device_number_;}

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

float interpretFixed8x8()
{
return *(reinterpret_cast<int16_t*>(data)) / static_cast<float>(1<<8);
return *(reinterpret_cast<int16_t *>(data)) / static_cast<float>(1 << 8);
}

double interpretFixed16x16()
{
return *(reinterpret_cast<int32_t*>(data)) / static_cast<double>(1<<16);
return *(reinterpret_cast<int32_t *>(data)) / static_cast<double>(1 << 16);
}
};


private:
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
std::shared_ptr<rclcpp::Node> nh_;
Expand Down Expand Up @@ -497,15 +495,15 @@ class Driver
*
* @return boolean if received is equal to expected.
*/
bool verifyRaw16x16(const uint8_t* received, const double expected);
bool verifyRaw16x16(const uint8_t * received, const double expected);

/**
* Comparing the raw bytes of the 8x8 fixed-point numbers
* to avoid comparing the floating point values.
*
* @return boolean if received is equal to expected.
*/
bool verifyRaw8x8(const uint8_t* received, const float expected);
bool verifyRaw8x8(const uint8_t * received, const float expected);

Field voltage_fields_[4];
Field spd_fields_[7];
Expand All @@ -515,13 +513,13 @@ class Driver
Field status_fields_[15];
Field cfg_fields_[15];

Field* voltageFieldForMessage(uint32_t api);
Field* spdFieldForMessage(uint32_t api);
Field* vcompFieldForMessage(uint32_t api);
Field* posFieldForMessage(uint32_t api);
Field* ictrlFieldForMessage(uint32_t api);
Field* statusFieldForMessage(uint32_t api);
Field* cfgFieldForMessage(uint32_t api);
Field * voltageFieldForMessage(uint32_t api);
Field * spdFieldForMessage(uint32_t api);
Field * vcompFieldForMessage(uint32_t api);
Field * posFieldForMessage(uint32_t api);
Field * ictrlFieldForMessage(uint32_t api);
Field * statusFieldForMessage(uint32_t api);
Field * cfgFieldForMessage(uint32_t api);
};

} // namespace puma_motor_driver
Expand Down
46 changes: 24 additions & 22 deletions puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,33 +38,35 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
#include "puma_motor_driver/driver.hpp"
// #include "puma_motor_driver/diagnostic_updater.hpp"

namespace FeedbackBit{
enum
{
DutyCycle,
Current,
Position,
Speed,
Setpoint,
Count,
};
namespace FeedbackBit
{
enum
{
DutyCycle,
Current,
Position,
Speed,
Setpoint,
Count,
};
}

namespace StatusBit{
enum
{
BusVoltage,
OutVoltage,
AnalogInput,
Temperature,
Mode,
Fault,
Count,
};
namespace StatusBit
{
enum
{
BusVoltage,
OutVoltage,
AnalogInput,
Temperature,
Mode,
Fault,
Count,
};
}

class MultiPumaNode
: public rclcpp::Node
: public rclcpp::Node
{
public:
MultiPumaNode(const std::string node_name);
Expand Down
3 changes: 2 additions & 1 deletion puma_motor_driver/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,10 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>can_msgs</depend>
<depend>clearpath_motor_msgs</depend>
<depend>rclcpp</depend>
<depend>clearpath_ros2_socketcan_interface</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>

Expand Down
Loading
Loading