Program Listing for File lely_master_bridge.hpp
↰ Return to documentation for file (include/canopen_master_driver/lely_master_bridge.hpp
)
// Copyright 2022 Harshavadan Deshpande
// Christoph Hellmann Santos
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef LELY_MASTER_BRIDGE_HPP
#define LELY_MASTER_BRIDGE_HPP
#include <lely/ev/loop.hpp>
#include <lely/io2/posix/poll.hpp>
#include "lely/coapp/master.hpp"
#include <chrono>
#include <condition_variable>
#include <future>
#include <memory>
#include <mutex>
#include "canopen_core/exchange.hpp"
using namespace std::literals::chrono_literals;
namespace ros2_canopen
{
class LelyMasterBridge : public lely::canopen::AsyncMaster
{
std::shared_ptr<std::promise<COData>>
sdo_read_data_promise;
std::shared_ptr<std::promise<bool>>
sdo_write_data_promise;
std::promise<bool> nmt_promise;
std::mutex sdo_mutex;
bool running;
std::condition_variable sdo_cond;
uint8_t node_id;
public:
LelyMasterBridge(
ev_exec_t * exec, lely::io::TimerBase & timer, lely::io::CanChannelBase & chan,
const std::string & dcf_txt, const std::string & dcf_bin = "", uint8_t id = (uint8_t)255U)
: lely::canopen::AsyncMaster(exec, timer, chan, dcf_txt, dcf_bin, id), node_id(id)
{
}
std::future<bool> async_write_sdo(uint8_t id, COData data, uint8_t datatype);
std::future<COData> async_read_sdo(uint8_t id, COData data, uint8_t datatype);
std::future<bool> async_write_nmt(uint8_t id, uint8_t command);
template <typename T>
void submit_write_sdo(uint8_t id, uint16_t idx, uint8_t subidx, T value)
{
this->SubmitWrite(
this->GetExecutor(), id, idx, subidx, value,
[this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec) mutable
{
if (ec)
{
this->sdo_write_data_promise->set_exception(
lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncDownload"));
}
else
{
this->sdo_write_data_promise->set_value(true);
}
std::unique_lock<std::mutex> lck(this->sdo_mutex);
this->running = false;
this->sdo_cond.notify_one();
},
20ms);
}
template <typename T>
void submit_read_sdo(uint8_t id, uint16_t idx, uint8_t subidx)
{
this->SubmitRead<T>(
this->GetExecutor(), id, idx, subidx,
[this](uint8_t id, uint16_t idx, uint8_t subidx, ::std::error_code ec, T value) mutable
{
if (ec)
{
this->sdo_read_data_promise->set_exception(
lely::canopen::make_sdo_exception_ptr(id, idx, subidx, ec, "AsyncUpload"));
}
else
{
COData codata = {idx, subidx, 0U};
std::memcpy(&codata.data_, &value, sizeof(T));
this->sdo_read_data_promise->set_value(codata);
}
std::unique_lock<std::mutex> lck(this->sdo_mutex);
this->running = false;
this->sdo_cond.notify_one();
},
20ms);
}
};
} // namespace ros2_canopen
#endif // LELY_MASTER_BRIDGE_HPP