15 #ifndef NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_
16 #define NODE_CANOPEN_PROXY_DRIVER_IMPL_HPP_
23 template <
class NODETYPE>
29 template <
class NODETYPE>
32 RCLCPP_ERROR(this->node_->get_logger(),
"Not init implemented.");
38 nmt_state_publisher = this->node_->create_publisher<std_msgs::msg::String>(
39 std::string(this->node_->get_name()).append(
"/nmt_state").c_str(), 10);
40 tpdo_subscriber = this->node_->create_subscription<canopen_interfaces::msg::COData>(
41 std::string(this->node_->get_name()).append(
"/tpdo").c_str(), 10,
44 rpdo_publisher = this->node_->create_publisher<canopen_interfaces::msg::COData>(
45 std::string(this->node_->get_name()).append(
"/rpdo").c_str(), 10);
47 nmt_state_reset_service = this->node_->create_service<std_srvs::srv::Trigger>(
48 std::string(this->node_->get_name()).append(
"/nmt_reset_node").c_str(),
51 std::placeholders::_2));
53 nmt_state_start_service = this->node_->create_service<std_srvs::srv::Trigger>(
54 std::string(this->node_->get_name()).append(
"/nmt_start_node").c_str(),
57 std::placeholders::_2));
59 sdo_read_service = this->node_->create_service<canopen_interfaces::srv::CORead>(
60 std::string(this->node_->get_name()).append(
"/sdo_read").c_str(),
63 std::placeholders::_2));
65 sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWrite>(
66 std::string(this->node_->get_name()).append(
"/sdo_write").c_str(),
69 std::placeholders::_2));
75 nmt_state_publisher = this->node_->create_publisher<std_msgs::msg::String>(
76 std::string(this->node_->get_name()).append(
"/nmt_state").c_str(), 10);
77 tpdo_subscriber = this->node_->create_subscription<canopen_interfaces::msg::COData>(
78 std::string(this->node_->get_name()).append(
"/tpdo").c_str(), 10,
81 std::placeholders::_1));
83 rpdo_publisher = this->node_->create_publisher<canopen_interfaces::msg::COData>(
84 std::string(this->node_->get_name()).append(
"/rpdo").c_str(), 10);
86 nmt_state_reset_service = this->node_->create_service<std_srvs::srv::Trigger>(
87 std::string(this->node_->get_name()).append(
"/nmt_reset_node").c_str(),
90 std::placeholders::_1, std::placeholders::_2));
92 nmt_state_start_service = this->node_->create_service<std_srvs::srv::Trigger>(
93 std::string(this->node_->get_name()).append(
"/nmt_start_node").c_str(),
96 std::placeholders::_1, std::placeholders::_2));
98 sdo_read_service = this->node_->create_service<canopen_interfaces::srv::CORead>(
99 std::string(this->node_->get_name()).append(
"/sdo_read").c_str(),
102 std::placeholders::_1, std::placeholders::_2));
104 sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWrite>(
105 std::string(this->node_->get_name()).append(
"/sdo_write").c_str(),
108 std::placeholders::_1, std::placeholders::_2));
111 template <
class NODETYPE>
114 if (this->activated_.load())
116 auto message = std_msgs::msg::String();
120 case canopen::NmtState::BOOTUP:
121 message.data =
"BOOTUP";
122 this->diagnostic_collector_->updateAll(
123 diagnostic_msgs::msg::DiagnosticStatus::OK,
"NMT bootup",
"NMT",
"BOOTUP");
125 case canopen::NmtState::PREOP:
126 message.data =
"PREOP";
127 this->diagnostic_collector_->updateAll(
128 diagnostic_msgs::msg::DiagnosticStatus::OK,
"NMT preop",
"NMT",
"PREOP");
130 case canopen::NmtState::RESET_COMM:
131 message.data =
"RESET_COMM";
132 this->diagnostic_collector_->updateAll(
133 diagnostic_msgs::msg::DiagnosticStatus::WARN,
"NMT reset comm",
"NMT",
"RESET_COMM");
135 case canopen::NmtState::RESET_NODE:
136 message.data =
"RESET_NODE";
137 this->diagnostic_collector_->updateAll(
138 diagnostic_msgs::msg::DiagnosticStatus::WARN,
"NMT reset node",
"NMT",
"RESET_NODE");
140 case canopen::NmtState::START:
141 message.data =
"START";
142 this->diagnostic_collector_->updateAll(
143 diagnostic_msgs::msg::DiagnosticStatus::OK,
"NMT start",
"NMT",
"START");
145 case canopen::NmtState::STOP:
146 message.data =
"STOP";
147 this->diagnostic_collector_->updateAll(
148 diagnostic_msgs::msg::DiagnosticStatus::OK,
"NMT stop",
"NMT",
"STOP");
150 case canopen::NmtState::TOGGLE:
151 message.data =
"TOGGLE";
152 this->diagnostic_collector_->updateAll(
153 diagnostic_msgs::msg::DiagnosticStatus::OK,
"NMT toggle",
"NMT",
"TOGGLE");
156 RCLCPP_ERROR(this->node_->get_logger(),
"Unknown NMT State.");
157 message.data =
"ERROR";
158 this->diagnostic_collector_->updateAll(
159 diagnostic_msgs::msg::DiagnosticStatus::ERROR,
"NMT unknown state",
"NMT",
"ERROR");
163 this->node_->get_logger(),
"Slave 0x%X: Switched NMT state to %s",
164 this->lely_driver_->get_id(), message.data.c_str());
166 nmt_state_publisher->publish(message);
170 template <
class NODETYPE>
174 if (!tpdo_transmit(data))
176 RCLCPP_ERROR(this->node_->get_logger(),
"Could transmit PDO because driver not activated.");
180 template <
class NODETYPE>
183 if (this->activated_.load())
186 this->node_->get_logger(),
"Node ID 0x%X: Transmit PDO index %x, subindex %hhu, data %d",
189 this->lely_driver_->tpdo_transmit(data);
195 template <
class NODETYPE>
198 if (this->activated_.load())
203 auto message = canopen_interfaces::msg::COData();
206 message.data = d.
data_;
207 rpdo_publisher->publish(message);
211 template <
class NODETYPE>
213 const std_srvs::srv::Trigger::Request::SharedPtr request,
214 std_srvs::srv::Trigger::Response::SharedPtr response)
216 response->success = reset_node_nmt_command();
219 template <
class NODETYPE>
222 if (this->activated_.load())
224 this->lely_driver_->nmt_command(canopen::NmtCommand::RESET_NODE);
228 this->node_->get_logger(),
"Could not reset device via NMT because driver not activated.");
232 template <
class NODETYPE>
234 const std_srvs::srv::Trigger::Request::SharedPtr request,
235 std_srvs::srv::Trigger::Response::SharedPtr response)
237 response->success = start_node_nmt_command();
240 template <
class NODETYPE>
243 if (this->activated_.load())
245 this->lely_driver_->nmt_command(canopen::NmtCommand::START);
249 this->node_->get_logger(),
"Could not start device via NMT because driver not activated.");
253 template <
class NODETYPE>
255 const canopen_interfaces::srv::CORead::Request::SharedPtr request,
256 canopen_interfaces::srv::CORead::Response::SharedPtr response)
259 response->success = sdo_read(data);
260 response->data = data.
data_;
263 template <
class NODETYPE>
266 if (this->activated_.load())
269 this->node_->get_logger(),
"Slave 0x%X: SDO Read Call index=0x%X subindex=%hhu",
273 std::scoped_lock<std::mutex> lk(sdo_mtex);
275 auto f = this->lely_driver_->async_sdo_read(data);
281 data.
data_ = f.get().data_;
283 catch (std::exception & e)
285 RCLCPP_ERROR(this->node_->get_logger(), e.what());
290 RCLCPP_ERROR(this->node_->get_logger(),
"Could not read from SDO because driver not activated.");
294 template <
class NODETYPE>
296 const canopen_interfaces::srv::COWrite::Request::SharedPtr request,
297 canopen_interfaces::srv::COWrite::Response::SharedPtr response)
300 response->success = sdo_write(data);
303 template <
class NODETYPE>
306 if (this->activated_.load())
309 this->node_->get_logger(),
"Slave 0x%X: SDO Write Call index=0x%X subindex=%hhu data=%u",
313 std::scoped_lock<std::mutex> lk(sdo_mtex);
316 auto f = this->lely_driver_->async_sdo_write(data);
325 catch (std::exception & e)
327 RCLCPP_ERROR(this->node_->get_logger(), e.what());
332 RCLCPP_ERROR(this->node_->get_logger(),
"Could not write to SDO because driver not activated.");
336 template <
class NODETYPE>
338 diagnostic_updater::DiagnosticStatusWrapper & stat)
340 stat.summary(this->diagnostic_collector_->getLevel(), this->diagnostic_collector_->getMessage());
341 stat.add(
"device_state", this->diagnostic_collector_->getValue(
"DEVICE"));
342 stat.add(
"nmt_state", this->diagnostic_collector_->getValue(
"NMT"));
343 stat.add(
"emcy_state", this->diagnostic_collector_->getValue(
"EMCY"));
Definition: node_canopen_base_driver.hpp:34
void init()
Initialise the driver.
Definition: node_canopen_driver.hpp:127
Definition: node_canopen_proxy_driver.hpp:26
void on_sdo_read(const canopen_interfaces::srv::CORead::Request::SharedPtr request, canopen_interfaces::srv::CORead::Response::SharedPtr response)
Definition: node_canopen_proxy_driver_impl.hpp:254
virtual bool sdo_write(COData &data)
Definition: node_canopen_proxy_driver_impl.hpp:304
void on_nmt_state_reset(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Definition: node_canopen_proxy_driver_impl.hpp:212
virtual void diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat) override
Definition: node_canopen_proxy_driver_impl.hpp:337
void on_sdo_write(const canopen_interfaces::srv::COWrite::Request::SharedPtr request, canopen_interfaces::srv::COWrite::Response::SharedPtr response)
Definition: node_canopen_proxy_driver_impl.hpp:295
void on_nmt_state_start(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)
Definition: node_canopen_proxy_driver_impl.hpp:233
virtual void on_rpdo(COData data) override
Definition: node_canopen_proxy_driver_impl.hpp:196
NodeCanopenProxyDriver(NODETYPE *node)
Definition: node_canopen_proxy_driver_impl.hpp:24
virtual bool reset_node_nmt_command()
Definition: node_canopen_proxy_driver_impl.hpp:220
virtual void on_tpdo(const canopen_interfaces::msg::COData::SharedPtr msg)
Definition: node_canopen_proxy_driver_impl.hpp:171
virtual bool tpdo_transmit(COData &data)
Definition: node_canopen_proxy_driver_impl.hpp:181
virtual bool start_node_nmt_command()
Definition: node_canopen_proxy_driver_impl.hpp:241
virtual void on_nmt(canopen::NmtState nmt_state) override
Definition: node_canopen_proxy_driver_impl.hpp:112
virtual bool sdo_read(COData &data)
Definition: node_canopen_proxy_driver_impl.hpp:264
Definition: node_canopen_driver.hpp:46
Definition: configuration_manager.hpp:28
Definition: exchange.hpp:26
uint32_t data_
Definition: exchange.hpp:30
uint8_t subindex_
Definition: exchange.hpp:29
uint16_t index_
Definition: exchange.hpp:28