Skip to content

Commit

Permalink
Setup CANopen nodes in listed order if applicable
Browse files Browse the repository at this point in the history
  • Loading branch information
@mathias-luedtke
mathias-luedtke committed Jul 9, 2021
1 parent b80d590 commit 4e2dce2
Show file tree
Hide file tree
Showing 2 changed files with 88 additions and 84 deletions.
2 changes: 2 additions & 0 deletions canopen_chain_node/include/canopen_chain_node/ros_chain.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,8 +137,10 @@ template<typename T> class ClassAllocator : public GuardedClassLoader<typename T
}
};
class RosChain : GuardedClassLoaderList, public canopen::LayerStack {
private:
GuardedClassLoader<can::DriverInterface> driver_loader_;
ClassAllocator<canopen::Master> master_allocator_;
bool setup_node(const XmlRpc::XmlRpcValue &params, const std::string& name, const MergedXmlRpcStruct &defaults);
protected:
can::DriverInterfaceSharedPtr interface_;
MasterSharedPtr master_;
Expand Down
170 changes: 86 additions & 84 deletions canopen_chain_node/src/ros_chain.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -453,124 +453,126 @@ bool RosChain::setup_nodes(){
nh_priv_.getParam("defaults", defaults);

if(nodes.getType() == XmlRpc::XmlRpcValue::TypeArray){
XmlRpc::XmlRpcValue new_stuct;
for(size_t i = 0; i < nodes.size(); ++i){
if(nodes[i].hasMember("name")){
std::string &name = nodes[i]["name"];
new_stuct[name] = nodes[i];
if(!setup_node(nodes[i], nodes[i]["name"], defaults)) return false;
}else{
ROS_ERROR_STREAM("Node at list index " << i << " has no name");
return false;
}
}
nodes = new_stuct;
}else{
for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){
if(!setup_node(it->second, it->first, defaults)) return false;
}
}
return true;
}

for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){
int node_id;
try{
node_id = it->second["id"];
}
catch(...){
ROS_ERROR_STREAM("Node '" << it->first << "' has no id");
return false;
}
MergedXmlRpcStruct merged(it->second, defaults);
bool RosChain::setup_node(const XmlRpc::XmlRpcValue& params, const std::string &name, const MergedXmlRpcStruct &defaults){
int node_id;
try{
node_id = params["id"];
}
catch(...){
ROS_ERROR_STREAM("Node '" << name << "' has no id");
return false;
}
MergedXmlRpcStruct merged(params, defaults);

if(!it->second.hasMember("name")){
merged["name"]=it->first;
}
if(!merged.hasMember("name")){
merged["name"]=name;
}

ObjectDict::Overlay overlay;
if(merged.hasMember("dcf_overlay")){
XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"];
if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){
ROS_ERROR_STREAM("dcf_overlay is no struct");
ObjectDict::Overlay overlay;
if(merged.hasMember("dcf_overlay")){
XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"];
if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){
ROS_ERROR_STREAM("dcf_overlay is no struct");
return false;
}
for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){
if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){
ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string");
return false;
}
for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){
if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){
ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string");
return false;
}
overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second));
}

overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second));
}
}

std::string eds;
std::string eds;

try{
eds = (std::string) merged["eds_file"];
}
catch(...){
ROS_ERROR_STREAM("EDS path '" << eds << "' invalid");
return false;
}
try{
eds = (std::string) merged["eds_file"];
}
catch(...){
ROS_ERROR_STREAM("EDS path '" << eds << "' invalid");
return false;
}

try{
if(merged.hasMember("eds_pkg")){
std::string pkg = merged["eds_pkg"];
std::string p = ros::package::getPath(pkg);
if(p.empty()){
ROS_WARN_STREAM("Package '" << pkg << "' was not found");
}else{
eds = (boost::filesystem::path(p)/eds).make_preferred().native();;
}
try{
if(merged.hasMember("eds_pkg")){
std::string pkg = merged["eds_pkg"];
std::string p = ros::package::getPath(pkg);
if(p.empty()){
ROS_WARN_STREAM("Package '" << pkg << "' was not found");
}else{
eds = (boost::filesystem::path(p)/eds).make_preferred().native();;
}
}
catch(...){
}
}
catch(...){
}

ObjectDictSharedPtr dict = ObjectDict::fromFile(eds, overlay);
if(!dict){
ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed");
return false;
}
canopen::NodeSharedPtr node = std::make_shared<canopen::Node>(interface_, dict, node_id, sync_);
ObjectDictSharedPtr dict = ObjectDict::fromFile(eds, overlay);
if(!dict){
ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed");
return false;
}
canopen::NodeSharedPtr node = std::make_shared<canopen::Node>(interface_, dict, node_id, sync_);

LoggerSharedPtr logger = std::make_shared<Logger>(node);
LoggerSharedPtr logger = std::make_shared<Logger>(node);

if(!nodeAdded(merged, node, logger)) return false;
if(!nodeAdded(merged, node, logger)) return false;

if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false;
if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false;
if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false;
if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false;
if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false;
if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false;

loggers_.push_back(logger);
diag_updater_.add(it->first, std::bind(&Logger::log, logger, std::placeholders::_1));
loggers_.push_back(logger);
diag_updater_.add(name, std::bind(&Logger::log, logger, std::placeholders::_1));

std::string node_name = std::string(merged["name"]);
std::string node_name = std::string(merged["name"]);

if(merged.hasMember("publish")){
try{
XmlRpc::XmlRpcValue objs = merged["publish"];
for(int i = 0; i < objs.size(); ++i){
std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
if(merged.hasMember("publish")){
try{
XmlRpc::XmlRpcValue objs = merged["publish"];
for(int i = 0; i < objs.size(); ++i){
std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);

PublishFuncType pub = createPublishFunc(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second);
if(!pub){
ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'");
return false;
}
publishers_.push_back(pub);
PublishFuncType pub = createPublishFunc(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second);
if(!pub){
ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'");
return false;
}
publishers_.push_back(pub);
}
catch(...){
ROS_ERROR("Could not parse publish parameter");
return false;
}
}
nodes_->add(node);
nodes_lookup_.insert(std::make_pair(node_name, node));
catch(...){
ROS_ERROR("Could not parse publish parameter");
return false;
}
}
nodes_->add(node);
nodes_lookup_.insert(std::make_pair(node_name, node));

std::shared_ptr<canopen::EMCYHandler> emcy = std::make_shared<canopen::EMCYHandler>(interface_, node->getStorage());
emcy_handlers_->add(emcy);
logger->add(emcy);
std::shared_ptr<canopen::EMCYHandler> emcy = std::make_shared<canopen::EMCYHandler>(interface_, node->getStorage());
emcy_handlers_->add(emcy);
logger->add(emcy);

}
return true;
}

bool RosChain::nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger){
return true;
}
Expand Down

0 comments on commit 4e2dce2

Please sign in to comment.