Program Listing for File node_canopen_basic_master_impl.hpp
↰ Return to documentation for file (include/canopen_master_driver/node_interfaces/node_canopen_basic_master_impl.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 NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_
#define NODE_CANOPEN_BASIC_MASTER_IMPL_HPP_
#include "canopen_master_driver/lely_master_bridge.hpp"
#include "canopen_master_driver/node_interfaces/node_canopen_basic_master.hpp"
using namespace ros2_canopen::node_interfaces;
template <class NODETYPE>
void NodeCanopenBasicMaster<NODETYPE>::activate(bool called_from_base)
{
master_bridge_ = std::make_shared<LelyMasterBridge>(
*(this->exec_), *(this->timer_), *(this->chan_), this->master_dcf_, this->master_bin_,
this->node_id_);
master_bridge_->SetTimeout(std::chrono::milliseconds(this->timeout_));
this->master_ = std::static_pointer_cast<lely::canopen::AsyncMaster>(master_bridge_);
}
template <class NODETYPE>
void NodeCanopenBasicMaster<NODETYPE>::deactivate(bool called_from_base)
{
master_bridge_.reset();
this->master_.reset();
}
template <class NODETYPE>
void NodeCanopenBasicMaster<NODETYPE>::on_sdo_read(
const std::shared_ptr<canopen_interfaces::srv::COReadID::Request> request,
std::shared_ptr<canopen_interfaces::srv::COReadID::Response> response)
{
if (this->activated_.load())
{
ros2_canopen::COData data = {request->index, request->subindex, 0U};
std::future<ros2_canopen::COData> f =
this->master_bridge_->async_read_sdo(request->nodeid, data, request->canopen_datatype);
f.wait();
try
{
response->data = f.get().data_;
response->success = true;
}
catch (std::exception & e)
{
RCLCPP_ERROR(this->node_->get_logger(), e.what());
response->success = false;
}
}
else
{
RCLCPP_ERROR(
this->node_->get_logger(),
"LifecycleMasterNode is not in active state. SDO read service is not available.");
response->success = false;
}
}
template <class NODETYPE>
void NodeCanopenBasicMaster<NODETYPE>::on_sdo_write(
const std::shared_ptr<canopen_interfaces::srv::COWriteID::Request> request,
std::shared_ptr<canopen_interfaces::srv::COWriteID::Response> response)
{
if (this->activated_.load())
{
ros2_canopen::COData data = {request->index, request->subindex, request->data};
std::future<bool> f =
this->master_bridge_->async_write_sdo(request->nodeid, data, request->canopen_datatype);
f.wait();
try
{
response->success = f.get();
}
catch (std::exception & e)
{
RCLCPP_ERROR(this->node_->get_logger(), e.what());
response->success = false;
}
}
else
{
RCLCPP_ERROR(
this->node_->get_logger(),
"LifecycleMasterNode is not in active state. SDO write service is not available.");
response->success = false;
}
}
template <>
void NodeCanopenBasicMaster<rclcpp::Node>::init(bool called_from_base)
{
// declare services
sdo_read_service = this->node_->create_service<canopen_interfaces::srv::COReadID>(
std::string(this->node_->get_name()).append("/sdo_read").c_str(),
std::bind(
&ros2_canopen::node_interfaces::NodeCanopenBasicMaster<rclcpp::Node>::on_sdo_read, this,
std::placeholders::_1, std::placeholders::_2));
sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWriteID>(
std::string(this->node_->get_name()).append("/sdo_write").c_str(),
std::bind(
&ros2_canopen::node_interfaces::NodeCanopenBasicMaster<rclcpp::Node>::on_sdo_write, this,
std::placeholders::_1, std::placeholders::_2));
}
template <>
void NodeCanopenBasicMaster<rclcpp_lifecycle::LifecycleNode>::init(bool called_from_base)
{
// declare services
sdo_read_service = this->node_->create_service<canopen_interfaces::srv::COReadID>(
std::string(this->node_->get_name()).append("/sdo_read").c_str(),
std::bind(
&ros2_canopen::node_interfaces::NodeCanopenBasicMaster<
rclcpp_lifecycle::LifecycleNode>::on_sdo_read,
this, std::placeholders::_1, std::placeholders::_2));
sdo_write_service = this->node_->create_service<canopen_interfaces::srv::COWriteID>(
std::string(this->node_->get_name()).append("/sdo_write").c_str(),
std::bind(
&ros2_canopen::node_interfaces::NodeCanopenBasicMaster<
rclcpp_lifecycle::LifecycleNode>::on_sdo_write,
this, std::placeholders::_1, std::placeholders::_2));
}
#endif