Skip to content

Commit 04d3311

Browse files
roni-kreininhilary-luo
roni-kreinin
authored and
hilary-luo
committed
Drivetrains (#250)
* Allow for only 2 motors in fan control node * Fixed logic * Added 2 joint support for lynx hardware * Switch to using motor protection message
1 parent 510f4e5 commit 04d3311

File tree

11 files changed

+71
-78
lines changed

11 files changed

+71
-78
lines changed

‎clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lighting/lighting.hpp

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,20 @@ class Lighting : public rclcpp::Node
123123
{Platform::W200, {"Front Left", "Front Right", "Rear Left", "Rear Right"}},
124124
};
125125

126+
const std::map<std::string, uint8_t> LYNX_MOTOR_TO_LIGHTS_MAP = {
127+
{"front_left_wheel_joint", clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_LEFT},
128+
{"front_right_wheel_joint", clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_RIGHT},
129+
{"rear_left_wheel_joint", clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_LEFT},
130+
{"rear_right_wheel_joint", clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_RIGHT},
131+
};
132+
133+
const std::map<std::string, std::string> LYNX_MOTOR_TO_DIAGNOSTIC_QUALIFIER_MAP = {
134+
{"front_left_wheel_joint", "Front Left "},
135+
{"front_right_wheel_joint", "Front Right "},
136+
{"rear_left_wheel_joint", "Rear Left "},
137+
{"rear_right_wheel_joint", "Rear Right "},
138+
};
139+
126140
std::map<State, Sequence> lighting_sequence_;
127141

128142
Lighting();

‎clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lynx/hardware.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
namespace clearpath_hardware_interfaces
5353
{
5454

55+
static constexpr uint8_t DIFF_DRIVE_TWO_JOINTS = 2;
5556
static constexpr uint8_t DIFF_DRIVE_FOUR_JOINTS = 4;
5657
static constexpr double MINIMUM_VELOCITY_RADS = 0.01f;
5758
static constexpr double MAXIMUM_VELOCITY_NORMAL_RADS = 12.307f;

‎clearpath_hardware_interfaces/include/clearpath_hardware_interfaces/lynx/hardware_interface.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939

4040
#include "clearpath_motor_msgs/msg/lynx_feedback.hpp"
4141
#include "clearpath_motor_msgs/msg/lynx_multi_feedback.hpp"
42+
#include "clearpath_motor_msgs/msg/lynx_motor_protection.hpp"
4243
#include "clearpath_motor_msgs/msg/lynx_system_protection.hpp"
4344

4445
namespace clearpath_hardware_interfaces

‎clearpath_hardware_interfaces/src/a300/fan_control.cpp

Lines changed: 8 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -158,21 +158,17 @@ a300_cooling::FanController::FanController() : Node("a300_fan_controller"),
158158
[this](const clearpath_motor_msgs::msg::LynxMultiStatus::SharedPtr msg)
159159
{
160160
std::lock_guard<std::mutex> lock(update_mutex_);
161-
try
161+
if (msg->drivers.size() != 2 && msg->drivers.size() != 4)
162162
{
163-
this->thermal_sensors_.setSensorValue("pcb_motor1", msg->drivers.at(0).pcb_temperature);
164-
this->thermal_sensors_.setSensorValue("mcu_motor1", msg->drivers.at(0).mcu_temperature);
165-
this->thermal_sensors_.setSensorValue("pcb_motor2", msg->drivers.at(1).pcb_temperature);
166-
this->thermal_sensors_.setSensorValue("mcu_motor2", msg->drivers.at(1).mcu_temperature);
167-
this->thermal_sensors_.setSensorValue("pcb_motor3", msg->drivers.at(2).pcb_temperature);
168-
this->thermal_sensors_.setSensorValue("mcu_motor3", msg->drivers.at(2).mcu_temperature);
169-
this->thermal_sensors_.setSensorValue("pcb_motor4", msg->drivers.at(3).pcb_temperature);
170-
this->thermal_sensors_.setSensorValue("mcu_motor4", msg->drivers.at(3).mcu_temperature);
163+
RCLCPP_ERROR(this->get_logger(),
164+
"%s topic contains an invalid number of drivers: %ld", MOTOR_TEMPERATURE_TOPIC.c_str(), msg->drivers.size());
165+
return;
171166
}
172-
catch (const std::out_of_range & e)
167+
168+
for (size_t i = 0; i < msg->drivers.size(); i++)
173169
{
174-
RCLCPP_ERROR(this->get_logger(),
175-
"%s topic does not contain 4 drivers: %s", MOTOR_TEMPERATURE_TOPIC.c_str(), e.what());
170+
this->thermal_sensors_.setSensorValue("pcb_motor" + std::to_string(i+1), msg->drivers.at(i).pcb_temperature);
171+
this->thermal_sensors_.setSensorValue("mcu_motor" + std::to_string(i+1), msg->drivers.at(i).mcu_temperature);
176172
}
177173
this->lynx_status_stale_ = 0;
178174
});

‎clearpath_hardware_interfaces/src/lighting/lighting.cpp

Lines changed: 13 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -424,7 +424,7 @@ void Lighting::updateState()
424424
}
425425

426426
// Lynx motor errors
427-
if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxSystemProtection::ERROR)
427+
if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxMotorProtection::ERROR)
428428
{
429429
setState(State::MotorFault);
430430
LightingState off_1 = LightingState(4, COLOR_RED);
@@ -434,44 +434,17 @@ void Lighting::updateState()
434434

435435
diagnostic_qualifier_ = "";
436436

437-
if (system_protection_msg_.motor_states.at(
438-
clearpath_motor_msgs::msg::LynxSystemProtection::A300_MOTOR_REAR_LEFT) == clearpath_motor_msgs::msg::LynxSystemProtection::ERROR)
437+
for (auto motor_state : system_protection_msg_.motor_states)
439438
{
440-
diagnostic_qualifier_ += "Rear Left ";
441-
off_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_LEFT) = COLOR_BLACK;
442-
on_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_LEFT) = COLOR_RED;
443-
off_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_LEFT) = COLOR_BLACK;
444-
on_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_LEFT) = COLOR_RED;
445-
}
446-
447-
if (system_protection_msg_.motor_states.at(
448-
clearpath_motor_msgs::msg::LynxSystemProtection::A300_MOTOR_FRONT_LEFT) == clearpath_motor_msgs::msg::LynxSystemProtection::ERROR)
449-
{
450-
diagnostic_qualifier_ += "Front Left ";
451-
off_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_LEFT) = COLOR_BLACK;
452-
on_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_LEFT) = COLOR_RED;
453-
off_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_LEFT) = COLOR_BLACK;
454-
on_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_LEFT) = COLOR_RED;
455-
}
456-
457-
if (system_protection_msg_.motor_states.at(
458-
clearpath_motor_msgs::msg::LynxSystemProtection::A300_MOTOR_REAR_RIGHT) == clearpath_motor_msgs::msg::LynxSystemProtection::ERROR)
459-
{
460-
diagnostic_qualifier_ += "Rear Right ";
461-
off_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_RIGHT) = COLOR_BLACK;
462-
on_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_RIGHT) = COLOR_RED;
463-
off_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_RIGHT) = COLOR_BLACK;
464-
on_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_REAR_RIGHT) = COLOR_RED;
465-
}
466-
467-
if (system_protection_msg_.motor_states.at(
468-
clearpath_motor_msgs::msg::LynxSystemProtection::A300_MOTOR_FRONT_RIGHT) == clearpath_motor_msgs::msg::LynxSystemProtection::ERROR)
469-
{
470-
diagnostic_qualifier_ += "Front Right ";
471-
off_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_RIGHT) = COLOR_BLACK;
472-
on_1.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_RIGHT) = COLOR_RED;
473-
off_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_RIGHT) = COLOR_BLACK;
474-
on_2.at(clearpath_platform_msgs::msg::Lights::A300_LIGHTS_FRONT_RIGHT) = COLOR_RED;
439+
if (motor_state.state == clearpath_motor_msgs::msg::LynxMotorProtection::ERROR)
440+
{
441+
diagnostic_qualifier_ += LYNX_MOTOR_TO_DIAGNOSTIC_QUALIFIER_MAP.at(motor_state.joint_name);
442+
auto light = LYNX_MOTOR_TO_LIGHTS_MAP.at(motor_state.joint_name);
443+
off_1.at(light) = COLOR_BLACK;
444+
on_1.at(light) = COLOR_RED;
445+
off_2.at(light) = COLOR_BLACK;
446+
on_2.at(light) = COLOR_RED;
447+
}
475448
}
476449

477450
lighting_sequence_.at(State::MotorFault) =
@@ -484,11 +457,11 @@ void Lighting::updateState()
484457
on_2,
485458
MS_TO_STEPS(200), 0.5);
486459
}
487-
else if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxSystemProtection::OVERHEATED)
460+
else if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxMotorProtection::OVERHEATED)
488461
{
489462
setState(State::MotorOverheated);
490463
}
491-
else if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxSystemProtection::THROTTLED)
464+
else if (system_protection_msg_.system_state == clearpath_motor_msgs::msg::LynxMotorProtection::THROTTLED)
492465
{
493466
setState(State::MotorThrottled);
494467
}

‎clearpath_hardware_interfaces/src/lynx/hardware.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -75,11 +75,11 @@ void LynxHardware::writeCommandsToHardware()
7575

7676
switch(node_->get_protection().system_state)
7777
{
78-
case clearpath_motor_msgs::msg::LynxSystemProtection::THROTTLED:
78+
case clearpath_motor_msgs::msg::LynxMotorProtection::THROTTLED:
7979
max_velocity = MAXIMUM_VELOCITY_THROTTLED_RADS;
8080
break;
8181

82-
case clearpath_motor_msgs::msg::LynxSystemProtection::OVERHEATED:
82+
case clearpath_motor_msgs::msg::LynxMotorProtection::OVERHEATED:
8383
max_velocity = MAXIMUM_VELOCITY_OVERHEATED_RADS;
8484
break;
8585
}
@@ -141,7 +141,7 @@ hardware_interface::CallbackReturn LynxHardware::getHardwareInfo(const hardware_
141141
RCLCPP_INFO(rclcpp::get_logger(hw_name_), "Name: %s", hw_name_.c_str());
142142

143143
// Check for valid number of joints
144-
if (num_joints_ != DIFF_DRIVE_FOUR_JOINTS)
144+
if (num_joints_ != DIFF_DRIVE_FOUR_JOINTS && num_joints_ != DIFF_DRIVE_TWO_JOINTS)
145145
{
146146
RCLCPP_ERROR(rclcpp::get_logger(hw_name_), "Invalid number of joints %u", num_joints_);
147147
return hardware_interface::CallbackReturn::ERROR;

‎clearpath_motor_drivers/lynx_motor_driver/include/lynx_motor_driver/lynx_motor_driver.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
3636

3737
#include "clearpath_motor_msgs/msg/lynx_debug.hpp"
3838
#include "clearpath_motor_msgs/msg/lynx_feedback.hpp"
39+
#include "clearpath_motor_msgs/msg/lynx_motor_protection.hpp"
3940
#include "clearpath_motor_msgs/msg/lynx_status.hpp"
4041
#include "clearpath_motor_msgs/msg/lynx_system_protection.hpp"
4142

‎clearpath_motor_drivers/lynx_motor_driver/include/lynx_motor_driver/lynx_motor_node.hpp

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
4747
#include "clearpath_motor_msgs/msg/lynx_status.hpp"
4848
#include "clearpath_motor_msgs/msg/lynx_multi_feedback.hpp"
4949
#include "clearpath_motor_msgs/msg/lynx_feedback.hpp"
50+
#include "clearpath_motor_msgs/msg/lynx_motor_protection.hpp"
5051
#include "clearpath_motor_msgs/msg/lynx_system_protection.hpp"
5152

5253

@@ -83,6 +84,7 @@ class LynxMotorNode
8384
using Update = clearpath_motor_msgs::action::LynxUpdate;
8485
using GoalHandleUpdate = rclcpp_action::ServerGoalHandle<Update>;
8586
using LynxStatus = clearpath_motor_msgs::msg::LynxStatus;
87+
using LynxMotorProtection = clearpath_motor_msgs::msg::LynxMotorProtection;
8688
using LynxSystemProtection = clearpath_motor_msgs::msg::LynxSystemProtection;
8789
using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper;
8890
using Empty = std_srvs::srv::Empty;
@@ -176,17 +178,17 @@ class LynxMotorNode
176178
};
177179

178180
const std::map<uint8_t, std::string> LYNX_PROTECTION_LABELS_ = {
179-
{LynxSystemProtection::NORMAL, "Normal"},
180-
{LynxSystemProtection::THROTTLED, "Throttled"},
181-
{LynxSystemProtection::OVERHEATED, "Overheated"},
182-
{LynxSystemProtection::ERROR, "Error"},
181+
{LynxMotorProtection::NORMAL, "Normal"},
182+
{LynxMotorProtection::THROTTLED, "Throttled"},
183+
{LynxMotorProtection::OVERHEATED, "Overheated"},
184+
{LynxMotorProtection::ERROR, "Error"},
183185
};
184186

185-
const std::map<uint8_t, std::string> LYNX_MOTOR_LABELS_ = {
186-
{LynxSystemProtection::A300_MOTOR_FRONT_LEFT, "Front Left"},
187-
{LynxSystemProtection::A300_MOTOR_FRONT_RIGHT, "Front Right"},
188-
{LynxSystemProtection::A300_MOTOR_REAR_LEFT, "Rear Left"},
189-
{LynxSystemProtection::A300_MOTOR_REAR_RIGHT, "Rear Right"},
187+
const std::map<std::string, std::string> LYNX_MOTOR_LABELS_ = {
188+
{"front_left_wheel_joint", "Front Left"},
189+
{"front_right_wheel_joint", "Front Right"},
190+
{"rear_left_wheel_joint", "Rear Left"},
191+
{"rear_right_wheel_joint", "Rear Right"},
190192
};
191193

192194
// System Protection

‎clearpath_motor_drivers/lynx_motor_driver/src/lynx_motor_driver.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ LynxMotorDriver::LynxMotorDriver(const int64_t& can_id,
4747
const int64_t direction,
4848
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> can_interface)
4949
: can_id_(can_id), joint_name_(joint_name), direction_(direction),
50-
protection_state_(clearpath_motor_msgs::msg::LynxSystemProtection::NORMAL), debug_(false),
50+
protection_state_(clearpath_motor_msgs::msg::LynxMotorProtection::NORMAL), debug_(false),
5151
can_interface_(can_interface)
5252
{
5353
for (auto & m : debug_mutexes_)

‎clearpath_motor_drivers/lynx_motor_driver/src/lynx_motor_node.cpp

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,12 @@ LynxMotorNode::LynxMotorNode(const std::string node_name) :
158158
debug_msg_.drivers.resize(drivers_.size());
159159
system_protection_msg_.header.frame_id = "base_link";
160160
system_protection_msg_.motor_states.resize(drivers_.size());
161-
system_protection_msg_.system_state = clearpath_motor_msgs::msg::LynxSystemProtection::NORMAL;
161+
system_protection_msg_.system_state = clearpath_motor_msgs::msg::LynxMotorProtection::NORMAL;
162+
for (auto i = 0; i < system_protection_msg_.motor_states.size(); i++)
163+
{
164+
system_protection_msg_.motor_states.at(i).can_id = drivers_.at(i).getCanID();
165+
system_protection_msg_.motor_states.at(i).joint_name = drivers_.at(i).getJointName();
166+
}
162167

163168
// Start timers
164169
feedback_timer_ = this->create_wall_timer(
@@ -422,10 +427,10 @@ void LynxMotorNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i)
422427

423428
try {
424429
// Set message to warn if motor is in overheated or throttled states
425-
if ((system_protection_msg_.motor_states[i] == LynxSystemProtection::THROTTLED) ||
426-
(system_protection_msg_.motor_states[i] == LynxSystemProtection::OVERHEATED)) {
430+
if ((system_protection_msg_.motor_states.at(i).state == LynxMotorProtection::THROTTLED) ||
431+
(system_protection_msg_.motor_states.at(i).state == LynxMotorProtection::OVERHEATED)) {
427432
stat.mergeSummary(DiagnosticStatusWrapper::WARN,
428-
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.motor_states[i]));
433+
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.motor_states.at(i).state));
429434
}
430435
} catch(const std::out_of_range & e) {
431436
RCLCPP_ERROR(this->get_logger(),
@@ -500,10 +505,10 @@ void LynxMotorNode::protectionDiagnostic(DiagnosticStatusWrapper & stat)
500505

501506
try {
502507
// Interpret system protection message
503-
if (system_protection_msg_.system_state == LynxSystemProtection::NORMAL) {
508+
if (system_protection_msg_.system_state == LynxMotorProtection::NORMAL) {
504509
stat.summary(DiagnosticStatusWrapper::OK,
505510
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.system_state));
506-
} else if (system_protection_msg_.system_state == LynxSystemProtection::ERROR) {
511+
} else if (system_protection_msg_.system_state == LynxMotorProtection::ERROR) {
507512
stat.summary(DiagnosticStatusWrapper::ERROR,
508513
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.system_state));
509514
} else {
@@ -518,8 +523,8 @@ void LynxMotorNode::protectionDiagnostic(DiagnosticStatusWrapper & stat)
518523

519524
try {
520525
for (unsigned i = 0; i < system_protection_msg_.motor_states.size(); i++) {
521-
stat.add(LYNX_MOTOR_LABELS_.at(i) + " Motor State",
522-
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.motor_states[i]));
526+
stat.add(LYNX_MOTOR_LABELS_.at(system_protection_msg_.motor_states.at(i).joint_name) + " Motor State",
527+
LYNX_PROTECTION_LABELS_.at(system_protection_msg_.motor_states.at(i).state));
523528
}
524529
} catch(const std::out_of_range & e) {
525530
RCLCPP_ERROR(this->get_logger(),

0 commit comments

Comments
 (0)