diff --git a/src/esw/can_library/can_manager.hpp b/src/esw/can_library/can_manager.hpp index b3ea84abe..92c4ab061 100644 --- a/src/esw/can_library/can_manager.hpp +++ b/src/esw/can_library/can_manager.hpp @@ -24,16 +24,22 @@ concept TriviallyCopyable = std::is_trivially_copyable_v; class CANManager { public: - CANManager(ros::NodeHandle& n, const std::string& name) { - m_can_publisher = n.advertise("can_requests", 1); + CANManager(ros::NodeHandle& nh, const std::string& name) { + m_can_publisher = nh.advertise("can_requests", 1); std::string can_bus_name = "can/" + name + "/bus"; - assert(n.getParam(can_bus_name, m_bus)); + assert(nh.hasParam(can_bus_name)); + int bus; + nh.getParam(can_bus_name, bus); + m_bus = static_cast(bus); std::string can_id_name = "can/" + name + "/id"; - assert(n.getParam(can_id_name, m_id)); + assert(nh.hasParam(can_id_name)); + int id; + nh.getParam(can_id_name, id); + m_id = static_cast(id); - assert(n.hasParam("can/messages")); XmlRpc::XmlRpcValue can_messages; - n.getParam("can/messages", can_messages); + assert(nh.hasParam("can/messages")); + nh.getParam("can/messages", can_messages); assert(can_messages.getType() == XmlRpc::XmlRpcValue::TypeStruct); for (auto [messageName, messageId]: can_messages) { if (messageId.getType() == XmlRpc::XmlRpcValue::TypeInt) { diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 169ee9ff4..bdbbcb831 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -33,7 +33,8 @@ int main(int argc, char** argv) { // Load motor multipliers from the ROS parameter server XmlRpc::XmlRpcValue driveControllers; - assert(nh.getParam("drive/controllers", driveControllers)); + assert(nh.hasParam("drive/controllers")); + nh.getParam("drive/controllers", driveControllers); assert(driveControllers.getType() == XmlRpc::XmlRpcValue::TypeStruct); for (const auto& driveName: driveNames) { assert(driveControllers.hasMember(driveName)); @@ -46,20 +47,24 @@ int main(int argc, char** argv) { // Load rover dimensions and other parameters from the ROS parameter server float roverWidth = 0.0f; float roverLength = 0.0f; - assert(nh.getParam("rover/width", roverWidth)); - assert(nh.getParam("rover/length", roverLength)); + assert(nh.hasParam("rover/width")); + nh.getParam("rover/width", roverWidth); + assert(nh.hasParam("rover/length")); + nh.getParam("rover/length", roverLength); WHEEL_DISTANCE_INNER = roverWidth / 2.0f; WHEEL_DISTANCE_OUTER = std::sqrt(((roverWidth / 2.0f) * (roverWidth / 2.0f)) + ((roverLength / 2.0f) * (roverLength / 2.0f))); float ratioMotorToWheel = 0.0f; - assert(nh.getParam("wheel/gear_ratio", ratioMotorToWheel)); + assert(nh.hasParam("wheel/gear_ratio")); + nh.getParam("wheel/gear_ratio", ratioMotorToWheel); // To convert m/s to rev/s, multiply by this constant. Divide by circumference, multiply by gear ratio. float wheelRadius = 0.0f; nh.getParam("wheel/radius", wheelRadius); WHEELS_M_S_TO_MOTOR_REV_S = (1.0f / (wheelRadius * 2.0f * static_cast(std::numbers::pi))) * ratioMotorToWheel; float maxSpeedMPerS = 0.0f; - assert(nh.getParam("rover/max_speed", maxSpeedMPerS)); + assert(nh.hasParam("rover/max_speed")); + nh.getParam("rover/max_speed", maxSpeedMPerS); assert(maxSpeedMPerS > 0.0f); MAX_MOTOR_SPEED_REV_S = maxSpeedMPerS * WHEELS_M_S_TO_MOTOR_REV_S; diff --git a/src/esw/motor_library/brushed.hpp b/src/esw/motor_library/brushed.hpp index bc9504dc4..bd4b7e95c 100644 --- a/src/esw/motor_library/brushed.hpp +++ b/src/esw/motor_library/brushed.hpp @@ -11,7 +11,7 @@ class BrushedController : public Controller { void set_desired_velocity(float velocity) override; void set_desired_position(float position) override; - BrushedController(ros::NodeHandle& n, const std::string& name) : Controller(n, name) {} + BrushedController(ros::NodeHandle& nh, const std::string& name) : Controller(nh, name) {} ~BrushedController() override = default; private: diff --git a/src/esw/motor_library/brushless.hpp b/src/esw/motor_library/brushless.hpp index a51a2bfa4..d9c046a6c 100644 --- a/src/esw/motor_library/brushless.hpp +++ b/src/esw/motor_library/brushless.hpp @@ -54,7 +54,7 @@ class BrushlessController : public Controller { void set_desired_velocity(float velocity) override; // in rev/s void set_desired_position(float position) override; - BrushlessController(ros::NodeHandle& n, const std::string& name) : Controller(n, name) { + BrushlessController(ros::NodeHandle& nh, const std::string& name) : Controller(nh, name) { torque = 0.3f; } ~BrushlessController() override = default; diff --git a/src/esw/motor_library/controller.hpp b/src/esw/motor_library/controller.hpp index e6ce8a84a..6c637bf93 100644 --- a/src/esw/motor_library/controller.hpp +++ b/src/esw/motor_library/controller.hpp @@ -8,7 +8,7 @@ class Controller { public: - Controller(ros::NodeHandle& n, const std::string& name) : name{name}, can_manager{CANManager{n, name}} {} + Controller(ros::NodeHandle& nh, const std::string& name) : name{name}, can_manager{CANManager{nh, name}} {} virtual ~Controller() = default; diff --git a/src/esw/motor_library/motors_manager.cpp b/src/esw/motor_library/motors_manager.cpp index ec5d5edf6..a24ac7b40 100644 --- a/src/esw/motor_library/motors_manager.cpp +++ b/src/esw/motor_library/motors_manager.cpp @@ -1,11 +1,12 @@ #include "motors_manager.hpp" -MotorsManager::MotorsManager(ros::NodeHandle& n, const std::string& groupName, const std::vector& controllerNames) { +MotorsManager::MotorsManager(ros::NodeHandle& nh, const std::string& groupName, const std::vector& controllerNames) { motorGroupName = groupName; motorNames = controllerNames; // Load motor controllers configuration from the ROS parameter server XmlRpc::XmlRpcValue controllersRoot; - assert(n.getParam("motors/controllers", controllersRoot)); + assert(nh.hasParam("motors/controllers")); + nh.getParam("motors/controllers", controllersRoot); assert(controllersRoot.getType() == XmlRpc::XmlRpcValue::TypeStruct); for (const std::string& name: controllerNames) { assert(controllersRoot[name].hasMember("type") && @@ -14,10 +15,10 @@ MotorsManager::MotorsManager(ros::NodeHandle& n, const std::string& groupName, c assert(type == "brushed" || type == "brushless"); if (type == "brushed") { - auto temp = std::make_unique(n, name); + auto temp = std::make_unique(nh, name); controllers[name] = std::move(temp); } else if (type == "brushless") { - auto temp = std::make_unique(n, name); + auto temp = std::make_unique(nh, name); controllers[name] = std::move(temp); } @@ -27,12 +28,12 @@ MotorsManager::MotorsManager(ros::NodeHandle& n, const std::string& groupName, c updateLastConnection(); // Subscribe to the ROS topic for commands - ros::Subscriber moveThrottleSub = n.subscribe(groupName + "_throttle_cmd", 1, &MotorsManager::moveMotorsThrottle, this); - ros::Subscriber moveVelocitySub = n.subscribe(groupName + "_velocity_cmd", 1, &MotorsManager::moveMotorsVelocity, this); - ros::Subscriber movePositionSub = n.subscribe(groupName + "_position_cmd", 1, &MotorsManager::moveMotorsPosition, this); + ros::Subscriber moveThrottleSub = nh.subscribe(groupName + "_throttle_cmd", 1, &MotorsManager::moveMotorsThrottle, this); + ros::Subscriber moveVelocitySub = nh.subscribe(groupName + "_velocity_cmd", 1, &MotorsManager::moveMotorsVelocity, this); + ros::Subscriber movePositionSub = nh.subscribe(groupName + "_position_cmd", 1, &MotorsManager::moveMotorsPosition, this); // Create a 0.1 second heartbeat timer - ros::Timer heartbeatTimer = n.createTimer(ros::Duration(0.1), &MotorsManager::heartbeatCallback, this); + ros::Timer heartbeatTimer = nh.createTimer(ros::Duration(0.1), &MotorsManager::heartbeatCallback, this); } Controller& MotorsManager::get_controller(std::string const& name) { diff --git a/src/esw/motor_library/motors_manager.hpp b/src/esw/motor_library/motors_manager.hpp index 80fcbba2b..6c6023ad4 100644 --- a/src/esw/motor_library/motors_manager.hpp +++ b/src/esw/motor_library/motors_manager.hpp @@ -15,7 +15,7 @@ class MotorsManager { public: MotorsManager() = default; - MotorsManager(ros::NodeHandle& n, const std::string& groupName, const std::vector& controllerNames); + MotorsManager(ros::NodeHandle& nh, const std::string& groupName, const std::vector& controllerNames); Controller& get_controller(std::string const& name);