SchunkCanopenHardwareInterface.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK Canopen Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00022 //----------------------------------------------------------------------
00023 
00024 #include <urdf_parser/urdf_parser.h>
00025 #include <urdf_model/model.h>
00026 #include <urdf/model.h>
00027 
00028 #include "schunk_canopen_driver/SchunkCanopenHardwareInterface.h"
00029 
00030 #include <icl_hardware_canopen/SchunkPowerBallNode.h>
00031 
00032 using namespace hardware_interface;
00033 using joint_limits_interface::JointLimits;
00034 using joint_limits_interface::SoftJointLimits;
00035 using joint_limits_interface::PositionJointSoftLimitsHandle;
00036 using joint_limits_interface::PositionJointSoftLimitsInterface;
00037 
00038 SchunkCanopenHardwareInterface::SchunkCanopenHardwareInterface (ros::NodeHandle& nh,
00039                                                                 boost::shared_ptr<CanOpenController>& canopen)
00040   : m_node_handle (nh),
00041     m_canopen_controller (canopen)
00042 {
00043   init();
00044 }
00045 
00046 void SchunkCanopenHardwareInterface::init()
00047 {
00048   m_node_ids = m_canopen_controller->getNodeList();
00049   size_t num_nodes = m_node_ids.size();
00050   m_joint_position_commands.resize(num_nodes);
00051   m_joint_position_commands_last.resize(num_nodes);
00052   m_joint_positions.resize(num_nodes);
00053   m_joint_velocity.resize(num_nodes);
00054   m_joint_effort.resize(num_nodes);
00055   m_joint_names.clear();
00056   m_nodes_in_fault.resize(num_nodes, false);
00057 
00058   bool rosparam_limits_ok = true;
00059 //  bool urdf_limits_ok = true;
00060 //  bool urdf_soft_limits_ok = true;
00061 
00062 
00063 
00064 //  boost::shared_ptr<urdf::ModelInterface> urdf = urdf::parseURDF("robot_description");
00065 
00066   urdf::Model urdf_model;
00067   if(!m_node_handle.hasParam("robot_description"))
00068   {
00069       ROS_ERROR("robot description not found!!");
00070   }
00071   if(!urdf_model.initParam("robot_description"))
00072   {
00073     ROS_ERROR("robot description parsing error!!");
00074   }
00075 
00076 
00077 
00078   // Initialize controller
00079   for (std::size_t i = 0; i < num_nodes; ++i) {
00080     std::string joint_name = "";
00081     std::string mapping_key = "~node_mapping_" + boost::lexical_cast<std::string>(static_cast<int>(m_node_ids[i]));
00082     ros::param::get(mapping_key, joint_name);
00083     m_joint_names.push_back(joint_name);
00084     ROS_DEBUG_STREAM("Controller Hardware interface: Loading joint with id " << static_cast<int>(m_node_ids[i]) << " named " << joint_name);
00085     if (joint_name == "")
00086     {
00087       ROS_ERROR_STREAM ("Could not find joint name for canopen device " << static_cast<int>(m_node_ids[i]) <<
00088         ". You will not be able to use this device with the controller!");
00089     }
00090     else
00091     {
00092       // Create joint state interface
00093       m_joint_state_interface.registerHandle(
00094           hardware_interface::JointStateHandle(m_joint_names[i],
00095               &m_joint_positions[i], &m_joint_velocity[i], &m_joint_effort[i]));
00096 
00097       // Create position joint interface
00098       hardware_interface::JointHandle hwi_handle(
00099           m_joint_state_interface.getHandle(m_joint_names[i]),
00100           &m_joint_position_commands[i]);
00101       m_position_joint_interface.registerHandle(hwi_handle);
00102 
00103       // Create a joint_limit_interface:
00104 
00105       // Populate (soft) joint limits from the ros parameter server
00106       rosparam_limits_ok = getJointLimits(joint_name, m_node_handle, m_joint_limits);
00107       if(!rosparam_limits_ok) {
00108         ROS_ERROR_STREAM ("Could not set the joint limits for joint " << joint_name << "!");
00109       }
00110 
00111       // Populate (soft) joint limits from URDF
00112       // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits'
00113       // Limits not specified in URDF preserve their existing values
00114 /*      boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model.getJoint(joint_name);
00115       urdf_limits_ok = getJointLimits(urdf_joint, m_joint_limits);
00116       if(!urdf_limits_ok) {
00117         ROS_ERROR_STREAM ("Could not set the joint limits for joint " << joint_name << "!");
00118       }
00119       urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, m_joint_soft_limits);
00120       if(!urdf_soft_limits_ok) {
00121         ROS_ERROR_STREAM ("Could not set the joint soft limits for joint " << joint_name << "!");
00122       }*/
00123 
00124       // Register handle in joint limits interface
00125       PositionJointSoftLimitsHandle limits_handle(hwi_handle, // We read the state and read/write the command
00126                                                   m_joint_limits,       // Limits spec
00127                                                   m_joint_soft_limits);  // Soft limits spec
00128 
00129       m_jnt_limits_interface.registerHandle(limits_handle);
00130     }
00131   }
00132 
00133 
00134   registerInterface(&m_joint_state_interface); // From RobotHW base class.
00135   registerInterface(&m_position_joint_interface); // From RobotHW base class.
00136 }
00137 
00138 void SchunkCanopenHardwareInterface::read()
00139 {
00140   m_is_fault = false;
00141   m_joint_positions.resize(m_node_ids.size());
00142   SchunkPowerBallNode::Ptr node;
00143   for (size_t i = 0; i < m_node_ids.size(); ++i)
00144   {
00145     node = m_canopen_controller->getNode<SchunkPowerBallNode>(m_node_ids[i]);
00146     m_joint_positions[i] = node->getTargetFeedback();
00147     ds402::Statusword node_status = node->getStatus();
00148     m_is_fault |= node_status.bit.fault;
00149     if (node_status.bit.fault)
00150     {
00151       if (!m_nodes_in_fault[i])
00152       {
00153         ROS_ERROR_STREAM ("Node " << static_cast<int>(m_node_ids[i]) << " is in FAULT state");
00154       }
00155       m_nodes_in_fault[i] = true;
00156     }
00157     else
00158     {
00159       m_nodes_in_fault[i] = false;
00160     }
00161   }
00162 }
00163 
00164 void SchunkCanopenHardwareInterface::write(ros::Time time, ros::Duration period)
00165 {
00166   if (m_node_ids.size() == m_joint_position_commands.size())
00167   {
00168     // For this position based interface, only the joint limits in degree and velocity are enforced. Acceleration is ignored.
00169     m_jnt_limits_interface.enforceLimits(period);
00170 
00171     std::stringstream commanded_positions;
00172     SchunkPowerBallNode::Ptr node;
00173     for (size_t i = 0; i < m_node_ids.size(); ++i)
00174     {
00175       const uint8_t& nr = m_node_ids[i];
00176       float pos = m_joint_position_commands[i];
00177       try
00178       {
00179         node = m_canopen_controller->getNode<SchunkPowerBallNode>(nr);
00180       }
00181       catch (const NotFoundException& e)
00182       {
00183         ROS_ERROR_STREAM ("One or more nodes could not be found in the controller. " << e.what());
00184         return;
00185       }
00186       m_canopen_controller->getNode<SchunkPowerBallNode>(nr)->setTarget(pos);
00187       commanded_positions << pos << " ";
00188     }
00189     if (m_joint_position_commands != m_joint_position_commands_last)
00190     {
00191       ROS_DEBUG_STREAM ("Commanded positions: " << commanded_positions.str());
00192       m_joint_position_commands_last = m_joint_position_commands;
00193     }
00194   }
00195   else
00196   {
00197     ROS_ERROR ("Number of known joints and number of commanded joints do not match!");
00198   }
00199 }
00200 
00201 bool SchunkCanopenHardwareInterface::prepareSwitch(const std::list< hardware_interface::ControllerInfo >& start_list, const std::list< hardware_interface::ControllerInfo >& stop_list)
00202 {
00203     return hardware_interface::RobotHW::prepareSwitch(start_list, stop_list);
00204 }
00205 
00206 void SchunkCanopenHardwareInterface::doSwitch(const std::list< hardware_interface::ControllerInfo >& start_list, const std::list< hardware_interface::ControllerInfo >& stop_list)
00207 {
00208     hardware_interface::RobotHW::doSwitch(start_list, stop_list);
00209 }
00210 
00211 sensor_msgs::JointState SchunkCanopenHardwareInterface::getJointMessage()
00212 {
00213   sensor_msgs::JointState joint_msg;
00214   joint_msg.name = m_joint_names;
00215   joint_msg.header.stamp = ros::Time::now();
00216   joint_msg.position = m_joint_positions;
00217   joint_msg.velocity = m_joint_velocity;
00218   joint_msg.effort = m_joint_effort;
00219 
00220   return joint_msg;
00221 }
00222 
00223 


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Thu Jun 6 2019 20:17:24