Skip to content

Commit

Permalink
Add service to clear errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Libor Wagner (wagnelib) committed Mar 15, 2024
1 parent 0094044 commit 7cfcb30
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 16 deletions.
3 changes: 2 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,13 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ControllerStatus.msg"
"msg/ODriveStatus.msg"
"srv/AxisState.srv"
"srv/ClearErrors.srv"
)
ament_export_dependencies(rosidl_default_runtime)

include_directories(include)

add_executable(odrive_can_node
add_executable(odrive_can_node
src/epoll_event_loop.cpp
src/socket_can.cpp
src/odrive_can_node.cpp
Expand Down
17 changes: 13 additions & 4 deletions include/odrive_can_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include "odrive_can/msg/controller_status.hpp"
#include "odrive_can/msg/control_message.hpp"
#include "odrive_can/srv/axis_state.hpp"
#include "odrive_can/srv/clear_errors.hpp"
#include "socket_can.hpp"

#include <mutex>
Expand All @@ -23,28 +24,31 @@ using ControllerStatus = odrive_can::msg::ControllerStatus;
using ControlMessage = odrive_can::msg::ControlMessage;

using AxisState = odrive_can::srv::AxisState;
using ClearErrors = odrive_can::srv::ClearErrors;

class ODriveCanNode : public rclcpp::Node {
public:
ODriveCanNode(const std::string& node_name);
bool init(EpollEventLoop* event_loop);
bool init(EpollEventLoop* event_loop);
void deinit();
private:
void recv_callback(const can_frame& frame);
void subscriber_callback(const ControlMessage::SharedPtr msg);
void service_callback(const std::shared_ptr<AxisState::Request> request, std::shared_ptr<AxisState::Response> response);
void service_clear_errors_callback(const std::shared_ptr<ClearErrors::Request> request, std::shared_ptr<ClearErrors::Response> response);
void request_state_callback();
void request_clear_errors_callback();
void ctrl_msg_callback();
inline bool verify_length(const std::string&name, uint8_t expected, uint8_t length);

uint16_t node_id_;
SocketCanIntf can_intf_ = SocketCanIntf();

short int ctrl_pub_flag_ = 0;
std::mutex ctrl_stat_mutex_;
ControllerStatus ctrl_stat_ = ControllerStatus();
rclcpp::Publisher<ControllerStatus>::SharedPtr ctrl_publisher_;

short int odrv_pub_flag_ = 0;
std::mutex odrv_stat_mutex_;
ODriveStatus odrv_stat_ = ODriveStatus();
Expand All @@ -61,6 +65,11 @@ class ODriveCanNode : public rclcpp::Node {
std::condition_variable fresh_heartbeat_;
rclcpp::Service<AxisState>::SharedPtr service_;

EpollEvent srv_clear_errors_evt_;
uint8_t identify_;
std::mutex clear_errors_mutex_;
rclcpp::Service<ClearErrors>::SharedPtr service_clear_errors_;

};

#endif // ODRIVE_CAN_NODE_HPP
58 changes: 47 additions & 11 deletions src/odrive_can_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ enum CmdId : uint32_t {
kGetTemp, // SystemStatus - publisher
kGetBusVoltageCurrent = 0x017, // SystemStatus - publisher
kGetTorques = 0x01c, // ControllerStatus - publisher
kClearErrors = 0x018, // ClearErrors - service
};

enum ControlMode : uint64_t {
Expand All @@ -27,13 +28,13 @@ enum ControlMode : uint64_t {
};

ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_name) {

rclcpp::Node::declare_parameter<std::string>("interface", "can0");
rclcpp::Node::declare_parameter<uint16_t>("node_id", 0);

rclcpp::QoS ctrl_stat_qos(rclcpp::KeepAll{});
ctrl_publisher_ = rclcpp::Node::create_publisher<ControllerStatus>("controller_status", ctrl_stat_qos);

rclcpp::QoS odrv_stat_qos(rclcpp::KeepAll{});
odrv_publisher_ = rclcpp::Node::create_publisher<ODriveStatus>("odrive_status", odrv_stat_qos);

Expand All @@ -42,6 +43,9 @@ ODriveCanNode::ODriveCanNode(const std::string& node_name) : rclcpp::Node(node_n

rclcpp::QoS srv_qos(rclcpp::KeepAll{});
service_ = rclcpp::Node::create_service<AxisState>("request_axis_state", std::bind(&ODriveCanNode::service_callback, this, _1, _2), srv_qos.get_rmw_qos_profile());

rclcpp::QoS srv_clear_errors_qos(rclcpp::KeepAll{});
service_clear_errors_ = rclcpp::Node::create_service<ClearErrors>("clear_errors", std::bind(&ODriveCanNode::service_clear_errors_callback, this, _1, _2), srv_clear_errors_qos.get_rmw_qos_profile());
}

void ODriveCanNode::deinit() {
Expand All @@ -67,6 +71,10 @@ bool ODriveCanNode::init(EpollEventLoop* event_loop) {
RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize service event");
return false;
}
if (!srv_clear_errors_evt_.init(event_loop, std::bind(&ODriveCanNode::request_clear_errors_callback, this))) {
RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize clear errors service event");
return false;
}
RCLCPP_INFO(rclcpp::Node::get_logger(), "node_id: %d", node_id_);
RCLCPP_INFO(rclcpp::Node::get_logger(), "interface: %s", interface.c_str());
return true;
Expand Down Expand Up @@ -133,20 +141,20 @@ void ODriveCanNode::recv_callback(const can_frame& frame) {
std::lock_guard<std::mutex> guard(ctrl_stat_mutex_);
ctrl_stat_.torque_target = read_le<float>(frame.data + 0);
ctrl_stat_.torque_estimate = read_le<float>(frame.data + 4);
ctrl_pub_flag_ |= 0b1000;
ctrl_pub_flag_ |= 0b1000;
break;
}
default: {
RCLCPP_WARN(rclcpp::Node::get_logger(), "Received unused message: ID = 0x%x", (frame.can_id & 0x1F));
break;
}
}

if (ctrl_pub_flag_ == 0b1111) {
ctrl_publisher_->publish(ctrl_stat_);
ctrl_pub_flag_ = 0;
}

if (odrv_pub_flag_ == 0b111) {
odrv_publisher_->publish(odrv_stat_);
odrv_pub_flag_ = 0;
Expand All @@ -171,15 +179,32 @@ void ODriveCanNode::service_callback(const std::shared_ptr<AxisState::Request> r
auto call_time = std::chrono::steady_clock::now();
fresh_heartbeat_.wait(guard, [this, &call_time]() {
bool complete = (this->ctrl_stat_.procedure_result != 1) && // make sure procedure_result is not busy
(std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second
return complete;
(std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second
return complete;
}); // wait for procedure_result

response->axis_state = ctrl_stat_.axis_state;
response->active_errors = ctrl_stat_.active_errors;
response->procedure_result = ctrl_stat_.procedure_result;
}

void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr<ClearErrors::Request> request, std::shared_ptr<ClearErrors::Response> response) {
{
std::unique_lock<std::mutex> guard(clear_errors_mutex_);
identify_ = request->identify;
RCLCPP_INFO(rclcpp::Node::get_logger(), "clearing errors identify: %d", identify_);
}
srv_clear_errors_evt_.set();

std::unique_lock<std::mutex> guard(ctrl_stat_mutex_); // define lock for controller status
auto call_time = std::chrono::steady_clock::now();
fresh_heartbeat_.wait(guard, [this, &call_time]() {
bool complete = (this->ctrl_stat_.procedure_result != 1) && // make sure procedure_result is not busy
(std::chrono::steady_clock::now() - call_time >= std::chrono::seconds(1)); // wait for minimum one second
return complete;
}); // wait for procedure_result
}

void ODriveCanNode::request_state_callback() {
struct can_frame frame;
frame.can_id = node_id_ << 5 | CmdId::kSetAxisState;
Expand All @@ -191,6 +216,17 @@ void ODriveCanNode::request_state_callback() {
can_intf_.send_can_frame(frame);
}

void ODriveCanNode::request_clear_errors_callback() {
struct can_frame frame;
frame.can_id = node_id_ << 5 | CmdId::kClearErrors;
{
std::unique_lock<std::mutex> guard(axis_state_mutex_);
write_le<uint8_t>(identify_, frame.data);
}
frame.can_dlc = 4;
can_intf_.send_can_frame(frame);
}

void ODriveCanNode::ctrl_msg_callback() {

uint32_t control_mode;
Expand All @@ -204,7 +240,7 @@ void ODriveCanNode::ctrl_msg_callback() {
}
frame.can_dlc = 8;
can_intf_.send_can_frame(frame);

frame = can_frame{};
switch (control_mode) {
case ControlMode::kVoltageControl: {
Expand Down Expand Up @@ -237,8 +273,8 @@ void ODriveCanNode::ctrl_msg_callback() {
write_le<int8_t>(((int8_t)((ctrl_msg_.input_torque) * 1000)), frame.data + 6);
frame.can_dlc = 8;
break;
}
default:
}
default:
RCLCPP_ERROR(rclcpp::Node::get_logger(), "unsupported control_mode: %d", control_mode);
return;
}
Expand Down

0 comments on commit 7cfcb30

Please sign in to comment.