Skip to content

Commit

Permalink
Move hw functionality inside of motor_hardware
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatjazBostic
MatjazBostic committed Aug 28, 2024
1 parent c64baa2 commit 574e9b6
Show file tree
Hide file tree
Showing 5 changed files with 773 additions and 501 deletions.
11 changes: 7 additions & 4 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ ament_export_libraries(ur_serial)


# Add library
add_library(${PROJECT_NAME}_lib
add_library(${PROJECT_NAME}
src/motor_message.cc
src/motor_serial.cc
src/motor_hardware.cc
Expand All @@ -107,11 +107,13 @@ add_library(${PROJECT_NAME}_lib
# target_link_libraries(${PROJECT_NAME}_lib PUBLIC zenohcxx::zenohc::lib)

# Include directories for library
target_include_directories(${PROJECT_NAME}_lib
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${CMAKE_CURRENT_SOURCE_DIR}/serial/include # Add this line to include the serial headers
# ${controller_manager_INCLUDE_DIRS}

# ${CMAKE_CURRENT_SOURCE_DIR}/zenoh-c/include
# ${CMAKE_CURRENT_SOURCE_DIR}/zenoh-cpp/include
)
Expand All @@ -123,14 +125,15 @@ target_include_directories(${PROJECT_NAME}_lib
# )


ament_target_dependencies(${PROJECT_NAME}_lib
ament_target_dependencies(${PROJECT_NAME}
"rclcpp"
"std_msgs"
"sensor_msgs"
"diagnostic_msgs"
"hardware_interface"
"ros2_control"
"diagnostic_updater"
# "controller_manager"
# "rosidl_default_runtime"
"ubiquity_motor_ros2_msgs"
# "${PROJECT_NAME}" # This adds a dependency on the generated messages in the same package
Expand Down Expand Up @@ -168,7 +171,7 @@ target_link_libraries(motor_node Boost::thread ur_serial)
# Install targets
install(TARGETS
motor_node
${PROJECT_NAME}_lib
${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME}
)

Expand Down
88 changes: 83 additions & 5 deletions include/ubiquity_motor_ros2/motor_hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ubiquity_motor_ros2/motor_message.h>
#include <ubiquity_motor_ros2/motor_serial.h>

// #include "controller_manager/controller_manager.hpp"
// #include "hardware_interface/resource_manager.hpp"

// TODO: Enhancement - Make WHEEL_SLIP_THRESHOLD be a ROS param
#define WHEEL_SLIP_THRESHOLD (0.08) // Rotation below which we null excess wheel torque in 4wd drive_type

// Mininum hardware versions required for various features
#define MIN_HW_OPTION_SWITCH 50
// Until we have a holdoff for MCB message overruns we do this delay to be cautious
// Twice the period for status reports from MCB
// auto mcbStatusSleepPeriodNs = rclcpp::Duration::from_seconds(0.02).to_chrono<std::chrono::nanoseconds>();

struct MotorDiagnostics {
MotorDiagnostics()
Expand Down Expand Up @@ -133,16 +142,44 @@ struct MotorDiagnostics {

class MotorHardware : public hardware_interface::ActuatorInterface {
public:
MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams node_params, CommsParams serial_params, FirmwareParams firmware_params);
MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams& node_params, CommsParams& serial_params, FirmwareParams& firmware_params);
virtual ~MotorHardware();

hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
// hardware_interface::return_type start() override;
// hardware_interface::return_type stop() override;
hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
hardware_interface::return_type read(const rclcpp::Time& current_time, const rclcpp::Duration& elapsed_loop_time) override;
hardware_interface::return_type write(const rclcpp::Time& current_time, const rclcpp::Duration& elapsed_loop_time) override;

hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;


// hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override
// {

// return hardware_interface::CallbackReturn::SUCCESS;
// }

// hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
// {
// // Stop hardware operation (e.g., stop motors, stop sensor polling)
// return hardware_interface::CallbackReturn::SUCCESS;
// }

// hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override
// {
// // Cleanup resources (e.g., close connections, free memory)
// return hardware_interface::CallbackReturn::SUCCESS;
// }

// hardware_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override
// {
// // Final shutdown and release all resources
// return hardware_interface::CallbackReturn::SUCCESS;
// }


void closePort();
bool openPort();
Expand Down Expand Up @@ -186,6 +223,15 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
void getWheelJointPositions(double &leftWheelPosition, double &rightWheelPosition);
void setWheelJointVelocities(double leftWheelVelocity, double rightWheelVelocity);
void publishMotorState(void);
void initMcbParameters();

void manageMotorControllerState();
void setWheelVelocities(const rclcpp::Time& current_time, const rclcpp::Duration & elapsed_loop_time);
// hardware_interface::HardwareInfo getHwInfo();
void checkMcbReset();
void writeMotorSpeeds();


int firmware_version;
int firmware_date;
int firmware_options;
Expand All @@ -201,8 +247,11 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
int wheel_type;
double wheel_gear_ratio;
int drive_type;
int wheel_slip_nulling;

diagnostic_updater::Updater diag_updater;
// std::shared_ptr<controller_manager::ControllerManager> controller_manager = nullptr;

private:
void _addOdometryRequest(std::vector<MotorMessage>& commands) const;
void _addVelocityRequest(std::vector<MotorMessage>& commands) const;
Expand All @@ -219,11 +268,39 @@ class MotorHardware : public hardware_interface::ActuatorInterface {

rclcpp::Node::SharedPtr node;

rclcpp::Logger logger;
NodeParams& node_params;
CommsParams& serial_params;

FirmwareParams fw_params;
FirmwareParams& fw_params;
FirmwareParams prev_fw_params;

rclcpp::Logger logger;

int lastMcbEnabled;
int wheelSlipEvents;

// rclcpp::Time current_time;
// rclcpp::Time last_loop_time;
// rclcpp::Duration elapsed_loop_time;
rclcpp::Time last_sys_maint_time;
rclcpp::Time last_joint_time;
uint32_t loopIdx;
rclcpp::Rate ctrlLoopDelay;
rclcpp::Duration zeroVelocityTime;
std::chrono::nanoseconds mcbStatusSleepPeriodNs;

rclcpp::Duration sysMaintPeriod; // A periodic MCB maintenance operation
rclcpp::Duration jointUpdatePeriod; // A periodic time to update joint velocity
rclcpp::Duration wheelSlipNullingPeriod;

double leftLastWheelPos;
double rightLastWheelPos;
double leftWheelPos;
double rightWheelPos;

double estopReleaseDeadtime;
double estopReleaseDelay;

int32_t deadman_timer;

double ticks_per_radian; // Odom ticks per radian for wheel encoders in use
Expand All @@ -232,6 +309,7 @@ class MotorHardware : public hardware_interface::ActuatorInterface {

bool estop_motor_power_off; // Motor power inactive, most likely from ESTOP switch


struct Joint {
double position;
double velocity;
Expand Down
5 changes: 1 addition & 4 deletions include/ubiquity_motor_ros2/motor_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,6 @@



// TODO: Enhancement - Make WHEEL_SLIP_THRESHOLD be a ROS param
#define WHEEL_SLIP_THRESHOLD (0.08) // Rotation below which we null excess wheel torque in 4wd drive_type

static const double BILLION = 1000000000.0;


Expand All @@ -23,7 +20,7 @@ class MotorNode : public rclcpp::Node{

// void PID_update_callback(const ubiquity_motor::PIDConfig& config, uint32_t level);
void SystemControlCallback(const std_msgs::msg::String::SharedPtr msg);
void initMcbParameters();
// void initMcbParameters();
hardware_interface::HardwareInfo getHwInfo();
void run();

Expand Down
Loading

0 comments on commit 574e9b6

Please sign in to comment.